Directory: | ./ |
---|---|
File: | include/tsid/bindings/python/contacts/contact-point.hpp |
Date: | 2024-08-26 20:29:39 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 38 | 86 | 44.2% |
Branches: | 56 | 178 | 31.5% |
Line | Branch | Exec | Source |
---|---|---|---|
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 | |||
21 | #include "tsid/bindings/python/fwd.hpp" | ||
22 | |||
23 | #include "tsid/contacts/contact-point.hpp" | ||
24 | #include "tsid/robots/robot-wrapper.hpp" | ||
25 | #include "tsid/math/constraint-inequality.hpp" | ||
26 | #include "tsid/math/constraint-equality.hpp" | ||
27 | #include "tsid/math/constraint-base.hpp" | ||
28 | |||
29 | namespace tsid { | ||
30 | namespace python { | ||
31 | namespace bp = boost::python; | ||
32 | |||
33 | template <typename ContactPoint> | ||
34 | struct ContactPointPythonVisitor | ||
35 | : public boost::python::def_visitor< | ||
36 | ContactPointPythonVisitor<ContactPoint> > { | ||
37 | template <class PyClass> | ||
38 | |||
39 | 7 | void visit(PyClass& cl) const { | |
40 |
8/16✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7 times.
✗ Branch 23 not taken.
|
42 | cl.def(bp::init<std::string, robots::RobotWrapper&, std::string, |
41 | Eigen::VectorXd, double, double, double>( | ||
42 |
2/4✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
|
21 | (bp::arg("name"), bp::arg("robot"), bp::arg("framename"), |
43 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
21 | bp::arg("contactNormal"), bp::arg("frictionCoeff"), |
44 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
21 | bp::arg("minForce"), bp::arg("maxForce")), |
45 | "Default Constructor")) | ||
46 | 14 | .add_property("n_motion", &ContactPoint::n_motion, | |
47 | "return number of motion") | ||
48 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .add_property("n_force", &ContactPoint::n_force, |
49 | "return number of force") | ||
50 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
7 | .add_property("name", &ContactPointPythonVisitor::name, "return name") |
51 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("computeMotionTask", &ContactPointPythonVisitor::computeMotionTask, |
52 | bp::args("t", "q", "v", "data")) | ||
53 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("computeForceTask", &ContactPointPythonVisitor::computeForceTask, |
54 | bp::args("t", "q", "v", "data")) | ||
55 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("computeForceRegularizationTask", |
56 | &ContactPointPythonVisitor::computeForceRegularizationTask, | ||
57 | bp::args("t", "q", "v", "data")) | ||
58 | |||
59 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
7 | .add_property("getForceGeneratorMatrix", |
60 | bp::make_function( | ||
61 | &ContactPointPythonVisitor::getForceGeneratorMatrix, | ||
62 | 7 | bp::return_value_policy<bp::copy_const_reference>())) | |
63 | |||
64 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .def("getNormalForce", &ContactPointPythonVisitor::getNormalForce, |
65 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
14 | bp::arg("vec")) |
66 | 14 | .add_property("getMinNormalForce", &ContactPoint::getMinNormalForce) | |
67 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .add_property("getMaxNormalForce", &ContactPoint::getMaxNormalForce) |
68 | |||
69 |
3/6✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
|
7 | .add_property("Kp", |
70 | bp::make_function( | ||
71 | &ContactPointPythonVisitor::Kp, | ||
72 | 7 | bp::return_value_policy<bp::copy_const_reference>())) | |
73 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
7 | .add_property("Kd", |
74 | bp::make_function( | ||
75 | &ContactPointPythonVisitor::Kd, | ||
76 | 7 | bp::return_value_policy<bp::copy_const_reference>())) | |
77 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("setKp", &ContactPointPythonVisitor::setKp, bp::arg("Kp")) |
78 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("setKd", &ContactPointPythonVisitor::setKd, bp::arg("Kd")) |
79 | |||
80 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .def("useLocalFrame", &ContactPointPythonVisitor::useLocalFrame, |
81 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
14 | bp::arg("local_frame")) |
82 | |||
83 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("setContactNormal", &ContactPointPythonVisitor::setContactNormal, |
84 | bp::args("vec")) | ||
85 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("setFrictionCoefficient", |
86 | &ContactPointPythonVisitor::setFrictionCoefficient, | ||
87 | bp::args("friction_coeff")) | ||
88 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("setMinNormalForce", &ContactPointPythonVisitor::setMinNormalForce, |
89 | bp::args("min_force")) | ||
90 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("setMaxNormalForce", &ContactPointPythonVisitor::setMaxNormalForce, |
91 | bp::args("max_force")) | ||
92 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("setReference", &ContactPointPythonVisitor::setReference, |
93 | bp::args("SE3")) | ||
94 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("setForceReference", &ContactPointPythonVisitor::setForceReference, |
95 | bp::args("f_vec")) | ||
96 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
7 | .def("setRegularizationTaskWeightVector", |
97 | &ContactPointPythonVisitor::setRegularizationTaskWeightVector, | ||
98 | bp::args("w_vec")); | ||
99 | 7 | } | |
100 | ✗ | static std::string name(ContactPoint& self) { | |
101 | ✗ | std::string name = self.name(); | |
102 | ✗ | return name; | |
103 | } | ||
104 | |||
105 | ✗ | static math::ConstraintEquality computeMotionTask(ContactPoint& self, | |
106 | const double t, | ||
107 | const Eigen::VectorXd& q, | ||
108 | const Eigen::VectorXd& v, | ||
109 | pinocchio::Data& data) { | ||
110 | ✗ | self.computeMotionTask(t, q, v, data); | |
111 | ✗ | math::ConstraintEquality cons(self.getMotionConstraint().name(), | |
112 | ✗ | self.getMotionConstraint().matrix(), | |
113 | ✗ | self.getMotionConstraint().vector()); | |
114 | ✗ | return cons; | |
115 | } | ||
116 | ✗ | static math::ConstraintInequality computeForceTask( | |
117 | ContactPoint& self, const double t, const Eigen::VectorXd& q, | ||
118 | const Eigen::VectorXd& v, const pinocchio::Data& data) { | ||
119 | ✗ | self.computeForceTask(t, q, v, data); | |
120 | ✗ | math::ConstraintInequality cons(self.getForceConstraint().name(), | |
121 | ✗ | self.getForceConstraint().matrix(), | |
122 | ✗ | self.getForceConstraint().lowerBound(), | |
123 | ✗ | self.getForceConstraint().upperBound()); | |
124 | ✗ | return cons; | |
125 | } | ||
126 | ✗ | static math::ConstraintEquality computeForceRegularizationTask( | |
127 | ContactPoint& self, const double t, const Eigen::VectorXd& q, | ||
128 | const Eigen::VectorXd& v, const pinocchio::Data& data) { | ||
129 | ✗ | self.computeForceRegularizationTask(t, q, v, data); | |
130 | ✗ | math::ConstraintEquality cons(self.getForceRegularizationTask().name(), | |
131 | ✗ | self.getForceRegularizationTask().matrix(), | |
132 | ✗ | self.getForceRegularizationTask().vector()); | |
133 | ✗ | return cons; | |
134 | } | ||
135 | |||
136 | ✗ | static void useLocalFrame(ContactPoint& self, const bool local_frame) { | |
137 | ✗ | self.useLocalFrame(local_frame); | |
138 | } | ||
139 | ✗ | static const Eigen::MatrixXd& getForceGeneratorMatrix(ContactPoint& self) { | |
140 | ✗ | return self.getForceGeneratorMatrix(); | |
141 | } | ||
142 | ✗ | static const Eigen::VectorXd& Kp(ContactPoint& self) { return self.Kp(); } | |
143 | ✗ | static const Eigen::VectorXd& Kd(ContactPoint& self) { return self.Kd(); } | |
144 | ✗ | static void setKp(ContactPoint& self, const ::Eigen::VectorXd Kp) { | |
145 | ✗ | return self.Kp(Kp); | |
146 | } | ||
147 | ✗ | static void setKd(ContactPoint& self, const ::Eigen::VectorXd Kd) { | |
148 | ✗ | return self.Kd(Kd); | |
149 | } | ||
150 | static bool setContactPoints(ContactPoint& self, | ||
151 | const ::Eigen::MatrixXd contactpoints) { | ||
152 | return self.setContactPoints(contactpoints); | ||
153 | } | ||
154 | ✗ | static bool setContactNormal(ContactPoint& self, | |
155 | const ::Eigen::VectorXd contactNormal) { | ||
156 | ✗ | return self.setContactNormal(contactNormal); | |
157 | } | ||
158 | ✗ | static bool setFrictionCoefficient(ContactPoint& self, | |
159 | const double frictionCoefficient) { | ||
160 | ✗ | return self.setFrictionCoefficient(frictionCoefficient); | |
161 | } | ||
162 | ✗ | static bool setMinNormalForce(ContactPoint& self, | |
163 | const double minNormalForce) { | ||
164 | ✗ | return self.setMinNormalForce(minNormalForce); | |
165 | } | ||
166 | ✗ | static bool setMaxNormalForce(ContactPoint& self, | |
167 | const double maxNormalForce) { | ||
168 | ✗ | return self.setMaxNormalForce(maxNormalForce); | |
169 | } | ||
170 | ✗ | static void setReference(ContactPoint& self, const pinocchio::SE3& ref) { | |
171 | ✗ | self.setReference(ref); | |
172 | } | ||
173 | ✗ | static void setForceReference(ContactPoint& self, | |
174 | const ::Eigen::VectorXd f_ref) { | ||
175 | ✗ | self.setForceReference(f_ref); | |
176 | } | ||
177 | ✗ | static void setRegularizationTaskWeightVector(ContactPoint& self, | |
178 | const ::Eigen::VectorXd w) { | ||
179 | ✗ | self.setRegularizationTaskWeightVector(w); | |
180 | } | ||
181 | ✗ | static double getNormalForce(ContactPoint& self, Eigen::VectorXd f) { | |
182 | ✗ | return self.getNormalForce(f); | |
183 | } | ||
184 | |||
185 | 7 | static void expose(const std::string& class_name) { | |
186 |
1/2✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
|
7 | std::string doc = "ContactPoint info."; |
187 |
1/2✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
|
7 | bp::class_<ContactPoint>(class_name.c_str(), doc.c_str(), bp::no_init) |
188 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .def(ContactPointPythonVisitor<ContactPoint>()); |
189 | 7 | } | |
190 | }; | ||
191 | } // namespace python | ||
192 | } // namespace tsid | ||
193 | |||
194 | #endif // ifndef __tsid_python_contact_hpp__ | ||
195 |