| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/////////////////////////////////////////////////////////////////////////////// |
||
2 |
// BSD 3-Clause License |
||
3 |
// |
||
4 |
// Copyright (C) 2019-2024, University of Edinburgh, CTU, INRIA, |
||
5 |
// Heriot-Watt University, University of Pisa |
||
6 |
// Copyright note valid unless otherwise stated in individual files. |
||
7 |
// All rights reserved. |
||
8 |
/////////////////////////////////////////////////////////////////////////////// |
||
9 |
|||
10 |
#include "diff_action.hpp" |
||
11 |
|||
12 |
#include "contact.hpp" |
||
13 |
#include "cost.hpp" |
||
14 |
#include "crocoddyl/core/actions/diff-lqr.hpp" |
||
15 |
#include "crocoddyl/core/activations/quadratic-barrier.hpp" |
||
16 |
#include "crocoddyl/core/activations/quadratic.hpp" |
||
17 |
#include "crocoddyl/core/costs/residual.hpp" |
||
18 |
#include "crocoddyl/core/residuals/joint-acceleration.hpp" |
||
19 |
#include "crocoddyl/core/residuals/joint-effort.hpp" |
||
20 |
#include "crocoddyl/core/utils/exception.hpp" |
||
21 |
#include "crocoddyl/multibody/actuations/floating-base.hpp" |
||
22 |
#include "crocoddyl/multibody/actuations/full.hpp" |
||
23 |
#include "crocoddyl/multibody/residuals/contact-force.hpp" |
||
24 |
#include "crocoddyl/multibody/residuals/contact-friction-cone.hpp" |
||
25 |
#include "crocoddyl/multibody/residuals/contact-wrench-cone.hpp" |
||
26 |
#include "crocoddyl/multibody/residuals/frame-placement.hpp" |
||
27 |
#include "crocoddyl/multibody/residuals/frame-translation.hpp" |
||
28 |
#include "crocoddyl/multibody/states/multibody.hpp" |
||
29 |
|||
30 |
namespace crocoddyl { |
||
31 |
namespace unittest { |
||
32 |
|||
33 |
const std::vector<DifferentialActionModelTypes::Type> |
||
34 |
DifferentialActionModelTypes::all(DifferentialActionModelTypes::init_all()); |
||
35 |
|||
36 |
352 |
std::ostream& operator<<(std::ostream& os, |
|
37 |
DifferentialActionModelTypes::Type type) { |
||
38 |
✓✓✓✓ ✓✓✓✓ ✓✓✓✓ ✓✓✓✓ ✓✓✓✓ ✓✓✗✗ |
352 |
switch (type) { |
39 |
16 |
case DifferentialActionModelTypes::DifferentialActionModelLQR: |
|
40 |
16 |
os << "DifferentialActionModelLQR"; |
|
41 |
16 |
break; |
|
42 |
16 |
case DifferentialActionModelTypes::DifferentialActionModelLQRDriftFree: |
|
43 |
16 |
os << "DifferentialActionModelLQRDriftFree"; |
|
44 |
16 |
break; |
|
45 |
16 |
case DifferentialActionModelTypes:: |
|
46 |
DifferentialActionModelFreeFwdDynamics_Hector: |
||
47 |
16 |
os << "DifferentialActionModelFreeFwdDynamics_Hector"; |
|
48 |
16 |
break; |
|
49 |
16 |
case DifferentialActionModelTypes:: |
|
50 |
DifferentialActionModelFreeFwdDynamics_TalosArm: |
||
51 |
16 |
os << "DifferentialActionModelFreeFwdDynamics_TalosArm"; |
|
52 |
16 |
break; |
|
53 |
16 |
case DifferentialActionModelTypes:: |
|
54 |
DifferentialActionModelFreeFwdDynamics_TalosArm_Squashed: |
||
55 |
16 |
os << "DifferentialActionModelFreeFwdDynamics_TalosArm_Squashed"; |
|
56 |
16 |
break; |
|
57 |
16 |
case DifferentialActionModelTypes:: |
|
58 |
DifferentialActionModelFreeInvDynamics_Hector: |
||
59 |
16 |
os << "DifferentialActionModelFreeInvDynamics_Hector"; |
|
60 |
16 |
break; |
|
61 |
16 |
case DifferentialActionModelTypes:: |
|
62 |
DifferentialActionModelFreeInvDynamics_TalosArm: |
||
63 |
16 |
os << "DifferentialActionModelFreeInvDynamics_TalosArm"; |
|
64 |
16 |
break; |
|
65 |
16 |
case DifferentialActionModelTypes:: |
|
66 |
DifferentialActionModelFreeInvDynamics_TalosArm_Squashed: |
||
67 |
16 |
os << "DifferentialActionModelFreeInvDynamics_TalosArm_Squashed"; |
|
68 |
16 |
break; |
|
69 |
16 |
case DifferentialActionModelTypes:: |
|
70 |
DifferentialActionModelContactFwdDynamics_TalosArm: |
||
71 |
16 |
os << "DifferentialActionModelContactFwdDynamics_TalosArm"; |
|
72 |
16 |
break; |
|
73 |
16 |
case DifferentialActionModelTypes:: |
|
74 |
DifferentialActionModelContact2DFwdDynamics_TalosArm: |
||
75 |
16 |
os << "DifferentialActionModelContact2DFwdDynamics_TalosArm"; |
|
76 |
16 |
break; |
|
77 |
16 |
case DifferentialActionModelTypes:: |
|
78 |
DifferentialActionModelContactFwdDynamics_HyQ: |
||
79 |
16 |
os << "DifferentialActionModelContactFwdDynamics_HyQ"; |
|
80 |
16 |
break; |
|
81 |
16 |
case DifferentialActionModelTypes:: |
|
82 |
DifferentialActionModelContactFwdDynamics_Talos: |
||
83 |
16 |
os << "DifferentialActionModelContactFwdDynamics_Talos"; |
|
84 |
16 |
break; |
|
85 |
16 |
case DifferentialActionModelTypes:: |
|
86 |
DifferentialActionModelContactFwdDynamicsWithFriction_TalosArm: |
||
87 |
16 |
os << "DifferentialActionModelContactFwdDynamicsWithFriction_TalosArm"; |
|
88 |
16 |
break; |
|
89 |
16 |
case DifferentialActionModelTypes:: |
|
90 |
DifferentialActionModelContact2DFwdDynamicsWithFriction_TalosArm: |
||
91 |
16 |
os << "DifferentialActionModelContact2DFwdDynamicsWithFriction_TalosArm"; |
|
92 |
16 |
break; |
|
93 |
16 |
case DifferentialActionModelTypes:: |
|
94 |
DifferentialActionModelContactFwdDynamicsWithFriction_HyQ: |
||
95 |
16 |
os << "DifferentialActionModelContactFwdDynamicsWithFriction_HyQ"; |
|
96 |
16 |
break; |
|
97 |
16 |
case DifferentialActionModelTypes:: |
|
98 |
DifferentialActionModelContactFwdDynamicsWithFriction_Talos: |
||
99 |
16 |
os << "DifferentialActionModelContactFwdDynamicsWithFriction_Talos"; |
|
100 |
16 |
break; |
|
101 |
16 |
case DifferentialActionModelTypes:: |
|
102 |
DifferentialActionModelContactInvDynamics_TalosArm: |
||
103 |
16 |
os << "DifferentialActionModelContactInvDynamics_TalosArm"; |
|
104 |
16 |
break; |
|
105 |
16 |
case DifferentialActionModelTypes:: |
|
106 |
DifferentialActionModelContactInvDynamics_HyQ: |
||
107 |
16 |
os << "DifferentialActionModelContactInvDynamics_HyQ"; |
|
108 |
16 |
break; |
|
109 |
16 |
case DifferentialActionModelTypes:: |
|
110 |
DifferentialActionModelContactInvDynamics_Talos: |
||
111 |
16 |
os << "DifferentialActionModelContactInvDynamics_Talos"; |
|
112 |
16 |
break; |
|
113 |
16 |
case DifferentialActionModelTypes:: |
|
114 |
DifferentialActionModelContactInvDynamicsWithFriction_TalosArm: |
||
115 |
16 |
os << "DifferentialActionModelContactInvDynamicsWithFriction_TalosArm"; |
|
116 |
16 |
break; |
|
117 |
16 |
case DifferentialActionModelTypes:: |
|
118 |
DifferentialActionModelContactInvDynamicsWithFriction_HyQ: |
||
119 |
16 |
os << "DifferentialActionModelContactInvDynamicsWithFriction_HyQ"; |
|
120 |
16 |
break; |
|
121 |
16 |
case DifferentialActionModelTypes:: |
|
122 |
DifferentialActionModelContactInvDynamicsWithFriction_Talos: |
||
123 |
16 |
os << "DifferentialActionModelContactInvDynamicsWithFriction_Talos"; |
|
124 |
16 |
break; |
|
125 |
case DifferentialActionModelTypes::NbDifferentialActionModelTypes: |
||
126 |
os << "NbDifferentialActionModelTypes"; |
||
127 |
break; |
||
128 |
default: |
||
129 |
break; |
||
130 |
} |
||
131 |
352 |
return os; |
|
132 |
} |
||
133 |
|||
134 |
1147 |
DifferentialActionModelFactory::DifferentialActionModelFactory() {} |
|
135 |
1147 |
DifferentialActionModelFactory::~DifferentialActionModelFactory() {} |
|
136 |
|||
137 |
boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> |
||
138 |
1099 |
DifferentialActionModelFactory::create(DifferentialActionModelTypes::Type type, |
|
139 |
bool with_baumgarte) const { |
||
140 |
1099 |
boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> action; |
|
141 |
✓✓✓✓ ✓✓✓✓ ✓✓✓✓ ✓✓✓✓ ✓✓✓✓ ✓✓✗ |
1099 |
switch (type) { |
142 |
50 |
case DifferentialActionModelTypes::DifferentialActionModelLQR: |
|
143 |
50 |
action = boost::make_shared<crocoddyl::DifferentialActionModelLQR>(40, 40, |
|
144 |
✓✗ | 50 |
false); |
145 |
50 |
break; |
|
146 |
50 |
case DifferentialActionModelTypes::DifferentialActionModelLQRDriftFree: |
|
147 |
50 |
action = boost::make_shared<crocoddyl::DifferentialActionModelLQR>(40, 40, |
|
148 |
✓✗ | 50 |
true); |
149 |
50 |
break; |
|
150 |
50 |
case DifferentialActionModelTypes:: |
|
151 |
DifferentialActionModelFreeFwdDynamics_Hector: |
||
152 |
✓✗ | 100 |
action = create_freeFwdDynamics( |
153 |
StateModelTypes::StateMultibody_Hector, |
||
154 |
50 |
ActuationModelTypes::ActuationModelFloatingBaseThrusters, false); |
|
155 |
50 |
break; |
|
156 |
50 |
case DifferentialActionModelTypes:: |
|
157 |
DifferentialActionModelFreeFwdDynamics_TalosArm: |
||
158 |
✓✗ | 100 |
action = create_freeFwdDynamics(StateModelTypes::StateMultibody_TalosArm, |
159 |
50 |
ActuationModelTypes::ActuationModelFull); |
|
160 |
50 |
break; |
|
161 |
49 |
case DifferentialActionModelTypes:: |
|
162 |
DifferentialActionModelFreeFwdDynamics_TalosArm_Squashed: |
||
163 |
✓✗ | 98 |
action = create_freeFwdDynamics( |
164 |
StateModelTypes::StateMultibody_TalosArm, |
||
165 |
49 |
ActuationModelTypes::ActuationModelSquashingFull); |
|
166 |
49 |
break; |
|
167 |
50 |
case DifferentialActionModelTypes:: |
|
168 |
DifferentialActionModelFreeInvDynamics_Hector: |
||
169 |
✓✗ | 100 |
action = create_freeInvDynamics( |
170 |
StateModelTypes::StateMultibody_Hector, |
||
171 |
50 |
ActuationModelTypes::ActuationModelFloatingBaseThrusters); |
|
172 |
50 |
break; |
|
173 |
50 |
case DifferentialActionModelTypes:: |
|
174 |
DifferentialActionModelFreeInvDynamics_TalosArm: |
||
175 |
✓✗ | 100 |
action = create_freeInvDynamics(StateModelTypes::StateMultibody_TalosArm, |
176 |
50 |
ActuationModelTypes::ActuationModelFull); |
|
177 |
50 |
break; |
|
178 |
50 |
case DifferentialActionModelTypes:: |
|
179 |
DifferentialActionModelFreeInvDynamics_TalosArm_Squashed: |
||
180 |
✓✗ | 100 |
action = create_freeInvDynamics( |
181 |
StateModelTypes::StateMultibody_TalosArm, |
||
182 |
50 |
ActuationModelTypes::ActuationModelSquashingFull); |
|
183 |
50 |
break; |
|
184 |
50 |
case DifferentialActionModelTypes:: |
|
185 |
DifferentialActionModelContactFwdDynamics_TalosArm: |
||
186 |
✓✗ | 100 |
action = create_contactFwdDynamics( |
187 |
StateModelTypes::StateMultibody_TalosArm, |
||
188 |
50 |
ActuationModelTypes::ActuationModelFull, false, with_baumgarte); |
|
189 |
50 |
break; |
|
190 |
50 |
case DifferentialActionModelTypes:: |
|
191 |
DifferentialActionModelContact2DFwdDynamics_TalosArm: |
||
192 |
✓✗ | 100 |
action = create_contactFwdDynamics( |
193 |
StateModelTypes::StateMultibodyContact2D_TalosArm, |
||
194 |
50 |
ActuationModelTypes::ActuationModelFull, false, with_baumgarte); |
|
195 |
50 |
break; |
|
196 |
50 |
case DifferentialActionModelTypes:: |
|
197 |
DifferentialActionModelContactFwdDynamics_HyQ: |
||
198 |
✓✗ | 100 |
action = create_contactFwdDynamics( |
199 |
StateModelTypes::StateMultibody_HyQ, |
||
200 |
ActuationModelTypes::ActuationModelFloatingBase, false, |
||
201 |
50 |
with_baumgarte); |
|
202 |
50 |
break; |
|
203 |
50 |
case DifferentialActionModelTypes:: |
|
204 |
DifferentialActionModelContactFwdDynamics_Talos: |
||
205 |
✓✗ | 100 |
action = create_contactFwdDynamics( |
206 |
StateModelTypes::StateMultibody_Talos, |
||
207 |
ActuationModelTypes::ActuationModelFloatingBase, false, |
||
208 |
50 |
with_baumgarte); |
|
209 |
50 |
break; |
|
210 |
50 |
case DifferentialActionModelTypes:: |
|
211 |
DifferentialActionModelContactFwdDynamicsWithFriction_TalosArm: |
||
212 |
✓✗ | 100 |
action = create_contactFwdDynamics( |
213 |
StateModelTypes::StateMultibody_TalosArm, |
||
214 |
50 |
ActuationModelTypes::ActuationModelFull, true, with_baumgarte); |
|
215 |
50 |
break; |
|
216 |
50 |
case DifferentialActionModelTypes:: |
|
217 |
DifferentialActionModelContact2DFwdDynamicsWithFriction_TalosArm: |
||
218 |
✓✗ | 100 |
action = create_contactFwdDynamics( |
219 |
StateModelTypes::StateMultibodyContact2D_TalosArm, |
||
220 |
50 |
ActuationModelTypes::ActuationModelFull, true, with_baumgarte); |
|
221 |
50 |
break; |
|
222 |
50 |
case DifferentialActionModelTypes:: |
|
223 |
DifferentialActionModelContactFwdDynamicsWithFriction_HyQ: |
||
224 |
✓✗ | 100 |
action = create_contactFwdDynamics( |
225 |
StateModelTypes::StateMultibody_HyQ, |
||
226 |
ActuationModelTypes::ActuationModelFloatingBase, true, |
||
227 |
50 |
with_baumgarte); |
|
228 |
50 |
break; |
|
229 |
50 |
case DifferentialActionModelTypes:: |
|
230 |
DifferentialActionModelContactFwdDynamicsWithFriction_Talos: |
||
231 |
✓✗ | 100 |
action = create_contactFwdDynamics( |
232 |
StateModelTypes::StateMultibody_Talos, |
||
233 |
ActuationModelTypes::ActuationModelFloatingBase, true, |
||
234 |
50 |
with_baumgarte); |
|
235 |
50 |
break; |
|
236 |
50 |
case DifferentialActionModelTypes:: |
|
237 |
DifferentialActionModelContactInvDynamics_TalosArm: |
||
238 |
✓✗ | 100 |
action = create_contactInvDynamics( |
239 |
StateModelTypes::StateMultibody_TalosArm, |
||
240 |
ActuationModelTypes::ActuationModelFloatingBase, false, |
||
241 |
50 |
with_baumgarte); |
|
242 |
50 |
break; |
|
243 |
50 |
case DifferentialActionModelTypes:: |
|
244 |
DifferentialActionModelContactInvDynamics_HyQ: |
||
245 |
✓✗ | 100 |
action = create_contactInvDynamics( |
246 |
StateModelTypes::StateMultibody_HyQ, |
||
247 |
ActuationModelTypes::ActuationModelFloatingBase, false, |
||
248 |
50 |
with_baumgarte); |
|
249 |
50 |
break; |
|
250 |
50 |
case DifferentialActionModelTypes:: |
|
251 |
DifferentialActionModelContactInvDynamics_Talos: |
||
252 |
✓✗ | 100 |
action = create_contactInvDynamics( |
253 |
StateModelTypes::StateMultibody_Talos, |
||
254 |
ActuationModelTypes::ActuationModelFloatingBase, false, |
||
255 |
50 |
with_baumgarte); |
|
256 |
50 |
break; |
|
257 |
50 |
case DifferentialActionModelTypes:: |
|
258 |
DifferentialActionModelContactInvDynamicsWithFriction_TalosArm: |
||
259 |
✓✗ | 100 |
action = create_contactInvDynamics( |
260 |
StateModelTypes::StateMultibody_TalosArm, |
||
261 |
50 |
ActuationModelTypes::ActuationModelFull, true, with_baumgarte); |
|
262 |
50 |
break; |
|
263 |
50 |
case DifferentialActionModelTypes:: |
|
264 |
DifferentialActionModelContactInvDynamicsWithFriction_HyQ: |
||
265 |
✓✗ | 100 |
action = create_contactInvDynamics( |
266 |
StateModelTypes::StateMultibody_HyQ, |
||
267 |
ActuationModelTypes::ActuationModelFloatingBase, true, |
||
268 |
50 |
with_baumgarte); |
|
269 |
50 |
break; |
|
270 |
50 |
case DifferentialActionModelTypes:: |
|
271 |
DifferentialActionModelContactInvDynamicsWithFriction_Talos: |
||
272 |
✓✗ | 100 |
action = create_contactInvDynamics( |
273 |
StateModelTypes::StateMultibody_Talos, |
||
274 |
ActuationModelTypes::ActuationModelFloatingBase, true, |
||
275 |
50 |
with_baumgarte); |
|
276 |
50 |
break; |
|
277 |
default: |
||
278 |
throw_pretty(__FILE__ ": Wrong DifferentialActionModelTypes::Type given"); |
||
279 |
break; |
||
280 |
} |
||
281 |
1099 |
return action; |
|
282 |
} |
||
283 |
|||
284 |
boost::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics> |
||
285 |
149 |
DifferentialActionModelFactory::create_freeFwdDynamics( |
|
286 |
StateModelTypes::Type state_type, ActuationModelTypes::Type actuation_type, |
||
287 |
bool constraints) const { |
||
288 |
149 |
boost::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics> action; |
|
289 |
149 |
boost::shared_ptr<crocoddyl::StateMultibody> state; |
|
290 |
149 |
boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation; |
|
291 |
149 |
boost::shared_ptr<crocoddyl::CostModelSum> cost; |
|
292 |
149 |
boost::shared_ptr<crocoddyl::ConstraintModelManager> constraint; |
|
293 |
298 |
state = boost::static_pointer_cast<crocoddyl::StateMultibody>( |
|
294 |
✓✗✓✗ |
447 |
StateModelFactory().create(state_type)); |
295 |
✓✗✓✗ |
149 |
actuation = ActuationModelFactory().create(actuation_type, state_type); |
296 |
149 |
const std::size_t nu = actuation->get_nu(); |
|
297 |
✓✗ | 149 |
cost = boost::make_shared<crocoddyl::CostModelSum>(state, nu); |
298 |
✓✗✓✗ |
298 |
cost->addCost("state", |
299 |
✓✗✓✗ |
298 |
CostModelFactory().create( |
300 |
CostModelTypes::CostModelResidualState, state_type, |
||
301 |
ActivationModelTypes::ActivationModelQuad, nu), |
||
302 |
1.); |
||
303 |
✓✗✓✗ |
298 |
cost->addCost("control", |
304 |
✓✗✓✗ |
298 |
CostModelFactory().create( |
305 |
CostModelTypes::CostModelResidualControl, state_type, |
||
306 |
ActivationModelTypes::ActivationModelQuad, nu), |
||
307 |
1.); |
||
308 |
✓✗✓✗ |
298 |
cost->addCost( |
309 |
"joint_eff", |
||
310 |
✓✗ | 298 |
boost::make_shared<crocoddyl::CostModelResidual>( |
311 |
✓✗ | 298 |
state, boost::make_shared<crocoddyl::ResidualModelJointEffort>( |
312 |
✓✗ | 149 |
state, actuation, Eigen::VectorXd::Zero(nu), nu, true)), |
313 |
1.); |
||
314 |
✓✗✓✗ |
298 |
cost->addCost( |
315 |
"joint_acc", |
||
316 |
✓✗ | 298 |
boost::make_shared<crocoddyl::CostModelResidual>( |
317 |
✓✗ | 298 |
state, boost::make_shared<crocoddyl::ResidualModelJointAcceleration>( |
318 |
state, nu)), |
||
319 |
0.01); |
||
320 |
✓✗✓✗ |
298 |
cost->addCost("frame", |
321 |
✓✗✓✗ |
298 |
CostModelFactory().create( |
322 |
CostModelTypes::CostModelResidualFramePlacement, state_type, |
||
323 |
ActivationModelTypes::ActivationModelQuad, nu), |
||
324 |
1.); |
||
325 |
✓✓ | 149 |
if (constraints) { |
326 |
constraint = |
||
327 |
✓✗ | 99 |
boost::make_shared<crocoddyl::ConstraintModelManager>(state, nu); |
328 |
✓✗✓✗ |
198 |
constraint->addConstraint( |
329 |
"frame", |
||
330 |
✓✗✓✗ |
198 |
ConstraintModelFactory().create( |
331 |
ConstraintModelTypes::ConstraintModelResidualFramePlacementEquality, |
||
332 |
state_type, nu)); |
||
333 |
✓✗✓✗ |
198 |
constraint->addConstraint( |
334 |
"frame-velocity", |
||
335 |
✓✗✓✗ |
198 |
ConstraintModelFactory().create( |
336 |
ConstraintModelTypes::ConstraintModelResidualFrameVelocityEquality, |
||
337 |
state_type, nu)); |
||
338 |
action = |
||
339 |
✓✗ | 198 |
boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>( |
340 |
99 |
state, actuation, cost, constraint); |
|
341 |
} else { |
||
342 |
action = |
||
343 |
✓✗ | 100 |
boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>( |
344 |
50 |
state, actuation, cost); |
|
345 |
} |
||
346 |
298 |
return action; |
|
347 |
} |
||
348 |
|||
349 |
boost::shared_ptr<crocoddyl::DifferentialActionModelFreeInvDynamics> |
||
350 |
150 |
DifferentialActionModelFactory::create_freeInvDynamics( |
|
351 |
StateModelTypes::Type state_type, ActuationModelTypes::Type actuation_type, |
||
352 |
bool constraints) const { |
||
353 |
150 |
boost::shared_ptr<crocoddyl::DifferentialActionModelFreeInvDynamics> action; |
|
354 |
150 |
boost::shared_ptr<crocoddyl::StateMultibody> state; |
|
355 |
150 |
boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation; |
|
356 |
150 |
boost::shared_ptr<crocoddyl::CostModelSum> cost; |
|
357 |
150 |
boost::shared_ptr<crocoddyl::ConstraintModelManager> constraint; |
|
358 |
300 |
state = boost::static_pointer_cast<crocoddyl::StateMultibody>( |
|
359 |
✓✗✓✗ |
450 |
StateModelFactory().create(state_type)); |
360 |
✓✗✓✗ |
150 |
actuation = ActuationModelFactory().create(actuation_type, state_type); |
361 |
150 |
const std::size_t nu = state->get_nv(); |
|
362 |
✓✗ | 150 |
cost = boost::make_shared<crocoddyl::CostModelSum>(state, nu); |
363 |
✓✗✓✗ |
300 |
cost->addCost("state", |
364 |
✓✗✓✗ |
300 |
CostModelFactory().create( |
365 |
CostModelTypes::CostModelResidualState, state_type, |
||
366 |
ActivationModelTypes::ActivationModelQuad, nu), |
||
367 |
1.); |
||
368 |
✓✗✓✗ |
300 |
cost->addCost("control", |
369 |
✓✗✓✗ |
300 |
CostModelFactory().create( |
370 |
CostModelTypes::CostModelResidualControl, state_type, |
||
371 |
ActivationModelTypes::ActivationModelQuad, nu), |
||
372 |
1.); |
||
373 |
✓✗✓✗ |
300 |
cost->addCost("frame", |
374 |
✓✗✓✗ |
300 |
CostModelFactory().create( |
375 |
CostModelTypes::CostModelResidualFramePlacement, state_type, |
||
376 |
ActivationModelTypes::ActivationModelQuad, nu), |
||
377 |
1.); |
||
378 |
✓✗ | 150 |
if (constraints) { |
379 |
constraint = |
||
380 |
✓✗ | 150 |
boost::make_shared<crocoddyl::ConstraintModelManager>(state, nu); |
381 |
✓✗✓✗ |
300 |
constraint->addConstraint( |
382 |
"frame", |
||
383 |
✓✗✓✗ |
300 |
ConstraintModelFactory().create( |
384 |
ConstraintModelTypes::ConstraintModelResidualFramePlacementEquality, |
||
385 |
state_type, nu)); |
||
386 |
✓✗✓✗ |
300 |
constraint->addConstraint( |
387 |
"frame-velocity", |
||
388 |
✓✗✓✗ |
300 |
ConstraintModelFactory().create( |
389 |
ConstraintModelTypes::ConstraintModelResidualFrameVelocityEquality, |
||
390 |
state_type, nu)); |
||
391 |
action = |
||
392 |
✓✗ | 300 |
boost::make_shared<crocoddyl::DifferentialActionModelFreeInvDynamics>( |
393 |
150 |
state, actuation, cost, constraint); |
|
394 |
} else { |
||
395 |
action = |
||
396 |
boost::make_shared<crocoddyl::DifferentialActionModelFreeInvDynamics>( |
||
397 |
state, actuation, cost); |
||
398 |
} |
||
399 |
300 |
return action; |
|
400 |
} |
||
401 |
|||
402 |
boost::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics> |
||
403 |
448 |
DifferentialActionModelFactory::create_contactFwdDynamics( |
|
404 |
StateModelTypes::Type state_type, ActuationModelTypes::Type actuation_type, |
||
405 |
bool with_friction, bool with_baumgarte) const { |
||
406 |
boost::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics> |
||
407 |
448 |
action; |
|
408 |
448 |
boost::shared_ptr<crocoddyl::StateMultibody> state; |
|
409 |
448 |
boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation; |
|
410 |
448 |
boost::shared_ptr<crocoddyl::ContactModelMultiple> contact; |
|
411 |
448 |
boost::shared_ptr<crocoddyl::CostModelSum> cost; |
|
412 |
896 |
state = boost::static_pointer_cast<crocoddyl::StateMultibody>( |
|
413 |
✓✗✓✗ |
1344 |
StateModelFactory().create(state_type)); |
414 |
✓✗✓✗ |
448 |
actuation = ActuationModelFactory().create(actuation_type, state_type); |
415 |
448 |
const std::size_t nu = actuation->get_nu(); |
|
416 |
✓✗ | 448 |
contact = boost::make_shared<crocoddyl::ContactModelMultiple>(state, nu); |
417 |
✓✗ | 448 |
cost = boost::make_shared<crocoddyl::CostModelSum>(state, nu); |
418 |
|||
419 |
✓✗✓✗ |
448 |
Eigen::Matrix3d R = Eigen::Matrix3d::Identity(); |
420 |
✓✗ | 448 |
pinocchio::Force force = pinocchio::Force::Zero(); |
421 |
✓✗ | 896 |
crocoddyl::FrictionCone friction_cone(R, 0.8, 4, false); |
422 |
✓✗ | 448 |
crocoddyl::WrenchCone wrench_cone(R, 0.8, Eigen::Vector2d(0.1, 0.1), 4, |
423 |
✓✗ | 1344 |
false); |
424 |
448 |
crocoddyl::ActivationBounds friction_bounds(friction_cone.get_lb(), |
|
425 |
✓✗ | 1344 |
friction_cone.get_ub()); |
426 |
448 |
crocoddyl::ActivationBounds wrench_bounds(wrench_cone.get_lb(), |
|
427 |
✓✗ | 1344 |
wrench_cone.get_ub()); |
428 |
boost::shared_ptr<crocoddyl::ActivationModelAbstract> friction_activation = |
||
429 |
✓✗ | 448 |
boost::make_shared<crocoddyl::ActivationModelQuadraticBarrier>( |
430 |
896 |
friction_bounds); |
|
431 |
boost::shared_ptr<crocoddyl::ActivationModelAbstract> wrench_activation = |
||
432 |
✓✗ | 448 |
boost::make_shared<crocoddyl::ActivationModelQuadraticBarrier>( |
433 |
896 |
wrench_bounds); |
|
434 |
✓✗✓✗ |
448 |
Eigen::Vector2d gains = Eigen::Vector2d::Random(); |
435 |
✓✓ | 448 |
if (!with_baumgarte) { |
436 |
✓✗ | 8 |
gains.setZero(); |
437 |
} |
||
438 |
|||
439 |
✓✓✓✓ ✗ |
448 |
switch (state_type) { |
440 |
100 |
case StateModelTypes::StateMultibody_TalosArm: |
|
441 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
100 |
contact->addContact("lf", ContactModelFactory().create( |
442 |
ContactModelTypes::ContactModel3D_LOCAL, |
||
443 |
PinocchioModelTypes::TalosArm, gains, |
||
444 |
"gripper_left_fingertip_1_link", nu)); |
||
445 |
✓✓ | 100 |
if (with_friction) { |
446 |
// friction cone |
||
447 |
✓✗✓✗ |
100 |
cost->addCost( |
448 |
"lf_cone", |
||
449 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
450 |
state, friction_activation, |
||
451 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>( |
|
452 |
state, |
||
453 |
✓✗✓✗ ✓✗ |
100 |
state->get_pinocchio()->getFrameId( |
454 |
"gripper_left_fingertip_1_link"), |
||
455 |
friction_cone, nu)), |
||
456 |
0.1); |
||
457 |
// force regularization |
||
458 |
✓✗✓✗ |
100 |
cost->addCost( |
459 |
"lf_forceReg", |
||
460 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
461 |
50 |
state, boost::make_shared<crocoddyl::ResidualModelContactForce>( |
|
462 |
state, |
||
463 |
✓✗✓✗ ✓✗ |
100 |
state->get_pinocchio()->getFrameId( |
464 |
"gripper_left_fingertip_1_link"), |
||
465 |
100 |
force, 3, nu)), |
|
466 |
0.1); |
||
467 |
} |
||
468 |
100 |
break; |
|
469 |
100 |
case StateModelTypes::StateMultibodyContact2D_TalosArm: |
|
470 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
100 |
contact->addContact("lf", ContactModelFactory().create( |
471 |
ContactModelTypes::ContactModel2D, |
||
472 |
PinocchioModelTypes::TalosArm, gains, |
||
473 |
"gripper_left_fingertip_1_link", nu)); |
||
474 |
✓✓ | 100 |
if (with_friction) { |
475 |
// friction cone |
||
476 |
✓✗✓✗ |
150 |
cost->addCost( |
477 |
"lf_cone", |
||
478 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
479 |
state, friction_activation, |
||
480 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>( |
|
481 |
state, |
||
482 |
✓✗✓✗ ✓✗ |
100 |
state->get_pinocchio()->getFrameId( |
483 |
"gripper_left_fingertip_1_link"), |
||
484 |
friction_cone, nu)), |
||
485 |
0.1); |
||
486 |
// TODO: enable force regularization once it would support Contact2D |
||
487 |
} |
||
488 |
100 |
break; |
|
489 |
118 |
case StateModelTypes::StateMultibody_HyQ: |
|
490 |
✓✗✓✗ |
236 |
contact->addContact( |
491 |
✓✗✓✗ ✓✗✓✗ |
472 |
"lf", ContactModelFactory().create( |
492 |
ContactModelTypes::ContactModel3D_LOCAL, |
||
493 |
✓✗ | 236 |
PinocchioModelTypes::HyQ, 0.25 * gains, "lf_foot", nu)); |
494 |
✓✗✓✗ |
236 |
contact->addContact( |
495 |
✓✗✓✗ ✓✗✓✗ |
472 |
"rf", ContactModelFactory().create( |
496 |
ContactModelTypes::ContactModel3D_WORLD, |
||
497 |
✓✗ | 236 |
PinocchioModelTypes::HyQ, 0.25 * gains, "rf_foot", nu)); |
498 |
✓✗✓✗ |
236 |
contact->addContact( |
499 |
✓✗✓✗ ✓✗✓✗ |
472 |
"lh", ContactModelFactory().create( |
500 |
ContactModelTypes::ContactModel3D_LWA, |
||
501 |
✓✗ | 236 |
PinocchioModelTypes::HyQ, 0.25 * gains, "lh_foot", nu)); |
502 |
✓✗✓✗ |
236 |
contact->addContact( |
503 |
✓✗✓✗ ✓✗✓✗ |
472 |
"rh", ContactModelFactory().create( |
504 |
ContactModelTypes::ContactModel3D_LOCAL, |
||
505 |
✓✗ | 236 |
PinocchioModelTypes::HyQ, 0.25 * gains, "rh_foot", nu)); |
506 |
✓✓ | 118 |
if (with_friction) { |
507 |
// friction cone |
||
508 |
✓✗✓✗ |
100 |
cost->addCost( |
509 |
"lf_cone", |
||
510 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
511 |
state, friction_activation, |
||
512 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>( |
|
513 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("lf_foot"), |
514 |
friction_cone, nu)), |
||
515 |
0.1); |
||
516 |
✓✗✓✗ |
100 |
cost->addCost( |
517 |
"rf_cone", |
||
518 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
519 |
state, friction_activation, |
||
520 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>( |
|
521 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("rf_foot"), |
522 |
friction_cone, nu)), |
||
523 |
0.1); |
||
524 |
✓✗✓✗ |
100 |
cost->addCost( |
525 |
"lh_cone", |
||
526 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
527 |
state, friction_activation, |
||
528 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>( |
|
529 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("lh_foot"), |
530 |
friction_cone, nu)), |
||
531 |
0.1); |
||
532 |
✓✗✓✗ |
100 |
cost->addCost( |
533 |
"rh_cone", |
||
534 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
535 |
state, friction_activation, |
||
536 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>( |
|
537 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("rh_foot"), |
538 |
friction_cone, nu)), |
||
539 |
0.1); |
||
540 |
// force regularization |
||
541 |
✓✗✓✗ |
100 |
cost->addCost( |
542 |
"lf_forceReg", |
||
543 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
544 |
50 |
state, boost::make_shared<crocoddyl::ResidualModelContactForce>( |
|
545 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("lf_foot"), |
546 |
50 |
force, 3, nu)), |
|
547 |
0.1); |
||
548 |
✓✗✓✗ |
100 |
cost->addCost( |
549 |
"rf_forceReg", |
||
550 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
551 |
50 |
state, boost::make_shared<crocoddyl::ResidualModelContactForce>( |
|
552 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("rf_foot"), |
553 |
50 |
force, 3, nu)), |
|
554 |
0.1); |
||
555 |
✓✗✓✗ |
100 |
cost->addCost( |
556 |
"lh_forceReg", |
||
557 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
558 |
50 |
state, boost::make_shared<crocoddyl::ResidualModelContactForce>( |
|
559 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("lh_foot"), |
560 |
50 |
force, 3, nu)), |
|
561 |
0.1); |
||
562 |
✓✗✓✗ |
100 |
cost->addCost( |
563 |
"rh_forceReg", |
||
564 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
565 |
50 |
state, boost::make_shared<crocoddyl::ResidualModelContactForce>( |
|
566 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("rh_foot"), |
567 |
100 |
force, 3, nu)), |
|
568 |
0.1); |
||
569 |
} |
||
570 |
118 |
break; |
|
571 |
130 |
case StateModelTypes::StateMultibody_Talos: |
|
572 |
✓✗✓✗ |
260 |
contact->addContact( |
573 |
✓✗✓✗ ✓✗✓✗ |
260 |
"lf", ContactModelFactory().create( |
574 |
ContactModelTypes::ContactModel6D_LOCAL, |
||
575 |
PinocchioModelTypes::Talos, gains, "left_sole_link", nu)); |
||
576 |
✓✗✓✗ |
260 |
contact->addContact( |
577 |
✓✗✓✗ ✓✗✓✗ |
260 |
"rf", ContactModelFactory().create( |
578 |
ContactModelTypes::ContactModel6D_WORLD, |
||
579 |
PinocchioModelTypes::Talos, gains, "right_sole_link", nu)); |
||
580 |
✓✓ | 130 |
if (with_friction) { |
581 |
// friction / wrench cone |
||
582 |
✓✗✓✗ |
100 |
cost->addCost( |
583 |
"lf_cone", |
||
584 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
585 |
state, friction_activation, |
||
586 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>( |
|
587 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("left_sole_link"), |
588 |
friction_cone, nu)), |
||
589 |
0.01); |
||
590 |
✓✗✓✗ |
100 |
cost->addCost( |
591 |
"rf_cone", |
||
592 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
593 |
state, wrench_activation, |
||
594 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactWrenchCone>( |
|
595 |
state, |
||
596 |
✓✗✓✗ ✓✗ |
100 |
state->get_pinocchio()->getFrameId("right_sole_link"), |
597 |
wrench_cone, nu)), |
||
598 |
0.01); |
||
599 |
// force regularization |
||
600 |
✓✗✓✗ |
100 |
cost->addCost( |
601 |
"lf_forceReg", |
||
602 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
603 |
state, |
||
604 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactForce>( |
|
605 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("left_sole_link"), |
606 |
50 |
force, 6, nu)), |
|
607 |
0.01); |
||
608 |
✓✗✓✗ |
100 |
cost->addCost( |
609 |
"rf_forceReg", |
||
610 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
611 |
state, |
||
612 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactForce>( |
|
613 |
state, |
||
614 |
✓✗✓✗ ✓✗ |
100 |
state->get_pinocchio()->getFrameId("right_sole_link"), |
615 |
100 |
force, 6, nu)), |
|
616 |
0.01); |
||
617 |
} |
||
618 |
130 |
break; |
|
619 |
default: |
||
620 |
throw_pretty(__FILE__ ": Wrong StateModelTypes::Type given"); |
||
621 |
break; |
||
622 |
} |
||
623 |
✓✗✓✗ |
896 |
cost->addCost("state", |
624 |
✓✗✓✗ |
896 |
CostModelFactory().create( |
625 |
CostModelTypes::CostModelResidualState, state_type, |
||
626 |
ActivationModelTypes::ActivationModelQuad, nu), |
||
627 |
0.1); |
||
628 |
✓✗✓✗ |
896 |
cost->addCost("control", |
629 |
✓✗✓✗ |
896 |
CostModelFactory().create( |
630 |
CostModelTypes::CostModelResidualControl, state_type, |
||
631 |
ActivationModelTypes::ActivationModelQuad, nu), |
||
632 |
0.1); |
||
633 |
✓✗✓✗ |
896 |
cost->addCost( |
634 |
"joint_eff", |
||
635 |
✓✗ | 896 |
boost::make_shared<crocoddyl::CostModelResidual>( |
636 |
✓✗ | 896 |
state, boost::make_shared<crocoddyl::ResidualModelJointEffort>( |
637 |
✓✗ | 448 |
state, actuation, Eigen::VectorXd::Zero(nu), nu, true)), |
638 |
0.1); |
||
639 |
action = |
||
640 |
448 |
boost::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>( |
|
641 |
✓✗ | 448 |
state, actuation, contact, cost, 0., true); |
642 |
896 |
return action; |
|
643 |
} |
||
644 |
|||
645 |
boost::shared_ptr<crocoddyl::DifferentialActionModelContactInvDynamics> |
||
646 |
300 |
DifferentialActionModelFactory::create_contactInvDynamics( |
|
647 |
StateModelTypes::Type state_type, ActuationModelTypes::Type actuation_type, |
||
648 |
bool with_friction, bool with_baumgarte) const { |
||
649 |
boost::shared_ptr<crocoddyl::DifferentialActionModelContactInvDynamics> |
||
650 |
300 |
action; |
|
651 |
300 |
boost::shared_ptr<crocoddyl::StateMultibody> state; |
|
652 |
300 |
boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation; |
|
653 |
300 |
boost::shared_ptr<crocoddyl::ContactModelMultiple> contact; |
|
654 |
300 |
boost::shared_ptr<crocoddyl::CostModelSum> cost; |
|
655 |
600 |
state = boost::static_pointer_cast<crocoddyl::StateMultibody>( |
|
656 |
✓✗✓✗ |
900 |
StateModelFactory().create(state_type)); |
657 |
✓✗✓✗ |
300 |
actuation = ActuationModelFactory().create(actuation_type, state_type); |
658 |
300 |
std::size_t nu = state->get_nv(); |
|
659 |
|||
660 |
✓✗✓✗ |
300 |
Eigen::Matrix3d R = Eigen::Matrix3d::Identity(); |
661 |
✓✗ | 300 |
pinocchio::Force force = pinocchio::Force::Zero(); |
662 |
✓✗ | 600 |
crocoddyl::FrictionCone friction_cone(R, 0.8, 4, false); |
663 |
✓✗ | 300 |
crocoddyl::WrenchCone wrench_cone(R, 0.8, Eigen::Vector2d(0.1, 0.1), 4, |
664 |
✓✗ | 900 |
false); |
665 |
300 |
crocoddyl::ActivationBounds friction_bounds(friction_cone.get_lb(), |
|
666 |
✓✗ | 900 |
friction_cone.get_ub()); |
667 |
300 |
crocoddyl::ActivationBounds wrench_bounds(wrench_cone.get_lb(), |
|
668 |
✓✗ | 900 |
wrench_cone.get_ub()); |
669 |
boost::shared_ptr<crocoddyl::ActivationModelAbstract> friction_activation = |
||
670 |
✓✗ | 300 |
boost::make_shared<crocoddyl::ActivationModelQuadraticBarrier>( |
671 |
600 |
friction_bounds); |
|
672 |
boost::shared_ptr<crocoddyl::ActivationModelAbstract> wrench_activation = |
||
673 |
✓✗ | 300 |
boost::make_shared<crocoddyl::ActivationModelQuadraticBarrier>( |
674 |
600 |
wrench_bounds); |
|
675 |
✓✗✓✗ |
300 |
Eigen::Vector2d gains = Eigen::Vector2d::Random(); |
676 |
✓✓ | 300 |
if (!with_baumgarte) { |
677 |
✓✗ | 6 |
gains.setZero(); |
678 |
} |
||
679 |
|||
680 |
✓✓✓✗ |
300 |
switch (state_type) { |
681 |
100 |
case StateModelTypes::StateMultibody_TalosArm: |
|
682 |
100 |
nu += 3; |
|
683 |
✓✗ | 100 |
contact = boost::make_shared<crocoddyl::ContactModelMultiple>(state, nu); |
684 |
✓✗ | 100 |
cost = boost::make_shared<crocoddyl::CostModelSum>(state, nu); |
685 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
100 |
contact->addContact("lf", ContactModelFactory().create( |
686 |
ContactModelTypes::ContactModel3D_LOCAL, |
||
687 |
PinocchioModelTypes::TalosArm, gains, |
||
688 |
"gripper_left_fingertip_1_link", nu)); |
||
689 |
✓✓ | 100 |
if (with_friction) { |
690 |
// friction cone |
||
691 |
✓✗✓✗ |
100 |
cost->addCost( |
692 |
"lf_cone", |
||
693 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
694 |
state, friction_activation, |
||
695 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>( |
|
696 |
state, |
||
697 |
✓✗✓✗ ✓✗ |
100 |
state->get_pinocchio()->getFrameId( |
698 |
"gripper_left_fingertip_1_link"), |
||
699 |
50 |
friction_cone, nu, false)), |
|
700 |
0.1); |
||
701 |
// force regularization |
||
702 |
✓✗✓✗ |
100 |
cost->addCost( |
703 |
"lf_forceReg", |
||
704 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
705 |
50 |
state, boost::make_shared<crocoddyl::ResidualModelContactForce>( |
|
706 |
state, |
||
707 |
✓✗✓✗ ✓✗ |
100 |
state->get_pinocchio()->getFrameId( |
708 |
"gripper_left_fingertip_1_link"), |
||
709 |
100 |
force, 3, nu, false)), |
|
710 |
0.1); |
||
711 |
} |
||
712 |
100 |
break; |
|
713 |
100 |
case StateModelTypes::StateMultibody_HyQ: |
|
714 |
100 |
nu += 12; // it includes nc |
|
715 |
✓✗ | 100 |
contact = boost::make_shared<crocoddyl::ContactModelMultiple>(state, nu); |
716 |
✓✗ | 100 |
cost = boost::make_shared<crocoddyl::CostModelSum>(state, nu); |
717 |
✓✗✓✗ |
200 |
contact->addContact("lf", |
718 |
✓✗✓✗ ✓✗✓✗ |
200 |
ContactModelFactory().create( |
719 |
ContactModelTypes::ContactModel3D_LOCAL, |
||
720 |
PinocchioModelTypes::HyQ, gains, "lf_foot", nu)); |
||
721 |
✓✗✓✗ |
200 |
contact->addContact("rf", |
722 |
✓✗✓✗ ✓✗✓✗ |
200 |
ContactModelFactory().create( |
723 |
ContactModelTypes::ContactModel3D_WORLD, |
||
724 |
PinocchioModelTypes::HyQ, gains, "rf_foot", nu)); |
||
725 |
✓✗✓✗ |
200 |
contact->addContact("lh", |
726 |
✓✗✓✗ ✓✗✓✗ |
200 |
ContactModelFactory().create( |
727 |
ContactModelTypes::ContactModel3D_LWA, |
||
728 |
PinocchioModelTypes::HyQ, gains, "lh_foot", nu)); |
||
729 |
✓✗✓✗ |
200 |
contact->addContact("rh", |
730 |
✓✗✓✗ ✓✗✓✗ |
200 |
ContactModelFactory().create( |
731 |
ContactModelTypes::ContactModel3D_LOCAL, |
||
732 |
PinocchioModelTypes::HyQ, gains, "rh_foot", nu)); |
||
733 |
✓✓ | 100 |
if (with_friction) { |
734 |
// friction cone |
||
735 |
✓✗✓✗ |
100 |
cost->addCost( |
736 |
"lf_cone", |
||
737 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
738 |
state, friction_activation, |
||
739 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>( |
|
740 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("lf_foot"), |
741 |
50 |
friction_cone, nu, false)), |
|
742 |
0.1); |
||
743 |
✓✗✓✗ |
100 |
cost->addCost( |
744 |
"rf_cone", |
||
745 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
746 |
state, friction_activation, |
||
747 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>( |
|
748 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("rf_foot"), |
749 |
50 |
friction_cone, nu, false)), |
|
750 |
0.1); |
||
751 |
✓✗✓✗ |
100 |
cost->addCost( |
752 |
"lh_cone", |
||
753 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
754 |
state, friction_activation, |
||
755 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>( |
|
756 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("lh_foot"), |
757 |
50 |
friction_cone, nu, false)), |
|
758 |
0.1); |
||
759 |
✓✗✓✗ |
100 |
cost->addCost( |
760 |
"rh_cone", |
||
761 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
762 |
state, friction_activation, |
||
763 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>( |
|
764 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("rh_foot"), |
765 |
50 |
friction_cone, nu, false)), |
|
766 |
0.1); |
||
767 |
// force regularization |
||
768 |
✓✗✓✗ |
100 |
cost->addCost( |
769 |
"lf_forceReg", |
||
770 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
771 |
50 |
state, boost::make_shared<crocoddyl::ResidualModelContactForce>( |
|
772 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("lf_foot"), |
773 |
50 |
force, 3, nu, false)), |
|
774 |
0.1); |
||
775 |
✓✗✓✗ |
100 |
cost->addCost( |
776 |
"rf_forceReg", |
||
777 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
778 |
50 |
state, boost::make_shared<crocoddyl::ResidualModelContactForce>( |
|
779 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("rf_foot"), |
780 |
50 |
force, 3, nu, false)), |
|
781 |
0.1); |
||
782 |
✓✗✓✗ |
100 |
cost->addCost( |
783 |
"lh_forceReg", |
||
784 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
785 |
50 |
state, boost::make_shared<crocoddyl::ResidualModelContactForce>( |
|
786 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("lh_foot"), |
787 |
50 |
force, 3, nu, false)), |
|
788 |
0.1); |
||
789 |
✓✗✓✗ |
100 |
cost->addCost( |
790 |
"rh_forceReg", |
||
791 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
792 |
50 |
state, boost::make_shared<crocoddyl::ResidualModelContactForce>( |
|
793 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("rh_foot"), |
794 |
100 |
force, 3, nu, false)), |
|
795 |
0.1); |
||
796 |
} |
||
797 |
100 |
break; |
|
798 |
100 |
case StateModelTypes::StateMultibody_Talos: |
|
799 |
100 |
nu += 12; // it includes nc |
|
800 |
✓✗ | 100 |
contact = boost::make_shared<crocoddyl::ContactModelMultiple>(state, nu); |
801 |
✓✗ | 100 |
cost = boost::make_shared<crocoddyl::CostModelSum>(state, nu); |
802 |
✓✗✓✗ |
200 |
contact->addContact( |
803 |
✓✗✓✗ ✓✗✓✗ |
200 |
"lf", ContactModelFactory().create( |
804 |
ContactModelTypes::ContactModel6D_LOCAL, |
||
805 |
PinocchioModelTypes::Talos, gains, "left_sole_link", nu)); |
||
806 |
✓✗✓✗ |
200 |
contact->addContact( |
807 |
✓✗✓✗ ✓✗✓✗ |
200 |
"rf", ContactModelFactory().create( |
808 |
ContactModelTypes::ContactModel6D_WORLD, |
||
809 |
PinocchioModelTypes::Talos, gains, "right_sole_link", nu)); |
||
810 |
✓✓ | 100 |
if (with_friction) { |
811 |
// friction / wrench cone |
||
812 |
✓✗✓✗ |
100 |
cost->addCost( |
813 |
"lf_cone", |
||
814 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
815 |
state, friction_activation, |
||
816 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>( |
|
817 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("left_sole_link"), |
818 |
50 |
friction_cone, nu, false)), |
|
819 |
0.01); |
||
820 |
✓✗✓✗ |
100 |
cost->addCost( |
821 |
"rf_cone", |
||
822 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
823 |
state, wrench_activation, |
||
824 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactWrenchCone>( |
|
825 |
state, |
||
826 |
✓✗✓✗ ✓✗ |
100 |
state->get_pinocchio()->getFrameId("right_sole_link"), |
827 |
50 |
wrench_cone, nu, false)), |
|
828 |
0.01); |
||
829 |
// force regularization |
||
830 |
✓✗✓✗ |
100 |
cost->addCost( |
831 |
"lf_forceReg", |
||
832 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
833 |
state, |
||
834 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactForce>( |
|
835 |
✓✗✓✗ ✓✗ |
100 |
state, state->get_pinocchio()->getFrameId("left_sole_link"), |
836 |
50 |
force, 6, nu, false)), |
|
837 |
0.01); |
||
838 |
✓✗✓✗ |
100 |
cost->addCost( |
839 |
"rf_forceReg", |
||
840 |
✓✗ | 100 |
boost::make_shared<crocoddyl::CostModelResidual>( |
841 |
state, |
||
842 |
50 |
boost::make_shared<crocoddyl::ResidualModelContactForce>( |
|
843 |
state, |
||
844 |
✓✗✓✗ ✓✗ |
100 |
state->get_pinocchio()->getFrameId("right_sole_link"), |
845 |
100 |
force, 6, nu, false)), |
|
846 |
0.01); |
||
847 |
} |
||
848 |
100 |
break; |
|
849 |
default: |
||
850 |
throw_pretty(__FILE__ ": Wrong StateModelTypes::Type given"); |
||
851 |
break; |
||
852 |
} |
||
853 |
✓✗✓✗ |
600 |
cost->addCost("state", |
854 |
✓✗✓✗ |
600 |
CostModelFactory().create( |
855 |
CostModelTypes::CostModelResidualState, state_type, |
||
856 |
ActivationModelTypes::ActivationModelQuad, nu), |
||
857 |
0.1); |
||
858 |
✓✗✓✗ |
600 |
cost->addCost("control", |
859 |
✓✗✓✗ |
600 |
CostModelFactory().create( |
860 |
CostModelTypes::CostModelResidualControl, state_type, |
||
861 |
ActivationModelTypes::ActivationModelQuad, nu), |
||
862 |
0.1); |
||
863 |
action = |
||
864 |
✓✗ | 600 |
boost::make_shared<crocoddyl::DifferentialActionModelContactInvDynamics>( |
865 |
300 |
state, actuation, contact, cost); |
|
866 |
600 |
return action; |
|
867 |
} |
||
868 |
|||
869 |
} // namespace unittest |
||
870 |
} // namespace crocoddyl |
| Generated by: GCOVR (Version 4.2) |