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