crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
quadruped-gaits.cpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019, LAAS-CNRS
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include "crocoddyl/multibody/utils/quadruped-gaits.hpp"
10 
11 namespace crocoddyl {
12 
13 SimpleQuadrupedGaitProblem::SimpleQuadrupedGaitProblem(const pinocchio::Model& rmodel, const std::string& lf_foot,
14  const std::string& rf_foot, const std::string& lh_foot,
15  const std::string& rh_foot)
16  : rmodel_(rmodel),
17  rdata_(rmodel_),
18  lf_foot_id_(rmodel_.getFrameId(
19  lf_foot, (pinocchio::FrameType)(pinocchio::JOINT | pinocchio::FIXED_JOINT | pinocchio::BODY))),
20  rf_foot_id_(rmodel_.getFrameId(
21  rf_foot, (pinocchio::FrameType)(pinocchio::JOINT | pinocchio::FIXED_JOINT | pinocchio::BODY))),
22  lh_foot_id_(rmodel_.getFrameId(
23  lh_foot, (pinocchio::FrameType)(pinocchio::JOINT | pinocchio::FIXED_JOINT | pinocchio::BODY))),
24  rh_foot_id_(rmodel_.getFrameId(
25  rh_foot, (pinocchio::FrameType)(pinocchio::JOINT | pinocchio::FIXED_JOINT | pinocchio::BODY))),
26  state_(boost::make_shared<crocoddyl::StateMultibody>(boost::make_shared<pinocchio::Model>(rmodel_))),
27  actuation_(boost::make_shared<crocoddyl::ActuationModelFloatingBase>(state_)),
28  firtstep_(true),
29  defaultstate_(rmodel_.nq + rmodel_.nv) {
30  defaultstate_.head(rmodel_.nq) = rmodel_.referenceConfigurations["standing"];
31  defaultstate_.tail(rmodel_.nv).setZero();
32 }
33 
34 SimpleQuadrupedGaitProblem::~SimpleQuadrupedGaitProblem() {}
35 
36 boost::shared_ptr<crocoddyl::ShootingProblem> SimpleQuadrupedGaitProblem::createWalkingProblem(
37  const Eigen::VectorXd& x0, const double steplength, const double stepheight, const double timestep,
38  const std::size_t stepknots, const std::size_t supportknots) {
39  int nq = rmodel_.nq;
40 
41  // Initial Condition
42  const Eigen::VectorBlock<const Eigen::VectorXd> q0 = x0.head(nq);
43  pinocchio::forwardKinematics(rmodel_, rdata_, q0);
44  pinocchio::centerOfMass(rmodel_, rdata_, q0);
45  pinocchio::updateFramePlacements(rmodel_, rdata_);
46 
47  const pinocchio::SE3::Vector3& rf_foot_pos0 = rdata_.oMf[rf_foot_id_].translation();
48  const pinocchio::SE3::Vector3& rh_foot_pos0 = rdata_.oMf[rh_foot_id_].translation();
49  const pinocchio::SE3::Vector3& lf_foot_pos0 = rdata_.oMf[lf_foot_id_].translation();
50  const pinocchio::SE3::Vector3& lh_foot_pos0 = rdata_.oMf[lh_foot_id_].translation();
51 
52  pinocchio::SE3::Vector3 comRef = (rf_foot_pos0 + rh_foot_pos0 + lf_foot_pos0 + lh_foot_pos0) / 4;
53  comRef[2] = rdata_.com[0][2];
54 
55  // Defining the action models along the time instances
56  std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > loco3d_model;
57  std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > rh_step, rf_step, lh_step, lf_step;
58 
59  // doublesupport
60  std::vector<pinocchio::FrameIndex> support_feet;
61  support_feet.push_back(lf_foot_id_);
62  support_feet.push_back(rf_foot_id_);
63  support_feet.push_back(lh_foot_id_);
64  support_feet.push_back(rh_foot_id_);
65  Eigen::Vector3d nullCoM = Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity());
66  const std::vector<crocoddyl::FramePlacement> emptyVector;
67  std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > doubleSupport(
68  supportknots, createSwingFootModel(timestep, support_feet, nullCoM, emptyVector));
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<crocoddyl::FramePlacement> foot_swing_task;
126  for (std::size_t k = 0; k < n_knots; ++k) {
127  double _kp1_n = 0;
128  Eigen::Vector3d dp = Eigen::Vector3d::Zero();
129  foot_swing_task.clear();
130  for (std::size_t i = 0; i < swingFootIds.size(); ++i) {
131  // Defining a foot swing task given the step length resKnot = n_knots % 2
132  std::size_t phaseknots = n_knots >> 1; // bitwise divide.
133  _kp1_n = static_cast<double>(k + 1) / static_cast<double>(n_knots);
134  double _k = static_cast<double>(k);
135  double _phaseknots = static_cast<double>(phaseknots);
136  if (k < phaseknots)
137  dp << steplength * _kp1_n, 0., stepheight * _k / _phaseknots;
138  else if (k == phaseknots)
139  dp << steplength * _kp1_n, 0., stepheight;
140  else
141  dp << steplength * _kp1_n, 0., stepheight * (1 - (_k - _phaseknots) / _phaseknots);
142  Eigen::Vector3d tref = feet_pos0[i] + dp;
143 
144  foot_swing_task.push_back(
145  crocoddyl::FramePlacement(swingFootIds[i], pinocchio::SE3(Eigen::Matrix3d::Identity(), tref)));
146  }
147 
148  // Action model for the foot switch
149  Eigen::Vector3d com_task = Eigen::Vector3d(steplength * _kp1_n, 0., 0.) * com_percentage + com_pos0;
150  foot_swing_model.push_back(createSwingFootModel(timestep, support_foot_ids, com_task, foot_swing_task));
151  }
152  // Action model for the foot switch
153  foot_swing_model.push_back(createFootSwitchModel(support_foot_ids, foot_swing_task));
154 
155  // Updating the current foot position for next step
156  com_pos0 += Eigen::Vector3d(steplength * com_percentage, 0., 0.);
157  for (std::size_t i = 0; i < feet_pos0.size(); ++i) {
158  feet_pos0[i] += Eigen::Vector3d(steplength, 0., 0.);
159  }
160  return foot_swing_model;
161 }
162 
163 boost::shared_ptr<crocoddyl::ActionModelAbstract> SimpleQuadrupedGaitProblem::createSwingFootModel(
164  double timestep, const std::vector<pinocchio::FrameIndex>& support_foot_ids, const Eigen::Vector3d& com_task,
165  const std::vector<crocoddyl::FramePlacement>& foot_swing_task) {
166  // Creating a 3D multi-contact model, and then including the supporting foot
167  boost::shared_ptr<crocoddyl::ContactModelMultiple> contact_model =
168  boost::make_shared<crocoddyl::ContactModelMultiple>(state_, actuation_->get_nu());
169  for (std::vector<pinocchio::FrameIndex>::const_iterator it = support_foot_ids.begin(); it != support_foot_ids.end();
170  ++it) {
171  crocoddyl::FrameTranslation xref(*it, Eigen::Vector3d::Zero());
172  boost::shared_ptr<crocoddyl::ContactModelAbstract> support_contact_model =
173  boost::make_shared<crocoddyl::ContactModel3D>(state_, xref, actuation_->get_nu(), Eigen::Vector2d(0., 50.));
174  contact_model->addContact(rmodel_.frames[*it].name + "_contact", support_contact_model);
175  }
176 
177  // Creating the cost model for a contact phase
178  boost::shared_ptr<crocoddyl::CostModelSum> cost_model =
179  boost::make_shared<crocoddyl::CostModelSum>(state_, actuation_->get_nu());
180  if (com_task.array().allFinite()) {
181  boost::shared_ptr<crocoddyl::CostModelAbstract> com_track =
182  boost::make_shared<crocoddyl::CostModelCoMPosition>(state_, com_task, actuation_->get_nu());
183  cost_model->addCost("comTrack", com_track, 1e6);
184  }
185  if (!foot_swing_task.empty()) {
186  for (std::vector<crocoddyl::FramePlacement>::const_iterator it = foot_swing_task.begin();
187  it != foot_swing_task.end(); ++it) {
188  crocoddyl::FrameTranslation xref(it->id, it->placement.translation());
189  boost::shared_ptr<crocoddyl::CostModelAbstract> foot_track =
190  boost::make_shared<crocoddyl::CostModelFrameTranslation>(state_, xref, actuation_->get_nu());
191  cost_model->addCost(rmodel_.frames[it->id].name + "_footTrack", foot_track, 1e6);
192  }
193  }
194  Eigen::VectorXd state_weights(2 * rmodel_.nv);
195  state_weights.head<3>().fill(0.);
196  state_weights.segment<3>(3).fill(pow(500., 2));
197  state_weights.segment(6, rmodel_.nv - 6).fill(pow(0.01, 2));
198  state_weights.segment(rmodel_.nv, 6).fill(pow(10., 2));
199  state_weights.segment(rmodel_.nv + 6, rmodel_.nv - 6).fill(pow(1., 2));
200  boost::shared_ptr<crocoddyl::ActivationModelAbstract> state_activation =
201  boost::make_shared<crocoddyl::ActivationModelWeightedQuad>(state_weights);
202  boost::shared_ptr<crocoddyl::CostModelAbstract> state_reg =
203  boost::make_shared<crocoddyl::CostModelState>(state_, state_activation, defaultstate_, actuation_->get_nu());
204  boost::shared_ptr<crocoddyl::CostModelAbstract> ctrl_reg =
205  boost::make_shared<crocoddyl::CostModelControl>(state_, actuation_->get_nu());
206  cost_model->addCost("stateReg", state_reg, 1e1);
207  cost_model->addCost("ctrlReg", ctrl_reg, 1e-1);
208 
209  // Creating the action model for the KKT dynamics with simpletic Euler integration scheme
210  boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dmodel =
211  boost::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(state_, actuation_, contact_model,
212  cost_model);
213  return boost::make_shared<crocoddyl::IntegratedActionModelEuler>(dmodel, timestep);
214 }
215 
216 boost::shared_ptr<ActionModelAbstract> SimpleQuadrupedGaitProblem::createFootSwitchModel(
217  const std::vector<pinocchio::FrameIndex>& support_foot_ids,
218  const std::vector<crocoddyl::FramePlacement>& foot_swing_task, bool pseudo_impulse) {
219  if (pseudo_impulse) {
220  return createPseudoImpulseModel(support_foot_ids, foot_swing_task);
221  } else {
222  return createImpulseModel(support_foot_ids, foot_swing_task);
223  }
224 }
225 
226 boost::shared_ptr<crocoddyl::ActionModelAbstract> SimpleQuadrupedGaitProblem::createPseudoImpulseModel(
227  const std::vector<pinocchio::FrameIndex>& support_foot_ids,
228  const std::vector<crocoddyl::FramePlacement>& foot_swing_task) {
229  // Creating a 3D multi-contact model, and then including the supporting foot
230  boost::shared_ptr<crocoddyl::ContactModelMultiple> contact_model =
231  boost::make_shared<crocoddyl::ContactModelMultiple>(state_, actuation_->get_nu());
232  for (std::vector<pinocchio::FrameIndex>::const_iterator it = support_foot_ids.begin(); it != support_foot_ids.end();
233  ++it) {
234  crocoddyl::FrameTranslation xref(*it, Eigen::Vector3d::Zero());
235  boost::shared_ptr<crocoddyl::ContactModelAbstract> support_contact_model =
236  boost::make_shared<crocoddyl::ContactModel3D>(state_, xref, actuation_->get_nu(), Eigen::Vector2d(0., 50.));
237  contact_model->addContact(rmodel_.frames[*it].name + "_contact", support_contact_model);
238  }
239 
240  // Creating the cost model for a contact phase
241  boost::shared_ptr<crocoddyl::CostModelSum> cost_model =
242  boost::make_shared<crocoddyl::CostModelSum>(state_, actuation_->get_nu());
243  if (!foot_swing_task.empty()) {
244  for (std::vector<crocoddyl::FramePlacement>::const_iterator it = foot_swing_task.begin();
245  it != foot_swing_task.end(); ++it) {
246  crocoddyl::FrameTranslation xref(it->id, it->placement.translation());
247  crocoddyl::FrameMotion vref(it->id, pinocchio::Motion::Zero());
248  boost::shared_ptr<crocoddyl::CostModelAbstract> foot_track =
249  boost::make_shared<crocoddyl::CostModelFrameTranslation>(state_, xref, actuation_->get_nu());
250  boost::shared_ptr<crocoddyl::CostModelAbstract> impulse_foot_vel =
251  boost::make_shared<crocoddyl::CostModelFrameVelocity>(state_, vref, actuation_->get_nu());
252  cost_model->addCost(rmodel_.frames[it->id].name + "_footTrack", foot_track, 1e7);
253  cost_model->addCost(rmodel_.frames[it->id].name + "_impulseVel", impulse_foot_vel, 1e6);
254  }
255  }
256  Eigen::VectorXd state_weights(2 * rmodel_.nv);
257  state_weights.head<3>().fill(0.);
258  state_weights.segment<3>(3).fill(pow(500., 2));
259  state_weights.segment(6, rmodel_.nv - 6).fill(pow(0.01, 2));
260  state_weights.segment(rmodel_.nv, rmodel_.nv).fill(pow(10., 2));
261  boost::shared_ptr<crocoddyl::ActivationModelAbstract> state_activation =
262  boost::make_shared<crocoddyl::ActivationModelWeightedQuad>(state_weights);
263  boost::shared_ptr<crocoddyl::CostModelAbstract> state_reg =
264  boost::make_shared<crocoddyl::CostModelState>(state_, state_activation, defaultstate_, actuation_->get_nu());
265  boost::shared_ptr<crocoddyl::CostModelAbstract> ctrl_reg =
266  boost::make_shared<crocoddyl::CostModelControl>(state_, actuation_->get_nu());
267  cost_model->addCost("stateReg", state_reg, 1e1);
268  cost_model->addCost("ctrlReg", ctrl_reg, 1e-3);
269 
270  // Creating the action model for the KKT dynamics with simpletic Euler integration scheme
271  boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dmodel =
272  boost::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(state_, actuation_, contact_model,
273  cost_model);
274  return boost::make_shared<crocoddyl::IntegratedActionModelEuler>(dmodel, 0.);
275 }
276 
277 boost::shared_ptr<ActionModelAbstract> SimpleQuadrupedGaitProblem::createImpulseModel(
278  const std::vector<pinocchio::FrameIndex>& support_foot_ids, const std::vector<FramePlacement>& foot_swing_task) {
279  // Creating a 3D multi-contact model, and then including the supporting foot
280  boost::shared_ptr<crocoddyl::ImpulseModelMultiple> impulse_model =
281  boost::make_shared<crocoddyl::ImpulseModelMultiple>(state_);
282  for (std::vector<pinocchio::FrameIndex>::const_iterator it = support_foot_ids.begin(); it != support_foot_ids.end();
283  ++it) {
284  boost::shared_ptr<crocoddyl::ImpulseModelAbstract> support_contact_model =
285  boost::make_shared<crocoddyl::ImpulseModel3D>(state_, *it);
286  impulse_model->addImpulse(rmodel_.frames[*it].name + "_impulse", support_contact_model);
287  }
288 
289  // Creating the cost model for a contact phase
290  boost::shared_ptr<crocoddyl::CostModelSum> cost_model = boost::make_shared<crocoddyl::CostModelSum>(state_, 0);
291  if (!foot_swing_task.empty()) {
292  for (std::vector<crocoddyl::FramePlacement>::const_iterator it = foot_swing_task.begin();
293  it != foot_swing_task.end(); ++it) {
294  crocoddyl::FrameTranslation xref(it->id, it->placement.translation());
295  boost::shared_ptr<crocoddyl::CostModelAbstract> foot_track =
296  boost::make_shared<crocoddyl::CostModelFrameTranslation>(state_, xref, 0);
297  cost_model->addCost(rmodel_.frames[it->id].name + "_footTrack", foot_track, 1e7);
298  }
299  }
300  Eigen::VectorXd state_weights(2 * rmodel_.nv);
301  state_weights.head<6>().fill(1.);
302  state_weights.segment(6, rmodel_.nv - 6).fill(pow(10., 2));
303  state_weights.segment(rmodel_.nv, rmodel_.nv).fill(pow(10., 2));
304  boost::shared_ptr<crocoddyl::ActivationModelAbstract> state_activation =
305  boost::make_shared<crocoddyl::ActivationModelWeightedQuad>(state_weights);
306  boost::shared_ptr<crocoddyl::CostModelAbstract> state_reg =
307  boost::make_shared<crocoddyl::CostModelState>(state_, state_activation, defaultstate_, 0);
308  cost_model->addCost("stateReg", state_reg, 1e1);
309 
310  // Creating the action model for the KKT dynamics with simpletic Euler integration scheme
311  return boost::make_shared<crocoddyl::ActionModelImpulseFwdDynamics>(state_, impulse_model, cost_model);
312 }
313 
314 const Eigen::VectorXd& SimpleQuadrupedGaitProblem::get_defaultState() const { return defaultstate_; }
315 
316 } // namespace crocoddyl