1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2021-2023, University of Edinburgh, Heriot-Watt University |
5 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
6 |
|
|
// All rights reserved. |
7 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
8 |
|
|
|
9 |
|
|
#include "crocoddyl/multibody/actions/contact-invdyn.hpp" |
10 |
|
|
|
11 |
|
|
#include "python/crocoddyl/core/diff-action-base.hpp" |
12 |
|
|
#include "python/crocoddyl/multibody/multibody.hpp" |
13 |
|
|
#include "python/crocoddyl/utils/copyable.hpp" |
14 |
|
|
|
15 |
|
|
namespace crocoddyl { |
16 |
|
|
namespace python { |
17 |
|
|
|
18 |
|
10 |
void exposeDifferentialActionContactInvDynamics() { |
19 |
✓✗✓✗
✓✗ |
10 |
bp::scope().attr("yes") = 1; |
20 |
✓✗✓✗
✓✗ |
10 |
bp::scope().attr("no") = 0; |
21 |
|
|
{ |
22 |
|
|
bp::register_ptr_to_python< |
23 |
✓✗ |
10 |
boost::shared_ptr<DifferentialActionModelContactInvDynamics> >(); |
24 |
|
|
bp::scope model_outer = |
25 |
✓✗ |
10 |
bp::class_<DifferentialActionModelContactInvDynamics, |
26 |
|
|
bp::bases<DifferentialActionModelAbstract> >( |
27 |
|
|
"DifferentialActionModelContactInvDynamics", |
28 |
|
|
"Differential action model for inverse dynamics in multibody " |
29 |
|
|
"systems with contacts.\n\n" |
30 |
|
|
"This class implements forward kinematic with holonomic contact " |
31 |
|
|
"constraints (defined at the acceleration\n" |
32 |
|
|
"level) and inverse-dynamics computation using the Recursive " |
33 |
|
|
"Newton Euler Algorithm (RNEA)\n" |
34 |
|
|
"On the other hand, the stack of cost and constraint functions are " |
35 |
|
|
"implemented in\n" |
36 |
|
|
"ConstraintModelManager() and CostModelSum(), respectively.", |
37 |
✓✗ |
20 |
bp::init<boost::shared_ptr<StateMultibody>, |
38 |
|
|
boost::shared_ptr<ActuationModelAbstract>, |
39 |
|
|
boost::shared_ptr<ContactModelMultiple>, |
40 |
|
|
boost::shared_ptr<CostModelSum>, |
41 |
|
|
bp::optional<boost::shared_ptr<ConstraintModelManager> > >( |
42 |
✓✗ |
10 |
bp::args("self", "state", "actuation", "contacts", "costs", |
43 |
|
|
"constraints"), |
44 |
|
|
"Initialize the inverse-dynamics action model for system with " |
45 |
|
|
"contact.\n\n" |
46 |
|
|
"It describes the kinematic evolution of the multibody system " |
47 |
|
|
"with contacts,\n" |
48 |
|
|
"and computes the needed torques using inverse-dynamics.\n." |
49 |
|
|
":param state: multibody state\n" |
50 |
|
|
":param actuation: abstract actuation model\n" |
51 |
|
|
":param contacts: stack of contact model\n" |
52 |
|
|
":param costs: stack of cost functions\n" |
53 |
|
|
":param constraints: stack of constraint functions")) |
54 |
|
|
.def<void (DifferentialActionModelContactInvDynamics::*)( |
55 |
|
|
const boost::shared_ptr<DifferentialActionDataAbstract>&, |
56 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&, |
57 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
58 |
|
|
"calc", &DifferentialActionModelContactInvDynamics::calc, |
59 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
60 |
|
|
"Compute the next state, cost value and constraints.\n\n" |
61 |
|
|
":param data: contact inverse-dynamics data\n" |
62 |
|
|
":param x: state point (dim. state.nx)\n" |
63 |
|
20 |
":param u: control input (dim. nu)") |
64 |
|
|
.def<void (DifferentialActionModelContactInvDynamics::*)( |
65 |
|
|
const boost::shared_ptr<DifferentialActionDataAbstract>&, |
66 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
67 |
|
|
"calc", &DifferentialActionModelAbstract::calc, |
68 |
✓✗✓✗
|
20 |
bp::args("self", "data", "x")) |
69 |
|
|
.def<void (DifferentialActionModelContactInvDynamics::*)( |
70 |
|
|
const boost::shared_ptr<DifferentialActionDataAbstract>&, |
71 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&, |
72 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
73 |
|
|
"calcDiff", |
74 |
|
|
&DifferentialActionModelContactInvDynamics::calcDiff, |
75 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
76 |
|
|
"Compute the derivatives of the differential multibody system, " |
77 |
|
|
"and its cost and constraint " |
78 |
|
|
"functions.\n\n" |
79 |
|
|
"It computes the partial derivatives of the differential " |
80 |
|
|
"multibody system, the cost and constraint\n" |
81 |
|
|
"functions. It assumes that calc has been run first. This " |
82 |
|
|
"function builds a quadratic approximation\n" |
83 |
|
|
"of the action model (i.e., dynamical system, cost and " |
84 |
|
|
"constraint functions).\n" |
85 |
|
|
":param data: contact inverse-dynamics data\n" |
86 |
|
|
":param x: state point (dim. state.nx)\n" |
87 |
✓✗ |
10 |
":param u: control input (dim. nu)") |
88 |
|
|
.def<void (DifferentialActionModelContactInvDynamics::*)( |
89 |
|
|
const boost::shared_ptr<DifferentialActionDataAbstract>&, |
90 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
91 |
|
|
"calcDiff", &DifferentialActionModelAbstract::calcDiff, |
92 |
✓✗✓✗
|
20 |
bp::args("self", "data", "x")) |
93 |
|
|
.def( |
94 |
|
|
"createData", |
95 |
|
|
&DifferentialActionModelContactInvDynamics::createData, |
96 |
✓✗ |
20 |
bp::args("self"), |
97 |
✓✗✓✗
|
20 |
"Create the contact inverse-dynamics differential action data.") |
98 |
|
|
.add_property( |
99 |
|
|
"actuation", |
100 |
✓✗ |
10 |
bp::make_function( |
101 |
|
|
&DifferentialActionModelContactInvDynamics::get_actuation, |
102 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
103 |
✓✗ |
10 |
"actuation model") |
104 |
|
|
.add_property( |
105 |
|
|
"contacts", |
106 |
✓✗ |
10 |
bp::make_function( |
107 |
|
|
&DifferentialActionModelContactInvDynamics::get_contacts, |
108 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
109 |
✓✗ |
10 |
"multiple contact model") |
110 |
|
|
.add_property( |
111 |
|
|
"costs", |
112 |
✓✗ |
10 |
bp::make_function( |
113 |
|
|
&DifferentialActionModelContactInvDynamics::get_costs, |
114 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
115 |
✓✗ |
10 |
"total cost model") |
116 |
|
|
.add_property( |
117 |
|
|
"constraints", |
118 |
✓✗ |
10 |
bp::make_function( |
119 |
|
|
&DifferentialActionModelContactInvDynamics::get_constraints, |
120 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
121 |
✓✗ |
10 |
"constraint model manager") |
122 |
✓✗✓✗
|
10 |
.def(CopyableVisitor<DifferentialActionModelContactInvDynamics>()); |
123 |
|
|
|
124 |
|
|
bp::register_ptr_to_python<boost::shared_ptr< |
125 |
✓✗ |
10 |
DifferentialActionModelContactInvDynamics::ResidualModelActuation> >(); |
126 |
|
|
|
127 |
✓✗ |
10 |
bp::class_< |
128 |
|
|
DifferentialActionModelContactInvDynamics::ResidualModelActuation, |
129 |
|
|
bp::bases<ResidualModelAbstract> >( |
130 |
|
|
"ResidualModelActuation", |
131 |
|
|
"This residual function enforces the torques of under-actuated joints " |
132 |
|
|
"(e.g., floating-base\n" |
133 |
|
|
"joints) to be zero. We compute these torques and their derivatives " |
134 |
|
|
"using RNEA inside \n" |
135 |
|
|
"DifferentialActionModelContactInvDynamics.", |
136 |
✓✗ |
10 |
bp::init<boost::shared_ptr<StateMultibody>, std::size_t, std::size_t>( |
137 |
✓✗ |
20 |
bp::args("self", "state", "nu", "nc"), |
138 |
|
|
"Initialize the actuation residual model.\n\n" |
139 |
|
|
":param nu: dimension of control vector\n" |
140 |
|
|
":param nc: number of the contacts")) |
141 |
|
|
.def<void (DifferentialActionModelContactInvDynamics:: |
142 |
|
|
ResidualModelActuation::*)( |
143 |
|
|
const boost::shared_ptr<ResidualDataAbstract>&, |
144 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&, |
145 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
146 |
|
|
"calc", |
147 |
|
|
&DifferentialActionModelContactInvDynamics::ResidualModelActuation:: |
148 |
|
|
calc, |
149 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
150 |
|
|
"Compute the actuation residual.\n\n" |
151 |
|
|
":param data: residual data\n" |
152 |
|
|
":param x: state point (dim. state.nx)\n" |
153 |
|
20 |
":param u: control input (dim. nu)") |
154 |
|
|
.def<void (DifferentialActionModelContactInvDynamics:: |
155 |
|
|
ResidualModelActuation::*)( |
156 |
|
|
const boost::shared_ptr<ResidualDataAbstract>&, |
157 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
158 |
✓✗✓✗
|
20 |
"calc", &ResidualModelAbstract::calc, bp::args("self", "data", "x")) |
159 |
|
|
.def<void (DifferentialActionModelContactInvDynamics:: |
160 |
|
|
ResidualModelActuation::*)( |
161 |
|
|
const boost::shared_ptr<ResidualDataAbstract>&, |
162 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&, |
163 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
164 |
|
|
"calcDiff", |
165 |
|
|
&DifferentialActionModelContactInvDynamics::ResidualModelActuation:: |
166 |
|
|
calcDiff, |
167 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
168 |
|
|
"Compute the Jacobians of the actuation residual.\n\n" |
169 |
|
|
"It assumes that calc has been run first.\n" |
170 |
|
|
":param data: action data\n" |
171 |
|
|
":param x: state point (dim. state.nx)\n" |
172 |
✓✗ |
10 |
":param u: control input (dim. nu)\n") |
173 |
|
|
.def<void (DifferentialActionModelContactInvDynamics:: |
174 |
|
|
ResidualModelActuation::*)( |
175 |
|
|
const boost::shared_ptr<ResidualDataAbstract>&, |
176 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
177 |
|
|
"calcDiff", &ResidualModelAbstract::calcDiff, |
178 |
✓✗✓✗
|
20 |
bp::args("self", "data", "x")) |
179 |
|
|
.def("createData", |
180 |
|
|
&DifferentialActionModelContactInvDynamics:: |
181 |
|
|
ResidualModelActuation::createData, |
182 |
|
|
bp::with_custodian_and_ward_postcall<0, 2>(), |
183 |
✓✗ |
20 |
bp::args("self", "data"), |
184 |
|
|
"Create the actuation residual data.\n\n" |
185 |
|
|
"Each residual model has its own data that needs to be allocated. " |
186 |
|
|
"This function\n" |
187 |
|
|
"returns the allocated data for the actuation residual.\n" |
188 |
|
|
":param data: shared data\n" |
189 |
✓✗✓✗
|
20 |
":return residual data.") |
190 |
|
10 |
.def(CopyableVisitor<DifferentialActionModelContactInvDynamics:: |
191 |
✓✗ |
10 |
ResidualModelActuation>()); |
192 |
|
|
|
193 |
|
|
bp::register_ptr_to_python<boost::shared_ptr< |
194 |
✓✗ |
10 |
DifferentialActionModelContactInvDynamics::ResidualModelContact> >(); |
195 |
|
|
|
196 |
✓✗ |
10 |
bp::class_<DifferentialActionModelContactInvDynamics::ResidualModelContact, |
197 |
|
|
bp::bases<ResidualModelAbstract> >( |
198 |
|
|
"ResidualModelContact", |
199 |
|
|
"This residual function for the contact acceleration, i.e., r = a0, " |
200 |
|
|
"where a0 is the desired\n" |
201 |
|
|
"contact acceleration which also considers the Baumgarte " |
202 |
|
|
"stabilization.", |
203 |
✓✗ |
10 |
bp::init<boost::shared_ptr<StateMultibody>, pinocchio::FrameIndex, |
204 |
|
|
std::size_t, std::size_t>( |
205 |
✓✗ |
20 |
bp::args("self", "state", "id", "nr", "nc"), |
206 |
|
|
"Initialize the contact-acceleration residual model.\n\n" |
207 |
|
|
":param id: contact id\n" |
208 |
|
|
":param nr: dimension of contact residual\n" |
209 |
|
|
":param nc: dimension of contact vector")) |
210 |
|
|
.def<void ( |
211 |
|
|
DifferentialActionModelContactInvDynamics::ResidualModelContact::*)( |
212 |
|
|
const boost::shared_ptr<ResidualDataAbstract>&, |
213 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&, |
214 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
215 |
|
|
"calc", |
216 |
|
|
&DifferentialActionModelContactInvDynamics::ResidualModelContact:: |
217 |
|
|
calc, |
218 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
219 |
|
|
"Compute the contact-acceleration residual.\n\n" |
220 |
|
|
":param data: residual data\n" |
221 |
|
|
":param x: state vector\n" |
222 |
|
20 |
":param u: control input") |
223 |
|
|
.def<void ( |
224 |
|
|
DifferentialActionModelContactInvDynamics::ResidualModelContact::*)( |
225 |
|
|
const boost::shared_ptr<ResidualDataAbstract>&, |
226 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
227 |
✓✗✓✗
|
20 |
"calc", &ResidualModelAbstract::calc, bp::args("self", "data", "x")) |
228 |
|
|
.def<void ( |
229 |
|
|
DifferentialActionModelContactInvDynamics::ResidualModelContact::*)( |
230 |
|
|
const boost::shared_ptr<ResidualDataAbstract>&, |
231 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&, |
232 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
233 |
|
|
"calcDiff", |
234 |
|
|
&DifferentialActionModelContactInvDynamics::ResidualModelContact:: |
235 |
|
|
calcDiff, |
236 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
237 |
|
|
"Compute the Jacobians of the contact-acceleration residual.\n\n" |
238 |
|
|
"It assumes that calc has been run first.\n" |
239 |
|
|
":param data: action data\n" |
240 |
|
|
":param x: state vector\n" |
241 |
✓✗ |
10 |
":param u: control input\n") |
242 |
|
|
.def<void ( |
243 |
|
|
DifferentialActionModelContactInvDynamics::ResidualModelContact::*)( |
244 |
|
|
const boost::shared_ptr<ResidualDataAbstract>&, |
245 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
246 |
|
|
"calcDiff", &ResidualModelAbstract::calcDiff, |
247 |
✓✗✓✗
|
20 |
bp::args("self", "data", "x")) |
248 |
|
|
.def("createData", |
249 |
|
|
&DifferentialActionModelContactInvDynamics::ResidualModelContact:: |
250 |
|
|
createData, |
251 |
|
|
bp::with_custodian_and_ward_postcall<0, 2>(), |
252 |
✓✗ |
20 |
bp::args("self", "data"), |
253 |
|
|
"Create the contact-acceleration residual data.\n\n" |
254 |
|
|
"Each residual model has its own data that needs to be allocated. " |
255 |
|
|
"This function\n" |
256 |
|
|
"returns the allocated data for the contact-acceleration " |
257 |
|
|
"residual.\n" |
258 |
|
|
":param data: shared data\n" |
259 |
✓✗✓✗
|
20 |
":return residual data.") |
260 |
|
10 |
.def(CopyableVisitor<DifferentialActionModelContactInvDynamics:: |
261 |
✓✗ |
10 |
ResidualModelContact>()); |
262 |
|
|
} |
263 |
|
|
|
264 |
|
|
bp::register_ptr_to_python< |
265 |
✓✗ |
10 |
boost::shared_ptr<DifferentialActionDataContactInvDynamics> >(); |
266 |
|
|
|
267 |
|
|
bp::scope data_outer = |
268 |
✓✗ |
10 |
bp::class_<DifferentialActionDataContactInvDynamics, |
269 |
|
|
bp::bases<DifferentialActionDataAbstract> >( |
270 |
|
|
"DifferentialActionDataContactInvDynamics", |
271 |
|
|
"Differential action data for the inverse-dynamics for system with " |
272 |
|
|
"contact.", |
273 |
✓✗ |
20 |
bp::init<DifferentialActionModelContactInvDynamics*>( |
274 |
✓✗ |
10 |
bp::args("self", "model"), |
275 |
|
|
"Create inverse-dynamics action data for system with " |
276 |
|
|
"contacts.\n\n" |
277 |
|
|
":param model: contact inverse-dynamics action model")) |
278 |
|
|
.add_property( |
279 |
|
|
"pinocchio", |
280 |
|
|
bp::make_getter( |
281 |
✓✗ |
10 |
&DifferentialActionDataContactInvDynamics::pinocchio, |
282 |
|
10 |
bp::return_internal_reference<>()), |
283 |
✓✗ |
10 |
"pinocchio data") |
284 |
|
|
.add_property( |
285 |
|
|
"multibody", |
286 |
|
|
bp::make_getter( |
287 |
✓✗ |
10 |
&DifferentialActionDataContactInvDynamics::multibody, |
288 |
|
10 |
bp::return_internal_reference<>()), |
289 |
✓✗ |
10 |
"multibody data") |
290 |
|
|
.add_property( |
291 |
|
|
"costs", |
292 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataContactInvDynamics::costs, |
293 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
294 |
✓✗ |
10 |
"total cost data") |
295 |
|
|
.add_property( |
296 |
|
|
"constraints", |
297 |
|
|
bp::make_getter( |
298 |
✓✗ |
10 |
&DifferentialActionDataContactInvDynamics::constraints, |
299 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
300 |
✓✗ |
10 |
"constraint data") |
301 |
✓✗✓✗
|
10 |
.def(CopyableVisitor<DifferentialActionDataContactInvDynamics>()); |
302 |
|
|
|
303 |
|
|
bp::register_ptr_to_python<boost::shared_ptr< |
304 |
✓✗ |
10 |
DifferentialActionDataContactInvDynamics::ResidualDataActuation> >(); |
305 |
|
|
|
306 |
✓✗ |
10 |
bp::class_<DifferentialActionDataContactInvDynamics::ResidualDataActuation, |
307 |
|
|
bp::bases<ResidualDataAbstract> >( |
308 |
|
|
"ResidualDataActuation", "Data for actuation residual.\n\n", |
309 |
✓✗ |
10 |
bp::init< |
310 |
|
|
DifferentialActionModelContactInvDynamics::ResidualModelActuation*, |
311 |
|
|
DataCollectorAbstract*>( |
312 |
✓✗ |
10 |
bp::args("self", "model", "data"), |
313 |
|
|
"Create actuation residual data.\n\n" |
314 |
|
|
":param model: actuation residual model\n" |
315 |
|
|
":param data: shared data")[bp::with_custodian_and_ward< |
316 |
✓✗ |
20 |
1, 2, bp::with_custodian_and_ward<1, 3> >()]); |
317 |
|
|
|
318 |
|
|
bp::register_ptr_to_python<boost::shared_ptr< |
319 |
✓✗ |
10 |
DifferentialActionDataContactInvDynamics::ResidualDataContact> >(); |
320 |
|
|
|
321 |
✓✗ |
10 |
bp::class_<DifferentialActionDataContactInvDynamics::ResidualDataContact, |
322 |
|
|
bp::bases<ResidualDataAbstract> >( |
323 |
|
|
"ResidualDataContact", "Data for contact acceleration residual.\n\n", |
324 |
✓✗ |
10 |
bp::init<DifferentialActionModelContactInvDynamics::ResidualModelContact*, |
325 |
|
|
DataCollectorAbstract*, std::size_t>( |
326 |
✓✗ |
10 |
bp::args("self", "model", "data", "id"), |
327 |
|
|
"Create contact-acceleration residual data.\n\n" |
328 |
|
|
":param model: contact-acceleration residual model\n" |
329 |
|
|
":param data: shared data\n" |
330 |
|
10 |
":param id: contact id")[bp::with_custodian_and_ward< |
331 |
✓✗ |
20 |
1, 2, bp::with_custodian_and_ward<1, 3> >()]) |
332 |
|
|
.add_property( |
333 |
|
|
"contact", |
334 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataContactInvDynamics:: |
335 |
|
|
ResidualDataContact::contact, |
336 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
337 |
✓✗ |
10 |
"contact data associated with the current residual") |
338 |
|
10 |
.def(CopyableVisitor< |
339 |
✓✗ |
10 |
DifferentialActionDataContactInvDynamics::ResidualDataContact>()); |
340 |
|
10 |
} |
341 |
|
|
|
342 |
|
|
} // namespace python |
343 |
|
|
} // namespace crocoddyl |