crocoddyl  1.8.0
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  const std::vector<crocoddyl::FramePlacement> emptyVector;
68  std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > doubleSupport(
69  supportknots, createSwingFootModel(timestep, support_feet, nullCoM, emptyVector));
70 
71  const pinocchio::FrameIndex rh_s[] = {lf_foot_id_, rf_foot_id_, lh_foot_id_};
72  const pinocchio::FrameIndex rf_s[] = {lf_foot_id_, lh_foot_id_, rh_foot_id_};
73  const pinocchio::FrameIndex lh_s[] = {lf_foot_id_, rf_foot_id_, rh_foot_id_};
74  const pinocchio::FrameIndex lf_s[] = {rf_foot_id_, lh_foot_id_, rh_foot_id_};
75 
76  std::vector<pinocchio::FrameIndex> rh_support(rh_s, rh_s + sizeof(rh_s) / sizeof(rh_s[0]));
77  std::vector<pinocchio::FrameIndex> rf_support(rf_s, rf_s + sizeof(rf_s) / sizeof(rf_s[0]));
78  std::vector<pinocchio::FrameIndex> lh_support(lh_s, lh_s + sizeof(lh_s) / sizeof(lh_s[0]));
79  std::vector<pinocchio::FrameIndex> lf_support(lf_s, lf_s + sizeof(lf_s) / sizeof(lf_s[0]));
80 
81  std::vector<pinocchio::FrameIndex> rh_foot(1, rh_foot_id_);
82  std::vector<pinocchio::FrameIndex> rf_foot(1, rf_foot_id_);
83  std::vector<pinocchio::FrameIndex> lf_foot(1, lf_foot_id_);
84  std::vector<pinocchio::FrameIndex> lh_foot(1, lh_foot_id_);
85 
86  std::vector<Eigen::Vector3d> rh_foot_pos0_v(1, rh_foot_pos0);
87  std::vector<Eigen::Vector3d> lh_foot_pos0_v(1, lh_foot_pos0);
88  std::vector<Eigen::Vector3d> rf_foot_pos0_v(1, rf_foot_pos0);
89  std::vector<Eigen::Vector3d> lf_foot_pos0_v(1, lf_foot_pos0);
90  if (firtstep_) {
91  rh_step = createFootStepModels(timestep, comRef, rh_foot_pos0_v, 0.5 * steplength, stepheight, stepknots,
92  rh_support, rh_foot);
93  rf_step = createFootStepModels(timestep, comRef, rf_foot_pos0_v, 0.5 * steplength, stepheight, stepknots,
94  rf_support, rf_foot);
95  firtstep_ = false;
96  } else {
97  rh_step =
98  createFootStepModels(timestep, comRef, rh_foot_pos0_v, steplength, stepheight, stepknots, rh_support, rh_foot);
99  rf_step =
100  createFootStepModels(timestep, comRef, rf_foot_pos0_v, steplength, stepheight, stepknots, rf_support, rf_foot);
101  }
102  lh_step =
103  createFootStepModels(timestep, comRef, lh_foot_pos0_v, steplength, stepheight, stepknots, lh_support, lh_foot);
104  lf_step =
105  createFootStepModels(timestep, comRef, lf_foot_pos0_v, steplength, stepheight, stepknots, lf_support, lf_foot);
106 
107  loco3d_model.insert(loco3d_model.end(), doubleSupport.begin(), doubleSupport.end());
108  loco3d_model.insert(loco3d_model.end(), rh_step.begin(), rh_step.end());
109  loco3d_model.insert(loco3d_model.end(), rf_step.begin(), rf_step.end());
110  loco3d_model.insert(loco3d_model.end(), doubleSupport.begin(), doubleSupport.end());
111  loco3d_model.insert(loco3d_model.end(), lh_step.begin(), lh_step.end());
112  loco3d_model.insert(loco3d_model.end(), lf_step.begin(), lf_step.end());
113 
114  return boost::make_shared<crocoddyl::ShootingProblem>(x0, loco3d_model, loco3d_model.back());
115 }
116 
117 std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > SimpleQuadrupedGaitProblem::createFootStepModels(
118  double timestep, Eigen::Vector3d& com_pos0, std::vector<Eigen::Vector3d>& feet_pos0, double steplength,
119  double stepheight, std::size_t n_knots, const std::vector<pinocchio::FrameIndex>& support_foot_ids,
120  const std::vector<pinocchio::FrameIndex>& swingFootIds) {
121  std::size_t n_legs = static_cast<std::size_t>(support_foot_ids.size() + swingFootIds.size());
122  double com_percentage = static_cast<double>(swingFootIds.size()) / static_cast<double>(n_legs);
123 
124  // Action models for the foot swing
125  std::vector<boost::shared_ptr<ActionModelAbstract> > foot_swing_model;
126  std::vector<crocoddyl::FramePlacement> 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  foot_swing_task.clear();
131  for (std::size_t i = 0; i < swingFootIds.size(); ++i) {
132  // Defining a foot swing task given the step length resKnot = n_knots % 2
133  std::size_t phaseknots = n_knots >> 1; // bitwise divide.
134  _kp1_n = static_cast<double>(k + 1) / static_cast<double>(n_knots);
135  double _k = static_cast<double>(k);
136  double _phaseknots = static_cast<double>(phaseknots);
137  if (k < phaseknots)
138  dp << steplength * _kp1_n, 0., stepheight * _k / _phaseknots;
139  else if (k == phaseknots)
140  dp << steplength * _kp1_n, 0., stepheight;
141  else
142  dp << steplength * _kp1_n, 0., stepheight * (1 - (_k - _phaseknots) / _phaseknots);
143  Eigen::Vector3d tref = feet_pos0[i] + dp;
144 
145  foot_swing_task.push_back(
146  crocoddyl::FramePlacement(swingFootIds[i], pinocchio::SE3(Eigen::Matrix3d::Identity(), tref)));
147  }
148 
149  // Action model for the foot switch
150  Eigen::Vector3d com_task = Eigen::Vector3d(steplength * _kp1_n, 0., 0.) * com_percentage + com_pos0;
151  foot_swing_model.push_back(createSwingFootModel(timestep, support_foot_ids, com_task, foot_swing_task));
152  }
153  // Action model for the foot switch
154  foot_swing_model.push_back(createFootSwitchModel(support_foot_ids, foot_swing_task));
155 
156  // Updating the current foot position for next step
157  com_pos0 += Eigen::Vector3d(steplength * com_percentage, 0., 0.);
158  for (std::size_t i = 0; i < feet_pos0.size(); ++i) {
159  feet_pos0[i] += Eigen::Vector3d(steplength, 0., 0.);
160  }
161  return foot_swing_model;
162 }
163 
164 boost::shared_ptr<crocoddyl::ActionModelAbstract> SimpleQuadrupedGaitProblem::createSwingFootModel(
165  double timestep, const std::vector<pinocchio::FrameIndex>& support_foot_ids, const Eigen::Vector3d& com_task,
166  const std::vector<crocoddyl::FramePlacement>& foot_swing_task) {
167  // Creating a 3D multi-contact model, and then including the supporting foot
168  boost::shared_ptr<crocoddyl::ContactModelMultiple> contact_model =
169  boost::make_shared<crocoddyl::ContactModelMultiple>(state_, actuation_->get_nu());
170  for (std::vector<pinocchio::FrameIndex>::const_iterator it = support_foot_ids.begin(); it != support_foot_ids.end();
171  ++it) {
172  crocoddyl::FrameTranslation xref(*it, Eigen::Vector3d::Zero());
173  boost::shared_ptr<crocoddyl::ContactModelAbstract> support_contact_model =
174  boost::make_shared<crocoddyl::ContactModel3D>(state_, xref, actuation_->get_nu(), Eigen::Vector2d(0., 50.));
175  contact_model->addContact(rmodel_.frames[*it].name + "_contact", support_contact_model);
176  }
177 
178  // Creating the cost model for a contact phase
179  boost::shared_ptr<crocoddyl::CostModelSum> cost_model =
180  boost::make_shared<crocoddyl::CostModelSum>(state_, actuation_->get_nu());
181  if (com_task.array().allFinite()) {
182  boost::shared_ptr<crocoddyl::CostModelAbstract> com_track = boost::make_shared<crocoddyl::CostModelResidual>(
183  state_, boost::make_shared<crocoddyl::ResidualModelCoMPosition>(state_, com_task, actuation_->get_nu()));
184  cost_model->addCost("comTrack", com_track, 1e6);
185  }
186  if (!foot_swing_task.empty()) {
187  for (std::vector<crocoddyl::FramePlacement>::const_iterator it = foot_swing_task.begin();
188  it != foot_swing_task.end(); ++it) {
189  boost::shared_ptr<crocoddyl::CostModelAbstract> foot_track = boost::make_shared<crocoddyl::CostModelResidual>(
190  state_, boost::make_shared<crocoddyl::ResidualModelFrameTranslation>(
191  state_, it->id, it->placement.translation(), actuation_->get_nu()));
192  cost_model->addCost(rmodel_.frames[it->id].name + "_footTrack", foot_track, 1e6);
193  }
194  }
195  Eigen::VectorXd state_weights(2 * rmodel_.nv);
196  state_weights.head<3>().fill(0.);
197  state_weights.segment<3>(3).fill(pow(500., 2));
198  state_weights.segment(6, rmodel_.nv - 6).fill(pow(0.01, 2));
199  state_weights.segment(rmodel_.nv, 6).fill(pow(10., 2));
200  state_weights.segment(rmodel_.nv + 6, rmodel_.nv - 6).fill(pow(1., 2));
201  boost::shared_ptr<crocoddyl::ActivationModelAbstract> state_activation =
202  boost::make_shared<crocoddyl::ActivationModelWeightedQuad>(state_weights);
203  boost::shared_ptr<crocoddyl::CostModelAbstract> state_reg = boost::make_shared<crocoddyl::CostModelResidual>(
204  state_, state_activation,
205  boost::make_shared<crocoddyl::ResidualModelState>(state_, defaultstate_, actuation_->get_nu()));
206  boost::shared_ptr<crocoddyl::CostModelAbstract> ctrl_reg = boost::make_shared<crocoddyl::CostModelResidual>(
207  state_, boost::make_shared<crocoddyl::ResidualModelControl>(state_, actuation_->get_nu()));
208  cost_model->addCost("stateReg", state_reg, 1e1);
209  cost_model->addCost("ctrlReg", ctrl_reg, 1e-1);
210 
211  // Creating the action model for the KKT dynamics with simpletic Euler integration scheme
212  boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dmodel =
213  boost::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(state_, actuation_, contact_model,
214  cost_model);
215  return boost::make_shared<crocoddyl::IntegratedActionModelEuler>(dmodel, timestep);
216 }
217 
218 boost::shared_ptr<ActionModelAbstract> SimpleQuadrupedGaitProblem::createFootSwitchModel(
219  const std::vector<pinocchio::FrameIndex>& support_foot_ids,
220  const std::vector<crocoddyl::FramePlacement>& foot_swing_task, bool pseudo_impulse) {
221  if (pseudo_impulse) {
222  return createPseudoImpulseModel(support_foot_ids, foot_swing_task);
223  } else {
224  return createImpulseModel(support_foot_ids, foot_swing_task);
225  }
226 }
227 
228 boost::shared_ptr<crocoddyl::ActionModelAbstract> SimpleQuadrupedGaitProblem::createPseudoImpulseModel(
229  const std::vector<pinocchio::FrameIndex>& support_foot_ids,
230  const std::vector<crocoddyl::FramePlacement>& foot_swing_task) {
231  // Creating a 3D multi-contact model, and then including the supporting foot
232  boost::shared_ptr<crocoddyl::ContactModelMultiple> contact_model =
233  boost::make_shared<crocoddyl::ContactModelMultiple>(state_, actuation_->get_nu());
234  for (std::vector<pinocchio::FrameIndex>::const_iterator it = support_foot_ids.begin(); it != support_foot_ids.end();
235  ++it) {
236  crocoddyl::FrameTranslation xref(*it, Eigen::Vector3d::Zero());
237  boost::shared_ptr<crocoddyl::ContactModelAbstract> support_contact_model =
238  boost::make_shared<crocoddyl::ContactModel3D>(state_, xref, actuation_->get_nu(), Eigen::Vector2d(0., 50.));
239  contact_model->addContact(rmodel_.frames[*it].name + "_contact", support_contact_model);
240  }
241 
242  // Creating the cost model for a contact phase
243  boost::shared_ptr<crocoddyl::CostModelSum> cost_model =
244  boost::make_shared<crocoddyl::CostModelSum>(state_, actuation_->get_nu());
245  if (!foot_swing_task.empty()) {
246  for (std::vector<crocoddyl::FramePlacement>::const_iterator it = foot_swing_task.begin();
247  it != foot_swing_task.end(); ++it) {
248  boost::shared_ptr<crocoddyl::CostModelAbstract> foot_track = boost::make_shared<crocoddyl::CostModelResidual>(
249  state_, boost::make_shared<crocoddyl::ResidualModelFrameTranslation>(
250  state_, it->id, it->placement.translation(), actuation_->get_nu()));
251  boost::shared_ptr<crocoddyl::CostModelAbstract> impulse_foot_vel =
252  boost::make_shared<crocoddyl::CostModelResidual>(
253  state_,
254  boost::make_shared<crocoddyl::ResidualModelFrameVelocity>(
255  state_, it->id, pinocchio::Motion::Zero(), pinocchio::ReferenceFrame::LOCAL, actuation_->get_nu()));
256  cost_model->addCost(rmodel_.frames[it->id].name + "_footTrack", foot_track, 1e7);
257  cost_model->addCost(rmodel_.frames[it->id].name + "_impulseVel", impulse_foot_vel, 1e6);
258  }
259  }
260  Eigen::VectorXd state_weights(2 * rmodel_.nv);
261  state_weights.head<3>().fill(0.);
262  state_weights.segment<3>(3).fill(pow(500., 2));
263  state_weights.segment(6, rmodel_.nv - 6).fill(pow(0.01, 2));
264  state_weights.segment(rmodel_.nv, rmodel_.nv).fill(pow(10., 2));
265  boost::shared_ptr<crocoddyl::ActivationModelAbstract> state_activation =
266  boost::make_shared<crocoddyl::ActivationModelWeightedQuad>(state_weights);
267  boost::shared_ptr<crocoddyl::CostModelAbstract> state_reg = boost::make_shared<crocoddyl::CostModelResidual>(
268  state_, state_activation,
269  boost::make_shared<crocoddyl::ResidualModelState>(state_, defaultstate_, actuation_->get_nu()));
270  boost::shared_ptr<crocoddyl::CostModelAbstract> ctrl_reg = boost::make_shared<crocoddyl::CostModelResidual>(
271  state_, boost::make_shared<crocoddyl::ResidualModelControl>(state_, actuation_->get_nu()));
272  cost_model->addCost("stateReg", state_reg, 1e1);
273  cost_model->addCost("ctrlReg", ctrl_reg, 1e-3);
274 
275  // Creating the action model for the KKT dynamics with simpletic Euler integration scheme
276  boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dmodel =
277  boost::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(state_, actuation_, contact_model,
278  cost_model);
279  return boost::make_shared<crocoddyl::IntegratedActionModelEuler>(dmodel, 0.);
280 }
281 
282 boost::shared_ptr<ActionModelAbstract> SimpleQuadrupedGaitProblem::createImpulseModel(
283  const std::vector<pinocchio::FrameIndex>& support_foot_ids, const std::vector<FramePlacement>& foot_swing_task) {
284  // Creating a 3D multi-contact model, and then including the supporting foot
285  boost::shared_ptr<crocoddyl::ImpulseModelMultiple> impulse_model =
286  boost::make_shared<crocoddyl::ImpulseModelMultiple>(state_);
287  for (std::vector<pinocchio::FrameIndex>::const_iterator it = support_foot_ids.begin(); it != support_foot_ids.end();
288  ++it) {
289  boost::shared_ptr<crocoddyl::ImpulseModelAbstract> support_contact_model =
290  boost::make_shared<crocoddyl::ImpulseModel3D>(state_, *it);
291  impulse_model->addImpulse(rmodel_.frames[*it].name + "_impulse", support_contact_model);
292  }
293 
294  // Creating the cost model for a contact phase
295  boost::shared_ptr<crocoddyl::CostModelSum> cost_model = boost::make_shared<crocoddyl::CostModelSum>(state_, 0);
296  if (!foot_swing_task.empty()) {
297  for (std::vector<crocoddyl::FramePlacement>::const_iterator it = foot_swing_task.begin();
298  it != foot_swing_task.end(); ++it) {
299  boost::shared_ptr<crocoddyl::CostModelAbstract> foot_track = boost::make_shared<crocoddyl::CostModelResidual>(
300  state_, boost::make_shared<crocoddyl::ResidualModelFrameTranslation>(state_, it->id,
301  it->placement.translation(), 0));
302  cost_model->addCost(rmodel_.frames[it->id].name + "_footTrack", foot_track, 1e7);
303  }
304  }
305  Eigen::VectorXd state_weights(2 * rmodel_.nv);
306  state_weights.head<6>().fill(1.);
307  state_weights.segment(6, rmodel_.nv - 6).fill(pow(10., 2));
308  state_weights.segment(rmodel_.nv, rmodel_.nv).fill(pow(10., 2));
309  boost::shared_ptr<crocoddyl::ActivationModelAbstract> state_activation =
310  boost::make_shared<crocoddyl::ActivationModelWeightedQuad>(state_weights);
311  boost::shared_ptr<crocoddyl::CostModelAbstract> state_reg = boost::make_shared<crocoddyl::CostModelResidual>(
312  state_, state_activation, boost::make_shared<crocoddyl::ResidualModelState>(state_, defaultstate_, 0));
313  cost_model->addCost("stateReg", state_reg, 1e1);
314 
315  // Creating the action model for the KKT dynamics with simpletic Euler integration scheme
316  return boost::make_shared<crocoddyl::ActionModelImpulseFwdDynamics>(state_, impulse_model, cost_model);
317 }
318 
319 const Eigen::VectorXd& SimpleQuadrupedGaitProblem::get_defaultState() const { return defaultstate_; }
320 
321 } // namespace crocoddyl