| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| 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 |
✓✗ | 7 |
cl.def(bp::init<std::string, robots::RobotWrapper&, bool>( |
51 |
(bp::args("name", "robot", "verbose")))) |
||
52 |
✓✗✓✗ |
7 |
.def("data", &InvDynPythonVisitor::data) |
53 |
✓✗ | 7 |
.add_property("nVar", &T::nVar) |
54 |
14 |
.add_property("nEq", &T::nEq) |
|
55 |
✓✗ | 7 |
.add_property("nIn", &T::nIn) |
56 |
|||
57 |
✓✗ | 7 |
.def("addMotionTask", &InvDynPythonVisitor::addMotionTask_SE3, |
58 |
bp::args("task", "weight", "priorityLevel", "transition duration")) |
||
59 |
✓✗✓✗ ✓✗ |
14 |
.def("addMotionTask", &InvDynPythonVisitor::addMotionTask_COM, |
60 |
bp::args("task", "weight", "priorityLevel", "transition duration")) |
||
61 |
✓✗✓✗ |
14 |
.def("addMotionTask", &InvDynPythonVisitor::addMotionTask_Joint, |
62 |
bp::args("task", "weight", "priorityLevel", "transition duration")) |
||
63 |
✓✗✓✗ |
14 |
.def("addMotionTask", &InvDynPythonVisitor::addMotionTask_JointBounds, |
64 |
bp::args("task", "weight", "priorityLevel", "transition duration")) |
||
65 |
✓✗✓✗ |
14 |
.def("addMotionTask", |
66 |
&InvDynPythonVisitor::addMotionTask_JointPosVelAccBounds, |
||
67 |
bp::args("task", "weight", "priorityLevel", "transition duration")) |
||
68 |
✓✗✓✗ |
14 |
.def("addMotionTask", &InvDynPythonVisitor::addMotionTask_AM, |
69 |
bp::args("task", "weight", "priorityLevel", "transition duration")) |
||
70 |
✓✗✓✗ |
14 |
.def("addMotionTask", |
71 |
&InvDynPythonVisitor::addMotionTask_TwoFramesEquality, |
||
72 |
bp::args("task", "weight", "priorityLevel", "transition duration")) |
||
73 |
✓✗✓✗ |
14 |
.def("addForceTask", &InvDynPythonVisitor::addForceTask_COP, |
74 |
bp::args("task", "weight", "priorityLevel", "transition duration")) |
||
75 |
✓✗✓✗ |
14 |
.def("addActuationTask", &InvDynPythonVisitor::addActuationTask_Bounds, |
76 |
bp::args("task", "weight", "priorityLevel", "transition duration")) |
||
77 |
✓✗✓✗ |
14 |
.def("updateTaskWeight", &InvDynPythonVisitor::updateTaskWeight, |
78 |
bp::args("task_name", "weight")) |
||
79 |
✓✗✓✗ |
14 |
.def("updateRigidContactWeights", |
80 |
&InvDynPythonVisitor::updateRigidContactWeights, |
||
81 |
bp::args("contact_name", "force_regularization_weight")) |
||
82 |
✓✗✓✗ |
14 |
.def("updateRigidContactWeights", |
83 |
&InvDynPythonVisitor::updateRigidContactWeightsWithMotionWeight, |
||
84 |
bp::args("contact_name", "force_regularization_weight", |
||
85 |
"motion_weight")) |
||
86 |
✓✗✓✗ |
14 |
.def("addRigidContact", |
87 |
&InvDynPythonVisitor::addRigidContact6dDeprecated, |
||
88 |
bp::args("contact"), |
||
89 |
pinocchio::python::deprecated_function<>( |
||
90 |
"Method addRigidContact(ContactBase) is deprecated. You " |
||
91 |
"should use addRigidContact(ContactBase, double) instead")) |
||
92 |
✓✗✓✗ ✓✗✓✗ |
14 |
.def("addRigidContact", &InvDynPythonVisitor::addRigidContact6d, |
93 |
bp::args("contact", "force_reg_weight")) |
||
94 |
✓✗✓✗ |
14 |
.def("addRigidContact", |
95 |
&InvDynPythonVisitor::addRigidContact6dWithPriorityLevel, |
||
96 |
bp::args("contact", "force_reg_weight", "motion_weight", |
||
97 |
"priority_level")) |
||
98 |
✓✗✓✗ |
14 |
.def("addRigidContact", &InvDynPythonVisitor::addRigidContactPoint, |
99 |
bp::args("contact", "force_reg_weight")) |
||
100 |
✓✗✓✗ |
14 |
.def("addRigidContact", |
101 |
&InvDynPythonVisitor::addRigidContactPointWithPriorityLevel, |
||
102 |
bp::args("contact", "force_reg_weight", "motion_weight", |
||
103 |
"priority_level")) |
||
104 |
✓✗✓✗ |
14 |
.def("addRigidContact", |
105 |
&InvDynPythonVisitor::addRigidContactTwoFramePositions, |
||
106 |
bp::args("contact", "force_reg_weight")) |
||
107 |
✓✗✓✗ |
14 |
.def("addRigidContact", |
108 |
&InvDynPythonVisitor:: |
||
109 |
addRigidContactTwoFramePositionsWithPriorityLevel, |
||
110 |
bp::args("contact", "force_reg_weight", "motion_weight", |
||
111 |
"priority_level")) |
||
112 |
✓✗✓✗ |
14 |
.def("removeTask", &InvDynPythonVisitor::removeTask, |
113 |
bp::args("task_name", "duration")) |
||
114 |
✓✗✓✗ |
14 |
.def("removeRigidContact", &InvDynPythonVisitor::removeRigidContact, |
115 |
bp::args("contact_name", "duration")) |
||
116 |
✓✗✓✗ |
14 |
.def("removeFromHqpData", &InvDynPythonVisitor::removeFromHqpData, |
117 |
bp::args("constraint_name")) |
||
118 |
✓✗✓✗ |
14 |
.def("computeProblemData", &InvDynPythonVisitor::computeProblemData, |
119 |
bp::args("time", "q", "v")) |
||
120 |
|||
121 |
✓✗✓✗ |
14 |
.def("getActuatorForces", &InvDynPythonVisitor::getActuatorForces, |
122 |
bp::args("HQPOutput")) |
||
123 |
✓✗✓✗ |
14 |
.def("getAccelerations", &InvDynPythonVisitor::getAccelerations, |
124 |
bp::args("HQPOutput")) |
||
125 |
✓✗✓✗ |
14 |
.def("getContactForces", &InvDynPythonVisitor::getContactForces, |
126 |
bp::args("HQPOutput")) |
||
127 |
✓✗✓✗ |
14 |
.def("checkContact", &InvDynPythonVisitor::checkContact, |
128 |
bp::args("name", "HQPOutput")) |
||
129 |
✓✗✓✗ ✓✗ |
14 |
.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 |
✓✗✓✗ ✓✗✓✗ |
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 |
✓✗ | 36 |
tsid::math::Vector f = self.getContactForces(name, sol); |
269 |
✓✗✓✓ |
18 |
if (f.size() > 0) return true; |
270 |
8 |
return false; |
|
271 |
} |
||
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 |
✓✗ | 7 |
std::string doc = "InvDyn info."; |
279 |
✓✗✓✗ |
7 |
bp::class_<T>(class_name.c_str(), doc.c_str(), bp::no_init) |
280 |
.def(InvDynPythonVisitor<T>()); |
||
281 |
7 |
} |
|
282 |
}; |
||
283 |
} // namespace python |
||
284 |
} // namespace tsid |
||
285 |
|||
286 |
#endif // ifndef __tsid_python_HQPOutput_hpp__ |
| Generated by: GCOVR (Version 4.2) |