contact-point.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018 CNRS
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 
28 
29 namespace tsid
30 {
31  namespace python
32  {
33  namespace bp = boost::python;
34 
35  template<typename ContactPoint>
37  : public boost::python::def_visitor< ContactPointPythonVisitor<ContactPoint> >
38  {
39 
40  template<class PyClass>
41 
42  void visit(PyClass& cl) const
43  {
44  cl
45  .def(bp::init<std::string, robots::RobotWrapper &, std::string, Eigen::VectorXd, double, double, double> ((bp::arg("name"), bp::arg("robot"), bp::arg("framename"), bp::arg("contactNormal"), bp::arg("frictionCoeff"), bp::arg("minForce"), bp::arg("maxForce")), "Default Constructor"))
46  .add_property("n_motion", &ContactPoint::n_motion, "return number of motion")
47  .add_property("n_force", &ContactPoint::n_force, "return number of force")
48  .add_property("name", &ContactPointPythonVisitor::name, "return name")
49  .def("computeMotionTask", &ContactPointPythonVisitor::computeMotionTask, bp::args("t", "q", "v", "data"))
50  .def("computeForceTask", &ContactPointPythonVisitor::computeForceTask, bp::args("t", "q", "v", "data"))
51  .def("computeForceRegularizationTask", &ContactPointPythonVisitor::computeForceRegularizationTask, bp::args("t", "q", "v", "data"))
52 
53  .add_property("getForceGeneratorMatrix", bp::make_function(&ContactPointPythonVisitor::getForceGeneratorMatrix, bp::return_value_policy<bp::copy_const_reference>()))
54 
55  .def("getNormalForce", &ContactPointPythonVisitor::getNormalForce, bp::arg("vec"))
56  .add_property("getMinNormalForce", &ContactPoint::getMinNormalForce)
57  .add_property("getMaxNormalForce", &ContactPoint::getMaxNormalForce)
58 
59  .add_property("Kp", bp::make_function(&ContactPointPythonVisitor::Kp, bp::return_value_policy<bp::copy_const_reference>()))
60  .add_property("Kd", bp::make_function(&ContactPointPythonVisitor::Kd, bp::return_value_policy<bp::copy_const_reference>()))
61  .def("setKp", &ContactPointPythonVisitor::setKp, bp::arg("Kp"))
62  .def("setKd", &ContactPointPythonVisitor::setKd, bp::arg("Kd"))
63 
64  .def("useLocalFrame", &ContactPointPythonVisitor::useLocalFrame, bp::arg("local_frame"))
65 
66  .def("setContactNormal", &ContactPointPythonVisitor::setContactNormal, bp::args("vec"))
67  .def("setFrictionCoefficient", &ContactPointPythonVisitor::setFrictionCoefficient, bp::args("friction_coeff"))
68  .def("setMinNormalForce", &ContactPointPythonVisitor::setMinNormalForce, bp::args("min_force"))
69  .def("setMaxNormalForce", &ContactPointPythonVisitor::setMaxNormalForce, bp::args("max_force"))
70  .def("setReference", &ContactPointPythonVisitor::setReference, bp::args("SE3"))
71  .def("setForceReference", &ContactPointPythonVisitor::setForceReference, bp::args("f_vec"))
72  .def("setRegularizationTaskWeightVector", &ContactPointPythonVisitor::setRegularizationTaskWeightVector, bp::args("w_vec"))
73  ;
74  }
75  static std::string name(ContactPoint & self){
76  std::string name = self.name();
77  return name;
78  }
79 
80  static math::ConstraintEquality computeMotionTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, pinocchio::Data & data){
81  self.computeMotionTask(t, q, v, data);
82  math::ConstraintEquality cons(self.getMotionConstraint().name(), self.getMotionConstraint().matrix(), self.getMotionConstraint().vector());
83  return cons;
84  }
85  static math::ConstraintInequality computeForceTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
86  self.computeForceTask(t, q, v, data);
87  math::ConstraintInequality cons(self.getForceConstraint().name(), self.getForceConstraint().matrix(), self.getForceConstraint().lowerBound(), self.getForceConstraint().upperBound());
88  return cons;
89  }
90  static math::ConstraintEquality computeForceRegularizationTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
91  self.computeForceRegularizationTask(t, q, v, data);
92  math::ConstraintEquality cons(self.getForceRegularizationTask().name(), self.getForceRegularizationTask().matrix(), self.getForceRegularizationTask().vector());
93  return cons;
94  }
95 
96  static void useLocalFrame (ContactPoint & self, const bool local_frame) {
97  self.useLocalFrame(local_frame);
98  }
99  static const Eigen::MatrixXd & getForceGeneratorMatrix(ContactPoint & self){
100  return self.getForceGeneratorMatrix();
101  }
102  static const Eigen::VectorXd & Kp (ContactPoint & self){
103  return self.Kp();
104  }
105  static const Eigen::VectorXd & Kd (ContactPoint & self){
106  return self.Kd();
107  }
108  static void setKp (ContactPoint & self, const::Eigen::VectorXd Kp){
109  return self.Kp(Kp);
110  }
111  static void setKd (ContactPoint & self, const::Eigen::VectorXd Kd){
112  return self.Kd(Kd);
113  }
114  static bool setContactPoints (ContactPoint & self, const::Eigen::MatrixXd contactpoints){
115  return self.setContactPoints(contactpoints);
116  }
117  static bool setContactNormal (ContactPoint & self, const::Eigen::VectorXd contactNormal){
118  return self.setContactNormal(contactNormal);
119  }
120  static bool setFrictionCoefficient (ContactPoint & self, const double frictionCoefficient){
121  return self.setFrictionCoefficient(frictionCoefficient);
122  }
123  static bool setMinNormalForce (ContactPoint & self, const double minNormalForce){
124  return self.setMinNormalForce(minNormalForce);
125  }
126  static bool setMaxNormalForce (ContactPoint & self, const double maxNormalForce){
127  return self.setMaxNormalForce(maxNormalForce);
128  }
129  static void setReference(ContactPoint & self, const pinocchio::SE3 & ref){
130  self.setReference(ref);
131  }
132  static void setForceReference(ContactPoint & self, const::Eigen::VectorXd f_ref){
133  self.setForceReference(f_ref);
134  }
135  static void setRegularizationTaskWeightVector(ContactPoint & self, const::Eigen::VectorXd w){
136  self.setRegularizationTaskWeightVector(w);
137  }
138  static double getNormalForce(ContactPoint & self, Eigen::VectorXd f){
139  return self.getNormalForce(f);
140  }
141 
142  static void expose(const std::string & class_name)
143  {
144  std::string doc = "ContactPoint info.";
145  bp::class_<ContactPoint>(class_name.c_str(),
146  doc.c_str(),
147  bp::no_init)
149  }
150  };
151  }
152 }
153 
154 
155 #endif // ifndef __tsid_python_contact_hpp__
tsid::python::ContactPointPythonVisitor::useLocalFrame
static void useLocalFrame(ContactPoint &self, const bool local_frame)
Definition: contact-point.hpp:96
tsid::math::ConstraintEquality
Definition: constraint-equality.hpp:28
tsid::python::ContactPointPythonVisitor::getNormalForce
static double getNormalForce(ContactPoint &self, Eigen::VectorXd f)
Definition: contact-point.hpp:138
Data
pinocchio::Data Data
Definition: inverse-dynamics-formulation-acc-force.cpp:29
contact-point.hpp
constraint-equality.hpp
tsid::python::ContactPointPythonVisitor::getForceGeneratorMatrix
static const Eigen::MatrixXd & getForceGeneratorMatrix(ContactPoint &self)
Definition: contact-point.hpp:99
tsid::python::ContactPointPythonVisitor::setMinNormalForce
static bool setMinNormalForce(ContactPoint &self, const double minNormalForce)
Definition: contact-point.hpp:123
tsid::python::SE3
pinocchio::SE3 SE3
Definition: trajectory-base.hpp:34
tsid::python::ContactPointPythonVisitor::setKd
static void setKd(ContactPoint &self, const ::Eigen::VectorXd Kd)
Definition: contact-point.hpp:111
tsid::python::ContactPointPythonVisitor::setContactPoints
static bool setContactPoints(ContactPoint &self, const ::Eigen::MatrixXd contactpoints)
Definition: contact-point.hpp:114
tsid::python::ContactPointPythonVisitor::expose
static void expose(const std::string &class_name)
Definition: contact-point.hpp:142
tsid::python::ContactPointPythonVisitor::visit
void visit(PyClass &cl) const
Definition: contact-point.hpp:42
tsid::python::ContactPointPythonVisitor::computeForceTask
static math::ConstraintInequality computeForceTask(ContactPoint &self, const double t, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const pinocchio::Data &data)
Definition: contact-point.hpp:85
robot-wrapper.hpp
tsid::python::ContactPointPythonVisitor::setKp
static void setKp(ContactPoint &self, const ::Eigen::VectorXd Kp)
Definition: contact-point.hpp:108
constraint-base.hpp
tsid::python::ContactPointPythonVisitor::Kd
static const Eigen::VectorXd & Kd(ContactPoint &self)
Definition: contact-point.hpp:105
tsid::math::ConstraintInequality
Definition: constraint-inequality.hpp:28
tsid::python::ContactPointPythonVisitor::name
static std::string name(ContactPoint &self)
Definition: contact-point.hpp:75
tsid::python::ContactPointPythonVisitor::computeForceRegularizationTask
static math::ConstraintEquality computeForceRegularizationTask(ContactPoint &self, const double t, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const pinocchio::Data &data)
Definition: contact-point.hpp:90
constraint-inequality.hpp
tsid::python::ContactPointPythonVisitor::computeMotionTask
static math::ConstraintEquality computeMotionTask(ContactPoint &self, const double t, const Eigen::VectorXd &q, const Eigen::VectorXd &v, pinocchio::Data &data)
Definition: contact-point.hpp:80
tsid::python::ContactPointPythonVisitor::setReference
static void setReference(ContactPoint &self, const pinocchio::SE3 &ref)
Definition: contact-point.hpp:129
tsid
Definition: constraint-bound.hpp:26
tsid::python::ContactPointPythonVisitor::setFrictionCoefficient
static bool setFrictionCoefficient(ContactPoint &self, const double frictionCoefficient)
Definition: contact-point.hpp:120
fwd.hpp
tsid::python::ContactPointPythonVisitor
Definition: contact-point.hpp:36
tsid::python::ContactPointPythonVisitor::setRegularizationTaskWeightVector
static void setRegularizationTaskWeightVector(ContactPoint &self, const ::Eigen::VectorXd w)
Definition: contact-point.hpp:135
tsid::python::ContactPointPythonVisitor::Kp
static const Eigen::VectorXd & Kp(ContactPoint &self)
Definition: contact-point.hpp:102
tsid::python::ContactPointPythonVisitor::setContactNormal
static bool setContactNormal(ContactPoint &self, const ::Eigen::VectorXd contactNormal)
Definition: contact-point.hpp:117
tsid::python::ContactPointPythonVisitor::setForceReference
static void setForceReference(ContactPoint &self, const ::Eigen::VectorXd f_ref)
Definition: contact-point.hpp:132
tsid::python::ContactPointPythonVisitor::setMaxNormalForce
static bool setMaxNormalForce(ContactPoint &self, const double maxNormalForce)
Definition: contact-point.hpp:126