Directory: | ./ |
---|---|
File: | include/tsid/bindings/python/formulations/formulation.hpp |
Date: | 2024-08-26 20:29:39 |
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 |