9 #include "crocoddyl/multibody/utils/quadruped-gaits.hpp" 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)
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_)),
29 defaultstate_(rmodel_.nq + rmodel_.nv) {
30 defaultstate_.head(rmodel_.nq) = rmodel_.referenceConfigurations[
"standing"];
31 defaultstate_.tail(rmodel_.nv).setZero();
34 SimpleQuadrupedGaitProblem::~SimpleQuadrupedGaitProblem() {}
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) {
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_);
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();
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];
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;
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));
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());
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);
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) {
128 Eigen::Vector3d dp = Eigen::Vector3d::Zero();
129 foot_swing_task.clear();
130 for (std::size_t i = 0; i < swingFootIds.size(); ++i) {
132 std::size_t phaseknots = n_knots >> 1;
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);
137 dp << steplength * _kp1_n, 0., stepheight * _k / _phaseknots;
138 else if (k == phaseknots)
139 dp << steplength * _kp1_n, 0., stepheight;
141 dp << steplength * _kp1_n, 0., stepheight * (1 - (_k - _phaseknots) / _phaseknots);
142 Eigen::Vector3d tref = feet_pos0[i] + dp;
144 foot_swing_task.push_back(
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));
153 foot_swing_model.push_back(createFootSwitchModel(support_foot_ids, foot_swing_task));
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.);
160 return foot_swing_model;
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) {
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();
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);
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);
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) {
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);
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);
210 boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dmodel =
211 boost::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(state_, actuation_, contact_model,
213 return boost::make_shared<crocoddyl::IntegratedActionModelEuler>(dmodel, timestep);
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);
222 return createImpulseModel(support_foot_ids, foot_swing_task);
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) {
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();
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);
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) {
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);
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);
271 boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dmodel =
272 boost::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(state_, actuation_, contact_model,
274 return boost::make_shared<crocoddyl::IntegratedActionModelEuler>(dmodel, 0.);
277 boost::shared_ptr<ActionModelAbstract> SimpleQuadrupedGaitProblem::createImpulseModel(
278 const std::vector<pinocchio::FrameIndex>& support_foot_ids,
const std::vector<FramePlacement>& foot_swing_task) {
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();
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);
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) {
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);
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);
311 return boost::make_shared<crocoddyl::ActionModelImpulseFwdDynamics>(state_, impulse_model, cost_model);
314 const Eigen::VectorXd& SimpleQuadrupedGaitProblem::get_defaultState()
const {
return defaultstate_; }