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