9 #include "crocoddyl/multibody/utils/quadruped-gaits.hpp" 10 #include "crocoddyl/core/costs/residual.hpp" 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)
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();
35 SimpleQuadrupedGaitProblem::~SimpleQuadrupedGaitProblem() {}
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) {
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 const std::vector<crocoddyl::FramePlacement> emptyVector;
68 std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > doubleSupport(
69 supportknots, createSwingFootModel(timestep, support_feet, nullCoM, emptyVector));
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_};
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]));
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_);
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);
91 rh_step = createFootStepModels(timestep, comRef, rh_foot_pos0_v, 0.5 * steplength, stepheight, stepknots,
93 rf_step = createFootStepModels(timestep, comRef, rf_foot_pos0_v, 0.5 * steplength, stepheight, stepknots,
98 createFootStepModels(timestep, comRef, rh_foot_pos0_v, steplength, stepheight, stepknots, rh_support, rh_foot);
100 createFootStepModels(timestep, comRef, rf_foot_pos0_v, steplength, stepheight, stepknots, rf_support, rf_foot);
103 createFootStepModels(timestep, comRef, lh_foot_pos0_v, steplength, stepheight, stepknots, lh_support, lh_foot);
105 createFootStepModels(timestep, comRef, lf_foot_pos0_v, steplength, stepheight, stepknots, lf_support, lf_foot);
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());
114 return boost::make_shared<crocoddyl::ShootingProblem>(x0, loco3d_model, loco3d_model.back());
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);
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) {
129 Eigen::Vector3d dp = Eigen::Vector3d::Zero();
130 foot_swing_task.clear();
131 for (std::size_t i = 0; i < swingFootIds.size(); ++i) {
133 std::size_t phaseknots = n_knots >> 1;
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);
138 dp << steplength * _kp1_n, 0., stepheight * _k / _phaseknots;
139 else if (k == phaseknots)
140 dp << steplength * _kp1_n, 0., stepheight;
142 dp << steplength * _kp1_n, 0., stepheight * (1 - (_k - _phaseknots) / _phaseknots);
143 Eigen::Vector3d tref = feet_pos0[i] + dp;
145 foot_swing_task.push_back(
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));
154 foot_swing_model.push_back(createFootSwitchModel(support_foot_ids, foot_swing_task));
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.);
161 return foot_swing_model;
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) {
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();
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);
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);
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);
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);
212 boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dmodel =
213 boost::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(state_, actuation_, contact_model,
215 return boost::make_shared<crocoddyl::IntegratedActionModelEuler>(dmodel, timestep);
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);
224 return createImpulseModel(support_foot_ids, foot_swing_task);
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) {
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();
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);
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>(
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);
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);
276 boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dmodel =
277 boost::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(state_, actuation_, contact_model,
279 return boost::make_shared<crocoddyl::IntegratedActionModelEuler>(dmodel, 0.);
282 boost::shared_ptr<ActionModelAbstract> SimpleQuadrupedGaitProblem::createImpulseModel(
283 const std::vector<pinocchio::FrameIndex>& support_foot_ids,
const std::vector<FramePlacement>& foot_swing_task) {
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();
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);
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);
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);
316 return boost::make_shared<crocoddyl::ActionModelImpulseFwdDynamics>(state_, impulse_model, cost_model);
319 const Eigen::VectorXd& SimpleQuadrupedGaitProblem::get_defaultState()
const {
return defaultstate_; }