9#include "crocoddyl/multibody/utils/quadruped-gaits.hpp"
10#include "crocoddyl/core/costs/residual.hpp"
14SimpleQuadrupedGaitProblem::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)
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_)),
30 defaultstate_(rmodel_.nq + rmodel_.nv) {
31 defaultstate_.head(rmodel_.nq) = rmodel_.referenceConfigurations[
"standing"];
32 defaultstate_.tail(rmodel_.nv).setZero();
35SimpleQuadrupedGaitProblem::~SimpleQuadrupedGaitProblem() {}
37boost::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) {
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_);
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();
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];
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;
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));
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_};
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]));
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_);
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);
90 rh_step = createFootStepModels(timestep, comRef, rh_foot_pos0_v, 0.5 * steplength, stepheight, stepknots,
92 rf_step = createFootStepModels(timestep, comRef, rf_foot_pos0_v, 0.5 * steplength, stepheight, stepknots,
97 createFootStepModels(timestep, comRef, rh_foot_pos0_v, steplength, stepheight, stepknots, rh_support, rh_foot);
99 createFootStepModels(timestep, comRef, rf_foot_pos0_v, steplength, stepheight, stepknots, rf_support, rf_foot);
102 createFootStepModels(timestep, comRef, lh_foot_pos0_v, steplength, stepheight, stepknots, lh_support, lh_foot);
104 createFootStepModels(timestep, comRef, lf_foot_pos0_v, steplength, stepheight, stepknots, lf_support, lf_foot);
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());
113 return boost::make_shared<crocoddyl::ShootingProblem>(x0, loco3d_model, loco3d_model.back());
116std::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);
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) {
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) {
134 std::size_t phaseknots = n_knots >> 1;
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);
139 dp << steplength * _kp1_n, 0., stepheight * _k / _phaseknots;
140 else if (k == phaseknots)
141 dp << steplength * _kp1_n, 0., stepheight;
143 dp << steplength * _kp1_n, 0., stepheight * (1 - (_k - _phaseknots) / _phaseknots);
144 Eigen::Vector3d tref = feet_pos0[i] + dp;
146 id_foot_swing_task.push_back(swingFootIds[i]);
147 ref_foot_swing_task.push_back(pinocchio::SE3(Eigen::Matrix3d::Identity(), tref));
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));
156 foot_swing_model.push_back(createFootSwitchModel(support_foot_ids, id_foot_swing_task, ref_foot_swing_task));
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.);
163 return foot_swing_model;
166boost::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) {
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();
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);
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);
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);
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);
215 boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dmodel =
216 boost::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(state_, actuation_, contact_model,
218 return boost::make_shared<crocoddyl::IntegratedActionModelEuler>(dmodel, timestep);
221boost::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);
228 return createImpulseModel(support_foot_ids, id_foot_swing_task, ref_foot_swing_task);
232boost::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) {
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();
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);
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>(
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);
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);
281 boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dmodel =
282 boost::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(state_, actuation_, contact_model,
284 return boost::make_shared<crocoddyl::IntegratedActionModelEuler>(dmodel, 0.);
287boost::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) {
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();
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);
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);
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);
323 return boost::make_shared<crocoddyl::ActionModelImpulseFwdDynamics>(state_, impulse_model, cost_model);
326const Eigen::VectorXd& SimpleQuadrupedGaitProblem::get_defaultState()
const {
return defaultstate_; }