GCC Code Coverage Report

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