GCC Code Coverage Report

Directory: ./
File: include/tsid/bindings/python/formulations/formulation.hpp
Date: 2025-03-29 14:29:37
Exec Total Coverage
Lines: 72 114 63.2%
Branches: 76 150 50.7%
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_HQPOutput_hpp__
19 #define __tsid_python_HQPOutput_hpp__
20
21 #include "tsid/bindings/python/fwd.hpp"
22
23 #include <pinocchio/bindings/python/utils/deprecation.hpp>
24
25 #include "tsid/formulations/inverse-dynamics-formulation-acc-force.hpp"
26 #include "tsid/bindings/python/solvers/HQPData.hpp"
27 #include "tsid/contacts/contact-6d.hpp"
28 #include "tsid/contacts/contact-point.hpp"
29 #include "tsid/contacts/contact-two-frame-positions.hpp"
30 #include "tsid/contacts/measured-6d-wrench.hpp"
31 #include "tsid/tasks/task-joint-posture.hpp"
32 #include "tsid/tasks/task-se3-equality.hpp"
33 #include "tsid/tasks/task-com-equality.hpp"
34 #include "tsid/tasks/task-cop-equality.hpp"
35 #include "tsid/tasks/task-actuation-bounds.hpp"
36 #include "tsid/tasks/task-joint-bounds.hpp"
37 #include "tsid/tasks/task-joint-posVelAcc-bounds.hpp"
38 #include "tsid/tasks/task-angular-momentum-equality.hpp"
39 #include "tsid/tasks/task-two-frames-equality.hpp"
40
41 namespace tsid {
42 namespace python {
43 namespace bp = boost::python;
44
45 template <typename T>
46 struct InvDynPythonVisitor
47 : public boost::python::def_visitor<InvDynPythonVisitor<T> > {
48 template <class PyClass>
49
50 8 void visit(PyClass& cl) const {
51
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 8 cl.def(bp::init<std::string, robots::RobotWrapper&, bool>(
52 (bp::args("name", "robot", "verbose"))))
53
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 .def("data", &InvDynPythonVisitor::data)
54 16 .add_property("nVar", &T::nVar)
55
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 .add_property("nEq", &T::nEq)
56
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 .add_property("nIn", &T::nIn)
57
58
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 16 .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_SE3,
59 bp::args("task", "weight", "priorityLevel", "transition duration"))
60
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_COM,
61 bp::args("task", "weight", "priorityLevel", "transition duration"))
62
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_Joint,
63 bp::args("task", "weight", "priorityLevel", "transition duration"))
64
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_JointBounds,
65 bp::args("task", "weight", "priorityLevel", "transition duration"))
66
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("addMotionTask",
67 &InvDynPythonVisitor::addMotionTask_JointPosVelAccBounds,
68 bp::args("task", "weight", "priorityLevel", "transition duration"))
69
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_AM,
70 bp::args("task", "weight", "priorityLevel", "transition duration"))
71
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("addMotionTask",
72 &InvDynPythonVisitor::addMotionTask_TwoFramesEquality,
73 bp::args("task", "weight", "priorityLevel", "transition duration"))
74
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("addForceTask", &InvDynPythonVisitor::addForceTask_COP,
75 bp::args("task", "weight", "priorityLevel", "transition duration"))
76
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("addActuationTask", &InvDynPythonVisitor::addActuationTask_Bounds,
77 bp::args("task", "weight", "priorityLevel", "transition duration"))
78
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("updateTaskWeight", &InvDynPythonVisitor::updateTaskWeight,
79 bp::args("task_name", "weight"))
80
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("updateRigidContactWeights",
81 &InvDynPythonVisitor::updateRigidContactWeights,
82 bp::args("contact_name", "force_regularization_weight"))
83
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("updateRigidContactWeights",
84 &InvDynPythonVisitor::updateRigidContactWeightsWithMotionWeight,
85 bp::args("contact_name", "force_regularization_weight",
86 "motion_weight"))
87
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 .def("addRigidContact",
88 &InvDynPythonVisitor::addRigidContact6dDeprecated,
89 bp::args("contact"),
90
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 16 pinocchio::python::deprecated_function<>(
91 "Method addRigidContact(ContactBase) is deprecated. You "
92 "should use addRigidContact(ContactBase, double) instead"))
93
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("addRigidContact", &InvDynPythonVisitor::addRigidContact6d,
94 bp::args("contact", "force_reg_weight"))
95
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("addRigidContact",
96 &InvDynPythonVisitor::addRigidContact6dWithPriorityLevel,
97 bp::args("contact", "force_reg_weight", "motion_weight",
98 "priority_level"))
99
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("addRigidContact", &InvDynPythonVisitor::addRigidContactPoint,
100 bp::args("contact", "force_reg_weight"))
101
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("addRigidContact",
102 &InvDynPythonVisitor::addRigidContactPointWithPriorityLevel,
103 bp::args("contact", "force_reg_weight", "motion_weight",
104 "priority_level"))
105
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("addRigidContact",
106 &InvDynPythonVisitor::addRigidContactTwoFramePositions,
107 bp::args("contact", "force_reg_weight"))
108
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("addRigidContact",
109 &InvDynPythonVisitor::
110 addRigidContactTwoFramePositionsWithPriorityLevel,
111 bp::args("contact", "force_reg_weight", "motion_weight",
112 "priority_level"))
113
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("removeTask", &InvDynPythonVisitor::removeTask,
114 bp::args("task_name", "duration"))
115
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("removeRigidContact", &InvDynPythonVisitor::removeRigidContact,
116 bp::args("contact_name", "duration"))
117
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("removeFromHqpData", &InvDynPythonVisitor::removeFromHqpData,
118 bp::args("constraint_name"))
119
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("computeProblemData", &InvDynPythonVisitor::computeProblemData,
120 bp::args("time", "q", "v"))
121
122
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("getActuatorForces", &InvDynPythonVisitor::getActuatorForces,
123 bp::args("HQPOutput"))
124
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("getAccelerations", &InvDynPythonVisitor::getAccelerations,
125 bp::args("HQPOutput"))
126
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("getContactForces", &InvDynPythonVisitor::getContactForces,
127 bp::args("HQPOutput"))
128
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("checkContact", &InvDynPythonVisitor::checkContact,
129 bp::args("name", "HQPOutput"))
130
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("getContactForce", &InvDynPythonVisitor::getContactForce,
131 bp::args("name", "HQPOutput"))
132
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 .def("addMeasuredForce", &InvDynPythonVisitor::addMeasuredForce,
133 bp::args("measured_force"));
134 8 }
135 2 static pinocchio::Data data(T& self) {
136 2 pinocchio::Data data = self.data();
137 2 return data;
138 }
139 1 static bool addMotionTask_SE3(T& self, tasks::TaskSE3Equality& task,
140 double weight, unsigned int priorityLevel,
141 double transition_duration) {
142 1 return self.addMotionTask(task, weight, priorityLevel, transition_duration);
143 }
144 1 static bool addMotionTask_COM(T& self, tasks::TaskComEquality& task,
145 double weight, unsigned int priorityLevel,
146 double transition_duration) {
147 1 return self.addMotionTask(task, weight, priorityLevel, transition_duration);
148 }
149 1 static bool addMotionTask_Joint(T& self, tasks::TaskJointPosture& task,
150 double weight, unsigned int priorityLevel,
151 double transition_duration) {
152 1 return self.addMotionTask(task, weight, priorityLevel, transition_duration);
153 }
154 ✗ static bool addMotionTask_JointBounds(T& self, tasks::TaskJointBounds& task,
155 double weight,
156 unsigned int priorityLevel,
157 double transition_duration) {
158 ✗ return self.addMotionTask(task, weight, priorityLevel, transition_duration);
159 }
160 ✗ static bool addMotionTask_JointPosVelAccBounds(
161 T& self, tasks::TaskJointPosVelAccBounds& task, double weight,
162 unsigned int priorityLevel, double transition_duration) {
163 ✗ return self.addMotionTask(task, weight, priorityLevel, transition_duration);
164 }
165 ✗ static bool addMotionTask_AM(T& self, tasks::TaskAMEquality& task,
166 double weight, unsigned int priorityLevel,
167 double transition_duration) {
168 ✗ return self.addMotionTask(task, weight, priorityLevel, transition_duration);
169 }
170 ✗ static bool addMotionTask_TwoFramesEquality(
171 T& self, tasks::TaskTwoFramesEquality& task, double weight,
172 unsigned int priorityLevel, double transition_duration) {
173 ✗ return self.addMotionTask(task, weight, priorityLevel, transition_duration);
174 }
175 ✗ static bool addForceTask_COP(T& self, tasks::TaskCopEquality& task,
176 double weight, unsigned int priorityLevel,
177 double transition_duration) {
178 ✗ return self.addForceTask(task, weight, priorityLevel, transition_duration);
179 }
180 ✗ static bool addActuationTask_Bounds(T& self, tasks::TaskActuationBounds& task,
181 double weight, unsigned int priorityLevel,
182 double transition_duration) {
183 ✗ return self.addActuationTask(task, weight, priorityLevel,
184 ✗ transition_duration);
185 }
186 ✗ static bool updateTaskWeight(T& self, const std::string& task_name,
187 double weight) {
188 ✗ return self.updateTaskWeight(task_name, weight);
189 }
190 ✗ static bool updateRigidContactWeights(T& self,
191 const std::string& contact_name,
192 double force_regularization_weight) {
193 ✗ return self.updateRigidContactWeights(contact_name,
194 ✗ force_regularization_weight);
195 }
196 ✗ static bool updateRigidContactWeightsWithMotionWeight(
197 T& self, const std::string& contact_name,
198 double force_regularization_weight, double motion_weight) {
199 ✗ return self.updateRigidContactWeights(
200 ✗ contact_name, force_regularization_weight, motion_weight);
201 }
202 2 static bool addRigidContact6dDeprecated(T& self,
203 contacts::Contact6d& contact) {
204 2 return self.addRigidContact(contact, 1e-5);
205 }
206 ✗ static bool addRigidContact6d(T& self, contacts::Contact6d& contact,
207 double force_regularization_weight) {
208 ✗ return self.addRigidContact(contact, force_regularization_weight);
209 }
210 ✗ static bool addRigidContact6dWithPriorityLevel(
211 T& self, contacts::Contact6d& contact, double force_regularization_weight,
212 double motion_weight, const bool priority_level) {
213 ✗ return self.addRigidContact(contact, force_regularization_weight,
214 ✗ motion_weight, priority_level);
215 }
216 ✗ static bool addRigidContactPoint(T& self, contacts::ContactPoint& contact,
217 double force_regularization_weight) {
218 ✗ return self.addRigidContact(contact, force_regularization_weight);
219 }
220 ✗ static bool addRigidContactPointWithPriorityLevel(
221 T& self, contacts::ContactPoint& contact,
222 double force_regularization_weight, double motion_weight,
223 const bool priority_level) {
224 ✗ return self.addRigidContact(contact, force_regularization_weight,
225 ✗ motion_weight, priority_level);
226 }
227 ✗ static bool addRigidContactTwoFramePositions(
228 T& self, contacts::ContactTwoFramePositions& contact,
229 double force_regularization_weight) {
230 ✗ return self.addRigidContact(contact, force_regularization_weight);
231 }
232 ✗ static bool addRigidContactTwoFramePositionsWithPriorityLevel(
233 T& self, contacts::ContactTwoFramePositions& contact,
234 double force_regularization_weight, double motion_weight,
235 const bool priority_level) {
236 ✗ return self.addRigidContact(contact, force_regularization_weight,
237 ✗ motion_weight, priority_level);
238 }
239 ✗ static bool removeTask(T& self, const std::string& task_name,
240 double transition_duration) {
241 ✗ return self.removeTask(task_name, transition_duration);
242 }
243 1 static bool removeRigidContact(T& self, const std::string& contactName,
244 double transition_duration) {
245 1 return self.removeRigidContact(contactName, transition_duration);
246 }
247 ✗ static bool removeFromHqpData(T& self, const std::string& constraintName) {
248 ✗ return self.removeFromHqpData(constraintName);
249 }
250 1001 static HQPDatas computeProblemData(T& self, double time,
251 const Eigen::VectorXd& q,
252 const Eigen::VectorXd& v) {
253 1001 HQPDatas Hqp;
254
4/8
✓ Branch 1 taken 1001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1001 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1001 times.
✗ Branch 11 not taken.
 1001 Hqp.set(self.computeProblemData(time, q, v));
255 1001 return Hqp;
256 }
257 1000 static Eigen::VectorXd getActuatorForces(T& self,
258 const solvers::HQPOutput& sol) {
259 1000 return self.getActuatorForces(sol);
260 }
261 1000 static Eigen::VectorXd getAccelerations(T& self,
262 const solvers::HQPOutput& sol) {
263 1000 return self.getAccelerations(sol);
264 }
265 ✗ static Eigen::VectorXd getContactForces(T& self,
266 const solvers::HQPOutput& sol) {
267 ✗ return self.getContactForces(sol);
268 }
269 18 static bool checkContact(T& self, const std::string& name,
270 const solvers::HQPOutput& sol) {
271
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 tsid::math::Vector f = self.getContactForces(name, sol);
272
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 8 times.
 18 if (f.size() > 0) return true;
273 8 return false;
274 18 }
275 10 static Eigen::VectorXd getContactForce(T& self, const std::string& name,
276 const solvers::HQPOutput& sol) {
277 10 return self.getContactForces(name, sol);
278 }
279 1 static bool addMeasuredForce(T& self,
280 contacts::Measured6Dwrench& measured_force) {
281 1 return self.addMeasuredForce(measured_force);
282 }
283
284 8 static void expose(const std::string& class_name) {
285
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 std::string doc = "InvDyn info.";
286
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 8 bp::class_<T>(class_name.c_str(), doc.c_str(), bp::no_init)
287
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 .def(InvDynPythonVisitor<T>());
288 8 }
289 };
290 } // namespace python
291 } // namespace tsid
292
293 #endif // ifndef __tsid_python_HQPOutput_hpp__
294