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