GCC Code Coverage Report


Directory: ./
File: src/multibody/utils/quadruped-gaits.cpp
Date: 2025-02-24 23:41:29
Exec Total Coverage
Lines: 0 241 0.0%
Branches: 0 430 0.0%

Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include "crocoddyl/multibody/utils/quadruped-gaits.hpp"
10
11 #include "crocoddyl/core/costs/residual.hpp"
12
13 namespace crocoddyl {
14
15 SimpleQuadrupedGaitProblem::SimpleQuadrupedGaitProblem(
16 const pinocchio::Model& rmodel, const std::string& lf_foot,
17 const std::string& rf_foot, const std::string& lh_foot,
18 const std::string& rh_foot)
19 : rmodel_(rmodel),
20 rdata_(rmodel_),
21 lf_foot_id_(rmodel_.getFrameId(
22 lf_foot,
23 (pinocchio::FrameType)(pinocchio::JOINT | pinocchio::FIXED_JOINT |
24 pinocchio::BODY))),
25 rf_foot_id_(rmodel_.getFrameId(
26 rf_foot,
27 (pinocchio::FrameType)(pinocchio::JOINT | pinocchio::FIXED_JOINT |
28 pinocchio::BODY))),
29 lh_foot_id_(rmodel_.getFrameId(
30 lh_foot,
31 (pinocchio::FrameType)(pinocchio::JOINT | pinocchio::FIXED_JOINT |
32 pinocchio::BODY))),
33 rh_foot_id_(rmodel_.getFrameId(
34 rh_foot,
35 (pinocchio::FrameType)(pinocchio::JOINT | pinocchio::FIXED_JOINT |
36 pinocchio::BODY))),
37 state_(std::make_shared<crocoddyl::StateMultibody>(
38 std::make_shared<pinocchio::Model>(rmodel_))),
39 actuation_(
40 std::make_shared<crocoddyl::ActuationModelFloatingBase>(state_)),
41 firtstep_(true),
42 defaultstate_(rmodel_.nq + rmodel_.nv) {
43 defaultstate_.head(rmodel_.nq) = rmodel_.referenceConfigurations["standing"];
44 defaultstate_.tail(rmodel_.nv).setZero();
45 }
46
47 SimpleQuadrupedGaitProblem::~SimpleQuadrupedGaitProblem() {}
48
49 std::shared_ptr<crocoddyl::ShootingProblem>
50 SimpleQuadrupedGaitProblem::createWalkingProblem(
51 const Eigen::VectorXd& x0, const double steplength, const double stepheight,
52 const double timestep, const std::size_t stepknots,
53 const std::size_t supportknots) {
54 int nq = rmodel_.nq;
55
56 // Initial Condition
57 const Eigen::VectorBlock<const Eigen::VectorXd> q0 = x0.head(nq);
58 pinocchio::forwardKinematics(rmodel_, rdata_, q0);
59 pinocchio::centerOfMass(rmodel_, rdata_, q0);
60 pinocchio::updateFramePlacements(rmodel_, rdata_);
61
62 const pinocchio::SE3::Vector3& rf_foot_pos0 =
63 rdata_.oMf[rf_foot_id_].translation();
64 const pinocchio::SE3::Vector3& rh_foot_pos0 =
65 rdata_.oMf[rh_foot_id_].translation();
66 const pinocchio::SE3::Vector3& lf_foot_pos0 =
67 rdata_.oMf[lf_foot_id_].translation();
68 const pinocchio::SE3::Vector3& lh_foot_pos0 =
69 rdata_.oMf[lh_foot_id_].translation();
70
71 pinocchio::SE3::Vector3 comRef =
72 (rf_foot_pos0 + rh_foot_pos0 + lf_foot_pos0 + lh_foot_pos0) / 4;
73 comRef[2] = rdata_.com[0][2];
74
75 // Defining the action models along the time instances
76 std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract> > loco3d_model;
77 std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract> > rh_step,
78 rf_step, lh_step, lf_step;
79
80 // doublesupport
81 std::vector<pinocchio::FrameIndex> support_feet;
82 support_feet.push_back(lf_foot_id_);
83 support_feet.push_back(rf_foot_id_);
84 support_feet.push_back(lh_foot_id_);
85 support_feet.push_back(rh_foot_id_);
86 Eigen::Vector3d nullCoM =
87 Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity());
88 std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract> > doubleSupport(
89 supportknots, createSwingFootModel(timestep, support_feet, nullCoM));
90
91 const pinocchio::FrameIndex rh_s[] = {lf_foot_id_, rf_foot_id_, lh_foot_id_};
92 const pinocchio::FrameIndex rf_s[] = {lf_foot_id_, lh_foot_id_, rh_foot_id_};
93 const pinocchio::FrameIndex lh_s[] = {lf_foot_id_, rf_foot_id_, rh_foot_id_};
94 const pinocchio::FrameIndex lf_s[] = {rf_foot_id_, lh_foot_id_, rh_foot_id_};
95
96 std::vector<pinocchio::FrameIndex> rh_support(
97 rh_s, rh_s + sizeof(rh_s) / sizeof(rh_s[0]));
98 std::vector<pinocchio::FrameIndex> rf_support(
99 rf_s, rf_s + sizeof(rf_s) / sizeof(rf_s[0]));
100 std::vector<pinocchio::FrameIndex> lh_support(
101 lh_s, lh_s + sizeof(lh_s) / sizeof(lh_s[0]));
102 std::vector<pinocchio::FrameIndex> lf_support(
103 lf_s, lf_s + sizeof(lf_s) / sizeof(lf_s[0]));
104
105 std::vector<pinocchio::FrameIndex> rh_foot(1, rh_foot_id_);
106 std::vector<pinocchio::FrameIndex> rf_foot(1, rf_foot_id_);
107 std::vector<pinocchio::FrameIndex> lf_foot(1, lf_foot_id_);
108 std::vector<pinocchio::FrameIndex> lh_foot(1, lh_foot_id_);
109
110 std::vector<Eigen::Vector3d> rh_foot_pos0_v(1, rh_foot_pos0);
111 std::vector<Eigen::Vector3d> lh_foot_pos0_v(1, lh_foot_pos0);
112 std::vector<Eigen::Vector3d> rf_foot_pos0_v(1, rf_foot_pos0);
113 std::vector<Eigen::Vector3d> lf_foot_pos0_v(1, lf_foot_pos0);
114 if (firtstep_) {
115 rh_step =
116 createFootStepModels(timestep, comRef, rh_foot_pos0_v, 0.5 * steplength,
117 stepheight, stepknots, rh_support, rh_foot);
118 rf_step =
119 createFootStepModels(timestep, comRef, rf_foot_pos0_v, 0.5 * steplength,
120 stepheight, stepknots, rf_support, rf_foot);
121 firtstep_ = false;
122 } else {
123 rh_step = createFootStepModels(timestep, comRef, rh_foot_pos0_v, steplength,
124 stepheight, stepknots, rh_support, rh_foot);
125 rf_step = createFootStepModels(timestep, comRef, rf_foot_pos0_v, steplength,
126 stepheight, stepknots, rf_support, rf_foot);
127 }
128 lh_step = createFootStepModels(timestep, comRef, lh_foot_pos0_v, steplength,
129 stepheight, stepknots, lh_support, lh_foot);
130 lf_step = createFootStepModels(timestep, comRef, lf_foot_pos0_v, steplength,
131 stepheight, stepknots, lf_support, lf_foot);
132
133 loco3d_model.insert(loco3d_model.end(), doubleSupport.begin(),
134 doubleSupport.end());
135 loco3d_model.insert(loco3d_model.end(), rh_step.begin(), rh_step.end());
136 loco3d_model.insert(loco3d_model.end(), rf_step.begin(), rf_step.end());
137 loco3d_model.insert(loco3d_model.end(), doubleSupport.begin(),
138 doubleSupport.end());
139 loco3d_model.insert(loco3d_model.end(), lh_step.begin(), lh_step.end());
140 loco3d_model.insert(loco3d_model.end(), lf_step.begin(), lf_step.end());
141
142 return std::make_shared<crocoddyl::ShootingProblem>(x0, loco3d_model,
143 loco3d_model.back());
144 }
145
146 std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract> >
147 SimpleQuadrupedGaitProblem::createFootStepModels(
148 double timestep, Eigen::Vector3d& com_pos0,
149 std::vector<Eigen::Vector3d>& feet_pos0, double steplength,
150 double stepheight, std::size_t n_knots,
151 const std::vector<pinocchio::FrameIndex>& support_foot_ids,
152 const std::vector<pinocchio::FrameIndex>& swingFootIds) {
153 std::size_t n_legs =
154 static_cast<std::size_t>(support_foot_ids.size() + swingFootIds.size());
155 double com_percentage =
156 static_cast<double>(swingFootIds.size()) / static_cast<double>(n_legs);
157
158 // Action models for the foot swing
159 std::vector<std::shared_ptr<ActionModelAbstract> > foot_swing_model;
160 std::vector<pinocchio::FrameIndex> id_foot_swing_task;
161 std::vector<pinocchio::SE3> ref_foot_swing_task;
162 for (std::size_t k = 0; k < n_knots; ++k) {
163 double _kp1_n = 0;
164 Eigen::Vector3d dp = Eigen::Vector3d::Zero();
165 id_foot_swing_task.clear();
166 ref_foot_swing_task.clear();
167 for (std::size_t i = 0; i < swingFootIds.size(); ++i) {
168 // Defining a foot swing task given the step length resKnot = n_knots % 2
169 std::size_t phaseknots = n_knots >> 1; // bitwise divide.
170 _kp1_n = static_cast<double>(k + 1) / static_cast<double>(n_knots);
171 double _k = static_cast<double>(k);
172 double _phaseknots = static_cast<double>(phaseknots);
173 if (k < phaseknots)
174 dp << steplength * _kp1_n, 0., stepheight * _k / _phaseknots;
175 else if (k == phaseknots)
176 dp << steplength * _kp1_n, 0., stepheight;
177 else
178 dp << steplength * _kp1_n, 0.,
179 stepheight * (1 - (_k - _phaseknots) / _phaseknots);
180 Eigen::Vector3d tref = feet_pos0[i] + dp;
181
182 id_foot_swing_task.push_back(swingFootIds[i]);
183 ref_foot_swing_task.push_back(
184 pinocchio::SE3(Eigen::Matrix3d::Identity(), tref));
185 }
186
187 // Action model for the foot switch
188 Eigen::Vector3d com_task =
189 Eigen::Vector3d(steplength * _kp1_n, 0., 0.) * com_percentage +
190 com_pos0;
191 foot_swing_model.push_back(
192 createSwingFootModel(timestep, support_foot_ids, com_task,
193 id_foot_swing_task, ref_foot_swing_task));
194 }
195 // Action model for the foot switch
196 foot_swing_model.push_back(createFootSwitchModel(
197 support_foot_ids, id_foot_swing_task, ref_foot_swing_task));
198
199 // Updating the current foot position for next step
200 com_pos0 += Eigen::Vector3d(steplength * com_percentage, 0., 0.);
201 for (std::size_t i = 0; i < feet_pos0.size(); ++i) {
202 feet_pos0[i] += Eigen::Vector3d(steplength, 0., 0.);
203 }
204 return foot_swing_model;
205 }
206
207 std::shared_ptr<crocoddyl::ActionModelAbstract>
208 SimpleQuadrupedGaitProblem::createSwingFootModel(
209 double timestep, const std::vector<pinocchio::FrameIndex>& support_foot_ids,
210 const Eigen::Vector3d& com_task,
211 const std::vector<pinocchio::FrameIndex>& id_foot_swing_task,
212 const std::vector<pinocchio::SE3>& ref_foot_swing_task) {
213 // Creating a 3D multi-contact model, and then including the supporting foot
214 std::shared_ptr<crocoddyl::ContactModelMultiple> contact_model =
215 std::make_shared<crocoddyl::ContactModelMultiple>(state_,
216 actuation_->get_nu());
217 for (std::vector<pinocchio::FrameIndex>::const_iterator it =
218 support_foot_ids.begin();
219 it != support_foot_ids.end(); ++it) {
220 std::shared_ptr<crocoddyl::ContactModelAbstract> support_contact_model =
221 std::make_shared<crocoddyl::ContactModel3D>(
222 state_, *it, Eigen::Vector3d::Zero(),
223 pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
224 actuation_->get_nu(), Eigen::Vector2d(0., 50.));
225 contact_model->addContact(rmodel_.frames[*it].name + "_contact",
226 support_contact_model);
227 }
228
229 // Creating the cost model for a contact phase
230 std::shared_ptr<crocoddyl::CostModelSum> cost_model =
231 std::make_shared<crocoddyl::CostModelSum>(state_, actuation_->get_nu());
232 if (com_task.array().allFinite()) {
233 std::shared_ptr<crocoddyl::CostModelAbstract> com_track =
234 std::make_shared<crocoddyl::CostModelResidual>(
235 state_, std::make_shared<crocoddyl::ResidualModelCoMPosition>(
236 state_, com_task, actuation_->get_nu()));
237 cost_model->addCost("comTrack", com_track, 1e6);
238 }
239 if (!id_foot_swing_task.empty() && !ref_foot_swing_task.empty()) {
240 for (std::size_t i = 0; i < id_foot_swing_task.size(); ++i) {
241 const pinocchio::FrameIndex id = id_foot_swing_task[i];
242 std::shared_ptr<crocoddyl::CostModelAbstract> foot_track =
243 std::make_shared<crocoddyl::CostModelResidual>(
244 state_,
245 std::make_shared<crocoddyl::ResidualModelFrameTranslation>(
246 state_, id, ref_foot_swing_task[i].translation(),
247 actuation_->get_nu()));
248 cost_model->addCost(rmodel_.frames[id].name + "_footTrack", foot_track,
249 1e6);
250 }
251 }
252 Eigen::VectorXd state_weights(2 * rmodel_.nv);
253 state_weights.head<3>().fill(0.);
254 state_weights.segment<3>(3).fill(pow(500., 2));
255 state_weights.segment(6, rmodel_.nv - 6).fill(pow(0.01, 2));
256 state_weights.segment(rmodel_.nv, 6).fill(pow(10., 2));
257 state_weights.segment(rmodel_.nv + 6, rmodel_.nv - 6).fill(pow(1., 2));
258 std::shared_ptr<crocoddyl::ActivationModelAbstract> state_activation =
259 std::make_shared<crocoddyl::ActivationModelWeightedQuad>(state_weights);
260 std::shared_ptr<crocoddyl::CostModelAbstract> state_reg =
261 std::make_shared<crocoddyl::CostModelResidual>(
262 state_, state_activation,
263 std::make_shared<crocoddyl::ResidualModelState>(
264 state_, defaultstate_, actuation_->get_nu()));
265 std::shared_ptr<crocoddyl::CostModelAbstract> ctrl_reg =
266 std::make_shared<crocoddyl::CostModelResidual>(
267 state_, std::make_shared<crocoddyl::ResidualModelControl>(
268 state_, actuation_->get_nu()));
269 cost_model->addCost("stateReg", state_reg, 1e1);
270 cost_model->addCost("ctrlReg", ctrl_reg, 1e-1);
271
272 // Creating the action model for the KKT dynamics with simpletic Euler
273 // integration scheme
274 std::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dmodel =
275 std::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(
276 state_, actuation_, contact_model, cost_model);
277 return std::make_shared<crocoddyl::IntegratedActionModelEuler>(dmodel,
278 timestep);
279 }
280
281 std::shared_ptr<ActionModelAbstract>
282 SimpleQuadrupedGaitProblem::createFootSwitchModel(
283 const std::vector<pinocchio::FrameIndex>& support_foot_ids,
284 const std::vector<pinocchio::FrameIndex>& id_foot_swing_task,
285 const std::vector<pinocchio::SE3>& ref_foot_swing_task,
286 bool pseudo_impulse) {
287 if (pseudo_impulse) {
288 return createPseudoImpulseModel(support_foot_ids, id_foot_swing_task,
289 ref_foot_swing_task);
290 } else {
291 return createImpulseModel(support_foot_ids, id_foot_swing_task,
292 ref_foot_swing_task);
293 }
294 }
295
296 std::shared_ptr<crocoddyl::ActionModelAbstract>
297 SimpleQuadrupedGaitProblem::createPseudoImpulseModel(
298 const std::vector<pinocchio::FrameIndex>& support_foot_ids,
299 const std::vector<pinocchio::FrameIndex>& id_foot_swing_task,
300 const std::vector<pinocchio::SE3>& ref_foot_swing_task) {
301 // Creating a 3D multi-contact model, and then including the supporting foot
302 std::shared_ptr<crocoddyl::ContactModelMultiple> contact_model =
303 std::make_shared<crocoddyl::ContactModelMultiple>(state_,
304 actuation_->get_nu());
305 for (std::vector<pinocchio::FrameIndex>::const_iterator it =
306 support_foot_ids.begin();
307 it != support_foot_ids.end(); ++it) {
308 std::shared_ptr<crocoddyl::ContactModelAbstract> support_contact_model =
309 std::make_shared<crocoddyl::ContactModel3D>(
310 state_, *it, Eigen::Vector3d::Zero(),
311 pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
312 actuation_->get_nu(), Eigen::Vector2d(0., 50.));
313 contact_model->addContact(rmodel_.frames[*it].name + "_contact",
314 support_contact_model);
315 }
316
317 // Creating the cost model for a contact phase
318 std::shared_ptr<crocoddyl::CostModelSum> cost_model =
319 std::make_shared<crocoddyl::CostModelSum>(state_, actuation_->get_nu());
320 if (!id_foot_swing_task.empty() && !ref_foot_swing_task.empty()) {
321 for (std::size_t i = 0; i < id_foot_swing_task.size(); ++i) {
322 const pinocchio::FrameIndex id = id_foot_swing_task[i];
323 std::shared_ptr<crocoddyl::CostModelAbstract> foot_track =
324 std::make_shared<crocoddyl::CostModelResidual>(
325 state_,
326 std::make_shared<crocoddyl::ResidualModelFrameTranslation>(
327 state_, id, ref_foot_swing_task[i].translation(),
328 actuation_->get_nu()));
329 std::shared_ptr<crocoddyl::CostModelAbstract> impulse_foot_vel =
330 std::make_shared<crocoddyl::CostModelResidual>(
331 state_,
332 std::make_shared<crocoddyl::ResidualModelFrameVelocity>(
333 state_, id, pinocchio::Motion::Zero(),
334 pinocchio::ReferenceFrame::LOCAL, actuation_->get_nu()));
335 cost_model->addCost(rmodel_.frames[id].name + "_footTrack", foot_track,
336 1e7);
337 cost_model->addCost(rmodel_.frames[id].name + "_impulseVel",
338 impulse_foot_vel, 1e6);
339 }
340 }
341 Eigen::VectorXd state_weights(2 * rmodel_.nv);
342 state_weights.head<3>().fill(0.);
343 state_weights.segment<3>(3).fill(pow(500., 2));
344 state_weights.segment(6, rmodel_.nv - 6).fill(pow(0.01, 2));
345 state_weights.segment(rmodel_.nv, rmodel_.nv).fill(pow(10., 2));
346 std::shared_ptr<crocoddyl::ActivationModelAbstract> state_activation =
347 std::make_shared<crocoddyl::ActivationModelWeightedQuad>(state_weights);
348 std::shared_ptr<crocoddyl::CostModelAbstract> state_reg =
349 std::make_shared<crocoddyl::CostModelResidual>(
350 state_, state_activation,
351 std::make_shared<crocoddyl::ResidualModelState>(
352 state_, defaultstate_, actuation_->get_nu()));
353 std::shared_ptr<crocoddyl::CostModelAbstract> ctrl_reg =
354 std::make_shared<crocoddyl::CostModelResidual>(
355 state_, std::make_shared<crocoddyl::ResidualModelControl>(
356 state_, actuation_->get_nu()));
357 cost_model->addCost("stateReg", state_reg, 1e1);
358 cost_model->addCost("ctrlReg", ctrl_reg, 1e-3);
359
360 // Creating the action model for the KKT dynamics with simpletic Euler
361 // integration scheme
362 std::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dmodel =
363 std::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(
364 state_, actuation_, contact_model, cost_model);
365 return std::make_shared<crocoddyl::IntegratedActionModelEuler>(dmodel, 0.);
366 }
367
368 std::shared_ptr<ActionModelAbstract>
369 SimpleQuadrupedGaitProblem::createImpulseModel(
370 const std::vector<pinocchio::FrameIndex>& support_foot_ids,
371 const std::vector<pinocchio::FrameIndex>& id_foot_swing_task,
372 const std::vector<pinocchio::SE3>& ref_foot_swing_task) {
373 // Creating a 3D multi-contact model, and then including the supporting foot
374 std::shared_ptr<crocoddyl::ImpulseModelMultiple> impulse_model =
375 std::make_shared<crocoddyl::ImpulseModelMultiple>(state_);
376 for (std::vector<pinocchio::FrameIndex>::const_iterator it =
377 support_foot_ids.begin();
378 it != support_foot_ids.end(); ++it) {
379 std::shared_ptr<crocoddyl::ImpulseModelAbstract> support_contact_model =
380 std::make_shared<crocoddyl::ImpulseModel3D>(
381 state_, *it, pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED);
382 impulse_model->addImpulse(rmodel_.frames[*it].name + "_impulse",
383 support_contact_model);
384 }
385
386 // Creating the cost model for a contact phase
387 std::shared_ptr<crocoddyl::CostModelSum> cost_model =
388 std::make_shared<crocoddyl::CostModelSum>(state_, 0);
389 if (!id_foot_swing_task.empty() && !ref_foot_swing_task.empty()) {
390 for (std::size_t i = 0; i < id_foot_swing_task.size(); ++i) {
391 const pinocchio::FrameIndex id = id_foot_swing_task[i];
392 std::shared_ptr<crocoddyl::CostModelAbstract> foot_track =
393 std::make_shared<crocoddyl::CostModelResidual>(
394 state_,
395 std::make_shared<crocoddyl::ResidualModelFrameTranslation>(
396 state_, id, ref_foot_swing_task[i].translation(), 0));
397 cost_model->addCost(rmodel_.frames[id].name + "_footTrack", foot_track,
398 1e7);
399 }
400 }
401 Eigen::VectorXd state_weights(2 * rmodel_.nv);
402 state_weights.head<6>().fill(1.);
403 state_weights.segment(6, rmodel_.nv - 6).fill(pow(10., 2));
404 state_weights.segment(rmodel_.nv, rmodel_.nv).fill(pow(10., 2));
405 std::shared_ptr<crocoddyl::ActivationModelAbstract> state_activation =
406 std::make_shared<crocoddyl::ActivationModelWeightedQuad>(state_weights);
407 std::shared_ptr<crocoddyl::CostModelAbstract> state_reg =
408 std::make_shared<crocoddyl::CostModelResidual>(
409 state_, state_activation,
410 std::make_shared<crocoddyl::ResidualModelState>(state_, defaultstate_,
411 0));
412 cost_model->addCost("stateReg", state_reg, 1e1);
413
414 // Creating the action model for the KKT dynamics with simpletic Euler
415 // integration scheme
416 return std::make_shared<crocoddyl::ActionModelImpulseFwdDynamics>(
417 state_, impulse_model, cost_model);
418 }
419
420 const Eigen::VectorXd& SimpleQuadrupedGaitProblem::get_defaultState() const {
421 return defaultstate_;
422 }
423
424 } // namespace crocoddyl
425