contact-6d.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018 CNRS, NYU, MPI Tübingen
3 //
4 // This file is part of tsid
5 // tsid is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 // tsid is distributed in the hope that it will be
10 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 // General Lesser Public License for more details. You should have
13 // received a copy of the GNU Lesser General Public License along with
14 // tsid If not, see
15 // <http://www.gnu.org/licenses/>.
16 //
17 
18 #ifndef __tsid_python_contact_6d_hpp__
19 #define __tsid_python_contact_6d_hpp__
20 
22 
29 
30 namespace tsid
31 {
32  namespace python
33  {
34  namespace bp = boost::python;
35 
36  template<typename Contact6d>
38  : public boost::python::def_visitor< Contact6DPythonVisitor<Contact6d> >
39  {
40 
41  template<class PyClass>
42 
43  void visit(PyClass& cl) const
44  {
45  cl
46  .def(bp::init<std::string, robots::RobotWrapper &, std::string, Eigen::MatrixXd, Eigen::VectorXd, double, double, double> ((bp::arg("name"), bp::arg("robot"), bp::arg("framename"), bp::arg("contactPoint"), bp::arg("contactNormal"), bp::arg("frictionCoeff"), bp::arg("minForce"), bp::arg("maxForce")), "Default Constructor"))
47  .def(bp::init<std::string, robots::RobotWrapper &, std::string, Eigen::MatrixXd, Eigen::VectorXd, double, double, double, double> ((bp::arg("name"), bp::arg("robot"), bp::arg("framename"), bp::arg("contactPoint"), bp::arg("contactNormal"), bp::arg("frictionCoeff"), bp::arg("minForce"), bp::arg("maxForce"), bp::arg("wForceReg")), "Deprecated Constructor"))
48  .add_property("n_motion", &Contact6d::n_motion, "return number of motion")
49  .add_property("n_force", &Contact6d::n_force, "return number of force")
50  .add_property("name", &Contact6DPythonVisitor::name, "return name")
51  .def("computeMotionTask", &Contact6DPythonVisitor::computeMotionTask, bp::args("t", "q", "v", "data"))
52  .def("computeForceTask", &Contact6DPythonVisitor::computeForceTask, bp::args("t", "q", "v", "data"))
53  .def("computeForceRegularizationTask", &Contact6DPythonVisitor::computeForceRegularizationTask, bp::args("t", "q", "v", "data"))
54  .def("getMotionTask", &Contact6DPythonVisitor::getMotionTask)
55 
56  .add_property("getForceGeneratorMatrix", bp::make_function(&Contact6DPythonVisitor::getForceGeneratorMatrix, bp::return_value_policy<bp::copy_const_reference>()))
57 
58  .def("getNormalForce", &Contact6DPythonVisitor::getNormalForce, bp::arg("vec"))
59  .add_property("getMinNormalForce", &Contact6d::getMinNormalForce)
60  .add_property("getMaxNormalForce", &Contact6d::getMaxNormalForce)
61 
62  .add_property("Kp", bp::make_function(&Contact6DPythonVisitor::Kp, bp::return_value_policy<bp::copy_const_reference>()))
63  .add_property("Kd", bp::make_function(&Contact6DPythonVisitor::Kd, bp::return_value_policy<bp::copy_const_reference>()))
64  .def("setKp", &Contact6DPythonVisitor::setKp, bp::arg("Kp"))
65  .def("setKd", &Contact6DPythonVisitor::setKd, bp::arg("Kd"))
66 
67  .def("setContactPoints", &Contact6DPythonVisitor::setContactPoints, bp::args("vec"))
68  .def("setContactNormal", &Contact6DPythonVisitor::setContactNormal, bp::args("vec"))
69  .def("setFrictionCoefficient", &Contact6DPythonVisitor::setFrictionCoefficient, bp::args("friction_coeff"))
70  .def("setMinNormalForce", &Contact6DPythonVisitor::setMinNormalForce, bp::args("min_force"))
71  .def("setMaxNormalForce", &Contact6DPythonVisitor::setMaxNormalForce, bp::args("max_force"))
72  .def("setReference", &Contact6DPythonVisitor::setReference, bp::args("SE3"))
73  .def("setForceReference", &Contact6DPythonVisitor::setForceReference, bp::args("f_vec"))
74  .def("setRegularizationTaskWeightVector", &Contact6DPythonVisitor::setRegularizationTaskWeightVector, bp::args("w_vec"))
75  ;
76  }
77  static std::string name(Contact6d & self){
78  std::string name = self.name();
79  return name;
80  }
81 
82  static math::ConstraintEquality computeMotionTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, pinocchio::Data & data){
83  self.computeMotionTask(t, q, v, data);
84  math::ConstraintEquality cons(self.getMotionConstraint().name(), self.getMotionConstraint().matrix(), self.getMotionConstraint().vector());
85  return cons;
86  }
87  static math::ConstraintInequality computeForceTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
88  self.computeForceTask(t, q, v, data);
89  math::ConstraintInequality cons(self.getForceConstraint().name(), self.getForceConstraint().matrix(), self.getForceConstraint().lowerBound(), self.getForceConstraint().upperBound());
90  return cons;
91  }
92  static math::ConstraintEquality computeForceRegularizationTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
93  self.computeForceRegularizationTask(t, q, v, data);
94  math::ConstraintEquality cons(self.getForceRegularizationTask().name(), self.getForceRegularizationTask().matrix(), self.getForceRegularizationTask().vector());
95  return cons;
96  }
97  static tsid::tasks::TaskSE3Equality getMotionTask(Contact6d & self){
98  tsid::tasks::TaskSE3Equality t = self.getMotionTask();
99  return t;
100  }
101 
102  static const Eigen::MatrixXd & getForceGeneratorMatrix(Contact6d & self){
103  return self.getForceGeneratorMatrix();
104  }
105  static const Eigen::VectorXd & Kp (Contact6d & self){
106  return self.Kp();
107  }
108  static const Eigen::VectorXd & Kd (Contact6d & self){
109  return self.Kd();
110  }
111  static void setKp (Contact6d & self, const::Eigen::VectorXd Kp){
112  return self.Kp(Kp);
113  }
114  static void setKd (Contact6d & self, const::Eigen::VectorXd Kd){
115  return self.Kd(Kd);
116  }
117  static bool setContactPoints (Contact6d & self, const::Eigen::MatrixXd contactpoints){
118  return self.setContactPoints(contactpoints);
119  }
120  static bool setContactNormal (Contact6d & self, const::Eigen::VectorXd contactNormal){
121  return self.setContactNormal(contactNormal);
122  }
123  static bool setFrictionCoefficient (Contact6d & self, const double frictionCoefficient){
124  return self.setFrictionCoefficient(frictionCoefficient);
125  }
126  static bool setMinNormalForce (Contact6d & self, const double minNormalForce){
127  return self.setMinNormalForce(minNormalForce);
128  }
129  static bool setMaxNormalForce (Contact6d & self, const double maxNormalForce){
130  return self.setMaxNormalForce(maxNormalForce);
131  }
132  static void setReference(Contact6d & self, const pinocchio::SE3 & ref){
133  self.setReference(ref);
134  }
135  static void setForceReference(Contact6d & self, const::Eigen::VectorXd f_ref){
136  self.setForceReference(f_ref);
137  }
138  static void setRegularizationTaskWeightVector(Contact6d & self, const::Eigen::VectorXd w){
139  self.setRegularizationTaskWeightVector(w);
140  }
141  static double getNormalForce(Contact6d & self, Eigen::VectorXd f){
142  return self.getNormalForce(f);
143  }
144 
145  static void expose(const std::string & class_name)
146  {
147  std::string doc = "Contact6d info.";
148  bp::class_<Contact6d>(class_name.c_str(),
149  doc.c_str(),
150  bp::no_init)
152  }
153  };
154  }
155 }
156 
157 
158 #endif // ifndef __tsid_python_contact_hpp__
tsid::python::Contact6DPythonVisitor::setKd
static void setKd(Contact6d &self, const ::Eigen::VectorXd Kd)
Definition: contact-6d.hpp:114
tsid::python::Contact6DPythonVisitor::setContactNormal
static bool setContactNormal(Contact6d &self, const ::Eigen::VectorXd contactNormal)
Definition: contact-6d.hpp:120
tsid::python::Contact6DPythonVisitor::setMaxNormalForce
static bool setMaxNormalForce(Contact6d &self, const double maxNormalForce)
Definition: contact-6d.hpp:129
tsid::python::Contact6DPythonVisitor::name
static std::string name(Contact6d &self)
Definition: contact-6d.hpp:77
tsid::math::ConstraintEquality
Definition: constraint-equality.hpp:28
Data
pinocchio::Data Data
Definition: inverse-dynamics-formulation-acc-force.cpp:29
tsid::python::Contact6DPythonVisitor::expose
static void expose(const std::string &class_name)
Definition: contact-6d.hpp:145
tsid::python::Contact6DPythonVisitor::getNormalForce
static double getNormalForce(Contact6d &self, Eigen::VectorXd f)
Definition: contact-6d.hpp:141
tsid::python::Contact6DPythonVisitor::computeForceRegularizationTask
static math::ConstraintEquality computeForceRegularizationTask(Contact6d &self, const double t, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const pinocchio::Data &data)
Definition: contact-6d.hpp:92
tsid::python::Contact6DPythonVisitor::setRegularizationTaskWeightVector
static void setRegularizationTaskWeightVector(Contact6d &self, const ::Eigen::VectorXd w)
Definition: contact-6d.hpp:138
tsid::python::Contact6DPythonVisitor::setKp
static void setKp(Contact6d &self, const ::Eigen::VectorXd Kp)
Definition: contact-6d.hpp:111
constraint-equality.hpp
tsid::python::SE3
pinocchio::SE3 SE3
Definition: trajectory-base.hpp:34
tsid::python::Contact6DPythonVisitor::setReference
static void setReference(Contact6d &self, const pinocchio::SE3 &ref)
Definition: contact-6d.hpp:132
tsid::python::Contact6DPythonVisitor::setForceReference
static void setForceReference(Contact6d &self, const ::Eigen::VectorXd f_ref)
Definition: contact-6d.hpp:135
robot-wrapper.hpp
tsid::python::Contact6DPythonVisitor::visit
void visit(PyClass &cl) const
Definition: contact-6d.hpp:43
constraint-base.hpp
tsid::python::Contact6DPythonVisitor::computeForceTask
static math::ConstraintInequality computeForceTask(Contact6d &self, const double t, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const pinocchio::Data &data)
Definition: contact-6d.hpp:87
task-se3-equality.hpp
tsid::math::ConstraintInequality
Definition: constraint-inequality.hpp:28
tsid::python::Contact6DPythonVisitor::getMotionTask
static tsid::tasks::TaskSE3Equality getMotionTask(Contact6d &self)
Definition: contact-6d.hpp:97
tsid::python::Contact6DPythonVisitor::getForceGeneratorMatrix
static const Eigen::MatrixXd & getForceGeneratorMatrix(Contact6d &self)
Definition: contact-6d.hpp:102
constraint-inequality.hpp
tsid
Definition: constraint-bound.hpp:26
contact-6d.hpp
tsid::python::Contact6DPythonVisitor
Definition: contact-6d.hpp:37
fwd.hpp
tsid::tasks::TaskSE3Equality
Definition: task-se3-equality.hpp:33
tsid::python::Contact6DPythonVisitor::Kd
static const Eigen::VectorXd & Kd(Contact6d &self)
Definition: contact-6d.hpp:108
tsid::python::Contact6DPythonVisitor::computeMotionTask
static math::ConstraintEquality computeMotionTask(Contact6d &self, const double t, const Eigen::VectorXd &q, const Eigen::VectorXd &v, pinocchio::Data &data)
Definition: contact-6d.hpp:82
tsid::python::Contact6DPythonVisitor::setMinNormalForce
static bool setMinNormalForce(Contact6d &self, const double minNormalForce)
Definition: contact-6d.hpp:126
tsid::python::Contact6DPythonVisitor::Kp
static const Eigen::VectorXd & Kp(Contact6d &self)
Definition: contact-6d.hpp:105
tsid::python::Contact6DPythonVisitor::setFrictionCoefficient
static bool setFrictionCoefficient(Contact6d &self, const double frictionCoefficient)
Definition: contact-6d.hpp:123
tsid::python::Contact6DPythonVisitor::setContactPoints
static bool setContactPoints(Contact6d &self, const ::Eigen::MatrixXd contactpoints)
Definition: contact-6d.hpp:117