Loading...
Searching...
No Matches
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
30namespace 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 }
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__
Definition: constraint-equality.hpp:29
Definition: constraint-inequality.hpp:29
Definition: task-se3-equality.hpp:34
Definition: constraint-bound.hpp:27
Definition: contact-6d.hpp:39
static void setRegularizationTaskWeightVector(Contact6d &self, const ::Eigen::VectorXd w)
Definition: contact-6d.hpp:138
static const Eigen::MatrixXd & getForceGeneratorMatrix(Contact6d &self)
Definition: contact-6d.hpp:102
static void setKd(Contact6d &self, const ::Eigen::VectorXd Kd)
Definition: contact-6d.hpp:114
static void expose(const std::string &class_name)
Definition: contact-6d.hpp:145
static bool setContactNormal(Contact6d &self, const ::Eigen::VectorXd contactNormal)
Definition: contact-6d.hpp:120
static bool setMaxNormalForce(Contact6d &self, const double maxNormalForce)
Definition: contact-6d.hpp:129
static void setKp(Contact6d &self, const ::Eigen::VectorXd Kp)
Definition: contact-6d.hpp:111
static std::string name(Contact6d &self)
Definition: contact-6d.hpp:77
static tsid::tasks::TaskSE3Equality getMotionTask(Contact6d &self)
Definition: contact-6d.hpp:97
static void setForceReference(Contact6d &self, const ::Eigen::VectorXd f_ref)
Definition: contact-6d.hpp:135
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
static const Eigen::VectorXd & Kd(Contact6d &self)
Definition: contact-6d.hpp:108
static bool setMinNormalForce(Contact6d &self, const double minNormalForce)
Definition: contact-6d.hpp:126
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
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
static void setReference(Contact6d &self, const pinocchio::SE3 &ref)
Definition: contact-6d.hpp:132
static double getNormalForce(Contact6d &self, Eigen::VectorXd f)
Definition: contact-6d.hpp:141
void visit(PyClass &cl) const
Definition: contact-6d.hpp:43
static bool setContactPoints(Contact6d &self, const ::Eigen::MatrixXd contactpoints)
Definition: contact-6d.hpp:117
static const Eigen::VectorXd & Kp(Contact6d &self)
Definition: contact-6d.hpp:105
static bool setFrictionCoefficient(Contact6d &self, const double frictionCoefficient)
Definition: contact-6d.hpp:123