| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // 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. | ||
| 7 | /////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | #include "crocoddyl/multibody/utils/quadruped-gaits.hpp" | ||
| 10 | |||
| 11 | #include "crocoddyl/core/costs/residual.hpp" | ||
| 12 | |||
| 13 | namespace crocoddyl { | ||
| 14 | |||
| 15 | ✗ | SimpleQuadrupedGaitProblem::SimpleQuadrupedGaitProblem( | |
| 16 | const pinocchio::Model& rmodel, const std::string& lf_foot, | ||
| 17 | const std::string& rf_foot, const std::string& lh_foot, | ||
| 18 | ✗ | const std::string& rh_foot) | |
| 19 | ✗ | : rmodel_(rmodel), | |
| 20 | ✗ | rdata_(rmodel_), | |
| 21 | ✗ | lf_foot_id_(rmodel_.getFrameId( | |
| 22 | lf_foot, | ||
| 23 | ✗ | (pinocchio::FrameType)(pinocchio::JOINT | pinocchio::FIXED_JOINT | | |
| 24 | pinocchio::BODY))), | ||
| 25 | ✗ | rf_foot_id_(rmodel_.getFrameId( | |
| 26 | rf_foot, | ||
| 27 | ✗ | (pinocchio::FrameType)(pinocchio::JOINT | pinocchio::FIXED_JOINT | | |
| 28 | pinocchio::BODY))), | ||
| 29 | ✗ | lh_foot_id_(rmodel_.getFrameId( | |
| 30 | lh_foot, | ||
| 31 | ✗ | (pinocchio::FrameType)(pinocchio::JOINT | pinocchio::FIXED_JOINT | | |
| 32 | pinocchio::BODY))), | ||
| 33 | ✗ | rh_foot_id_(rmodel_.getFrameId( | |
| 34 | rh_foot, | ||
| 35 | ✗ | (pinocchio::FrameType)(pinocchio::JOINT | pinocchio::FIXED_JOINT | | |
| 36 | pinocchio::BODY))), | ||
| 37 | ✗ | state_(std::make_shared<crocoddyl::StateMultibody>( | |
| 38 | ✗ | std::make_shared<pinocchio::Model>(rmodel_))), | |
| 39 | ✗ | actuation_( | |
| 40 | ✗ | std::make_shared<crocoddyl::ActuationModelFloatingBase>(state_)), | |
| 41 | ✗ | firtstep_(true), | |
| 42 | ✗ | defaultstate_(rmodel_.nq + rmodel_.nv) { | |
| 43 | ✗ | defaultstate_.head(rmodel_.nq) = rmodel_.referenceConfigurations["standing"]; | |
| 44 | ✗ | defaultstate_.tail(rmodel_.nv).setZero(); | |
| 45 | ✗ | } | |
| 46 | |||
| 47 | ✗ | SimpleQuadrupedGaitProblem::~SimpleQuadrupedGaitProblem() {} | |
| 48 | |||
| 49 | std::shared_ptr<crocoddyl::ShootingProblem> | ||
| 50 | ✗ | SimpleQuadrupedGaitProblem::createWalkingProblem( | |
| 51 | const Eigen::VectorXd& x0, const double steplength, const double stepheight, | ||
| 52 | const double timestep, const std::size_t stepknots, | ||
| 53 | const std::size_t supportknots) { | ||
| 54 | ✗ | int nq = rmodel_.nq; | |
| 55 | |||
| 56 | // Initial Condition | ||
| 57 | ✗ | const Eigen::VectorBlock<const Eigen::VectorXd> q0 = x0.head(nq); | |
| 58 | ✗ | pinocchio::forwardKinematics(rmodel_, rdata_, q0); | |
| 59 | ✗ | pinocchio::centerOfMass(rmodel_, rdata_, q0); | |
| 60 | ✗ | pinocchio::updateFramePlacements(rmodel_, rdata_); | |
| 61 | |||
| 62 | const pinocchio::SE3::Vector3& rf_foot_pos0 = | ||
| 63 | ✗ | rdata_.oMf[rf_foot_id_].translation(); | |
| 64 | const pinocchio::SE3::Vector3& rh_foot_pos0 = | ||
| 65 | ✗ | rdata_.oMf[rh_foot_id_].translation(); | |
| 66 | const pinocchio::SE3::Vector3& lf_foot_pos0 = | ||
| 67 | ✗ | rdata_.oMf[lf_foot_id_].translation(); | |
| 68 | const pinocchio::SE3::Vector3& lh_foot_pos0 = | ||
| 69 | ✗ | rdata_.oMf[lh_foot_id_].translation(); | |
| 70 | |||
| 71 | pinocchio::SE3::Vector3 comRef = | ||
| 72 | ✗ | (rf_foot_pos0 + rh_foot_pos0 + lf_foot_pos0 + lh_foot_pos0) / 4; | |
| 73 | ✗ | comRef[2] = rdata_.com[0][2]; | |
| 74 | |||
| 75 | // Defining the action models along the time instances | ||
| 76 | ✗ | std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract> > loco3d_model; | |
| 77 | ✗ | std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract> > rh_step, | |
| 78 | ✗ | rf_step, lh_step, lf_step; | |
| 79 | |||
| 80 | // doublesupport | ||
| 81 | ✗ | std::vector<pinocchio::FrameIndex> support_feet; | |
| 82 | ✗ | support_feet.push_back(lf_foot_id_); | |
| 83 | ✗ | support_feet.push_back(rf_foot_id_); | |
| 84 | ✗ | support_feet.push_back(lh_foot_id_); | |
| 85 | ✗ | support_feet.push_back(rh_foot_id_); | |
| 86 | Eigen::Vector3d nullCoM = | ||
| 87 | ✗ | Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity()); | |
| 88 | std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract> > doubleSupport( | ||
| 89 | ✗ | supportknots, createSwingFootModel(timestep, support_feet, nullCoM)); | |
| 90 | |||
| 91 | ✗ | const pinocchio::FrameIndex rh_s[] = {lf_foot_id_, rf_foot_id_, lh_foot_id_}; | |
| 92 | ✗ | const pinocchio::FrameIndex rf_s[] = {lf_foot_id_, lh_foot_id_, rh_foot_id_}; | |
| 93 | ✗ | const pinocchio::FrameIndex lh_s[] = {lf_foot_id_, rf_foot_id_, rh_foot_id_}; | |
| 94 | ✗ | const pinocchio::FrameIndex lf_s[] = {rf_foot_id_, lh_foot_id_, rh_foot_id_}; | |
| 95 | |||
| 96 | std::vector<pinocchio::FrameIndex> rh_support( | ||
| 97 | ✗ | rh_s, rh_s + sizeof(rh_s) / sizeof(rh_s[0])); | |
| 98 | std::vector<pinocchio::FrameIndex> rf_support( | ||
| 99 | ✗ | rf_s, rf_s + sizeof(rf_s) / sizeof(rf_s[0])); | |
| 100 | std::vector<pinocchio::FrameIndex> lh_support( | ||
| 101 | ✗ | lh_s, lh_s + sizeof(lh_s) / sizeof(lh_s[0])); | |
| 102 | std::vector<pinocchio::FrameIndex> lf_support( | ||
| 103 | ✗ | lf_s, lf_s + sizeof(lf_s) / sizeof(lf_s[0])); | |
| 104 | |||
| 105 | ✗ | std::vector<pinocchio::FrameIndex> rh_foot(1, rh_foot_id_); | |
| 106 | ✗ | std::vector<pinocchio::FrameIndex> rf_foot(1, rf_foot_id_); | |
| 107 | ✗ | std::vector<pinocchio::FrameIndex> lf_foot(1, lf_foot_id_); | |
| 108 | ✗ | std::vector<pinocchio::FrameIndex> lh_foot(1, lh_foot_id_); | |
| 109 | |||
| 110 | ✗ | std::vector<Eigen::Vector3d> rh_foot_pos0_v(1, rh_foot_pos0); | |
| 111 | ✗ | std::vector<Eigen::Vector3d> lh_foot_pos0_v(1, lh_foot_pos0); | |
| 112 | ✗ | std::vector<Eigen::Vector3d> rf_foot_pos0_v(1, rf_foot_pos0); | |
| 113 | ✗ | std::vector<Eigen::Vector3d> lf_foot_pos0_v(1, lf_foot_pos0); | |
| 114 | ✗ | if (firtstep_) { | |
| 115 | rh_step = | ||
| 116 | ✗ | createFootStepModels(timestep, comRef, rh_foot_pos0_v, 0.5 * steplength, | |
| 117 | ✗ | stepheight, stepknots, rh_support, rh_foot); | |
| 118 | rf_step = | ||
| 119 | ✗ | createFootStepModels(timestep, comRef, rf_foot_pos0_v, 0.5 * steplength, | |
| 120 | ✗ | stepheight, stepknots, rf_support, rf_foot); | |
| 121 | ✗ | firtstep_ = false; | |
| 122 | } else { | ||
| 123 | ✗ | rh_step = createFootStepModels(timestep, comRef, rh_foot_pos0_v, steplength, | |
| 124 | ✗ | stepheight, stepknots, rh_support, rh_foot); | |
| 125 | ✗ | rf_step = createFootStepModels(timestep, comRef, rf_foot_pos0_v, steplength, | |
| 126 | ✗ | stepheight, stepknots, rf_support, rf_foot); | |
| 127 | } | ||
| 128 | ✗ | lh_step = createFootStepModels(timestep, comRef, lh_foot_pos0_v, steplength, | |
| 129 | ✗ | stepheight, stepknots, lh_support, lh_foot); | |
| 130 | ✗ | lf_step = createFootStepModels(timestep, comRef, lf_foot_pos0_v, steplength, | |
| 131 | ✗ | stepheight, stepknots, lf_support, lf_foot); | |
| 132 | |||
| 133 | ✗ | loco3d_model.insert(loco3d_model.end(), doubleSupport.begin(), | |
| 134 | doubleSupport.end()); | ||
| 135 | ✗ | loco3d_model.insert(loco3d_model.end(), rh_step.begin(), rh_step.end()); | |
| 136 | ✗ | loco3d_model.insert(loco3d_model.end(), rf_step.begin(), rf_step.end()); | |
| 137 | ✗ | loco3d_model.insert(loco3d_model.end(), doubleSupport.begin(), | |
| 138 | doubleSupport.end()); | ||
| 139 | ✗ | loco3d_model.insert(loco3d_model.end(), lh_step.begin(), lh_step.end()); | |
| 140 | ✗ | loco3d_model.insert(loco3d_model.end(), lf_step.begin(), lf_step.end()); | |
| 141 | |||
| 142 | return std::make_shared<crocoddyl::ShootingProblem>(x0, loco3d_model, | ||
| 143 | ✗ | loco3d_model.back()); | |
| 144 | ✗ | } | |
| 145 | |||
| 146 | std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract> > | ||
| 147 | ✗ | SimpleQuadrupedGaitProblem::createFootStepModels( | |
| 148 | double timestep, Eigen::Vector3d& com_pos0, | ||
| 149 | std::vector<Eigen::Vector3d>& feet_pos0, double steplength, | ||
| 150 | double stepheight, std::size_t n_knots, | ||
| 151 | const std::vector<pinocchio::FrameIndex>& support_foot_ids, | ||
| 152 | const std::vector<pinocchio::FrameIndex>& swingFootIds) { | ||
| 153 | std::size_t n_legs = | ||
| 154 | ✗ | static_cast<std::size_t>(support_foot_ids.size() + swingFootIds.size()); | |
| 155 | double com_percentage = | ||
| 156 | ✗ | static_cast<double>(swingFootIds.size()) / static_cast<double>(n_legs); | |
| 157 | |||
| 158 | // Action models for the foot swing | ||
| 159 | ✗ | std::vector<std::shared_ptr<ActionModelAbstract> > foot_swing_model; | |
| 160 | ✗ | std::vector<pinocchio::FrameIndex> id_foot_swing_task; | |
| 161 | ✗ | std::vector<pinocchio::SE3> ref_foot_swing_task; | |
| 162 | ✗ | for (std::size_t k = 0; k < n_knots; ++k) { | |
| 163 | ✗ | double _kp1_n = 0; | |
| 164 | ✗ | Eigen::Vector3d dp = Eigen::Vector3d::Zero(); | |
| 165 | ✗ | id_foot_swing_task.clear(); | |
| 166 | ✗ | ref_foot_swing_task.clear(); | |
| 167 | ✗ | for (std::size_t i = 0; i < swingFootIds.size(); ++i) { | |
| 168 | // Defining a foot swing task given the step length resKnot = n_knots % 2 | ||
| 169 | ✗ | std::size_t phaseknots = n_knots >> 1; // bitwise divide. | |
| 170 | ✗ | _kp1_n = static_cast<double>(k + 1) / static_cast<double>(n_knots); | |
| 171 | ✗ | double _k = static_cast<double>(k); | |
| 172 | ✗ | double _phaseknots = static_cast<double>(phaseknots); | |
| 173 | ✗ | if (k < phaseknots) | |
| 174 | ✗ | dp << steplength * _kp1_n, 0., stepheight * _k / _phaseknots; | |
| 175 | ✗ | else if (k == phaseknots) | |
| 176 | ✗ | dp << steplength * _kp1_n, 0., stepheight; | |
| 177 | else | ||
| 178 | ✗ | dp << steplength * _kp1_n, 0., | |
| 179 | ✗ | stepheight * (1 - (_k - _phaseknots) / _phaseknots); | |
| 180 | ✗ | Eigen::Vector3d tref = feet_pos0[i] + dp; | |
| 181 | |||
| 182 | ✗ | id_foot_swing_task.push_back(swingFootIds[i]); | |
| 183 | ✗ | ref_foot_swing_task.push_back( | |
| 184 | ✗ | pinocchio::SE3(Eigen::Matrix3d::Identity(), tref)); | |
| 185 | } | ||
| 186 | |||
| 187 | // Action model for the foot switch | ||
| 188 | Eigen::Vector3d com_task = | ||
| 189 | ✗ | Eigen::Vector3d(steplength * _kp1_n, 0., 0.) * com_percentage + | |
| 190 | ✗ | com_pos0; | |
| 191 | ✗ | foot_swing_model.push_back( | |
| 192 | ✗ | createSwingFootModel(timestep, support_foot_ids, com_task, | |
| 193 | id_foot_swing_task, ref_foot_swing_task)); | ||
| 194 | } | ||
| 195 | // Action model for the foot switch | ||
| 196 | ✗ | foot_swing_model.push_back(createFootSwitchModel( | |
| 197 | support_foot_ids, id_foot_swing_task, ref_foot_swing_task)); | ||
| 198 | |||
| 199 | // Updating the current foot position for next step | ||
| 200 | ✗ | com_pos0 += Eigen::Vector3d(steplength * com_percentage, 0., 0.); | |
| 201 | ✗ | for (std::size_t i = 0; i < feet_pos0.size(); ++i) { | |
| 202 | ✗ | feet_pos0[i] += Eigen::Vector3d(steplength, 0., 0.); | |
| 203 | } | ||
| 204 | ✗ | return foot_swing_model; | |
| 205 | ✗ | } | |
| 206 | |||
| 207 | std::shared_ptr<crocoddyl::ActionModelAbstract> | ||
| 208 | ✗ | SimpleQuadrupedGaitProblem::createSwingFootModel( | |
| 209 | double timestep, const std::vector<pinocchio::FrameIndex>& support_foot_ids, | ||
| 210 | const Eigen::Vector3d& com_task, | ||
| 211 | const std::vector<pinocchio::FrameIndex>& id_foot_swing_task, | ||
| 212 | const std::vector<pinocchio::SE3>& ref_foot_swing_task) { | ||
| 213 | // Creating a 3D multi-contact model, and then including the supporting foot | ||
| 214 | std::shared_ptr<crocoddyl::ContactModelMultiple> contact_model = | ||
| 215 | ✗ | std::make_shared<crocoddyl::ContactModelMultiple>(state_, | |
| 216 | ✗ | actuation_->get_nu()); | |
| 217 | ✗ | for (std::vector<pinocchio::FrameIndex>::const_iterator it = | |
| 218 | ✗ | support_foot_ids.begin(); | |
| 219 | ✗ | it != support_foot_ids.end(); ++it) { | |
| 220 | std::shared_ptr<crocoddyl::ContactModelAbstract> support_contact_model = | ||
| 221 | ✗ | std::make_shared<crocoddyl::ContactModel3D>( | |
| 222 | ✗ | state_, *it, Eigen::Vector3d::Zero(), | |
| 223 | ✗ | pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, | |
| 224 | ✗ | actuation_->get_nu(), Eigen::Vector2d(0., 50.)); | |
| 225 | ✗ | contact_model->addContact(rmodel_.frames[*it].name + "_contact", | |
| 226 | support_contact_model); | ||
| 227 | ✗ | } | |
| 228 | |||
| 229 | // Creating the cost model for a contact phase | ||
| 230 | std::shared_ptr<crocoddyl::CostModelSum> cost_model = | ||
| 231 | ✗ | std::make_shared<crocoddyl::CostModelSum>(state_, actuation_->get_nu()); | |
| 232 | ✗ | if (com_task.array().allFinite()) { | |
| 233 | std::shared_ptr<crocoddyl::CostModelAbstract> com_track = | ||
| 234 | ✗ | std::make_shared<crocoddyl::CostModelResidual>( | |
| 235 | ✗ | state_, std::make_shared<crocoddyl::ResidualModelCoMPosition>( | |
| 236 | ✗ | state_, com_task, actuation_->get_nu())); | |
| 237 | ✗ | cost_model->addCost("comTrack", com_track, 1e6); | |
| 238 | ✗ | } | |
| 239 | ✗ | if (!id_foot_swing_task.empty() && !ref_foot_swing_task.empty()) { | |
| 240 | ✗ | for (std::size_t i = 0; i < id_foot_swing_task.size(); ++i) { | |
| 241 | ✗ | const pinocchio::FrameIndex id = id_foot_swing_task[i]; | |
| 242 | std::shared_ptr<crocoddyl::CostModelAbstract> foot_track = | ||
| 243 | ✗ | std::make_shared<crocoddyl::CostModelResidual>( | |
| 244 | ✗ | state_, | |
| 245 | ✗ | std::make_shared<crocoddyl::ResidualModelFrameTranslation>( | |
| 246 | ✗ | state_, id, ref_foot_swing_task[i].translation(), | |
| 247 | ✗ | actuation_->get_nu())); | |
| 248 | ✗ | cost_model->addCost(rmodel_.frames[id].name + "_footTrack", foot_track, | |
| 249 | 1e6); | ||
| 250 | ✗ | } | |
| 251 | } | ||
| 252 | ✗ | Eigen::VectorXd state_weights(2 * rmodel_.nv); | |
| 253 | ✗ | state_weights.head<3>().fill(0.); | |
| 254 | ✗ | state_weights.segment<3>(3).fill(pow(500., 2)); | |
| 255 | ✗ | state_weights.segment(6, rmodel_.nv - 6).fill(pow(0.01, 2)); | |
| 256 | ✗ | state_weights.segment(rmodel_.nv, 6).fill(pow(10., 2)); | |
| 257 | ✗ | state_weights.segment(rmodel_.nv + 6, rmodel_.nv - 6).fill(pow(1., 2)); | |
| 258 | std::shared_ptr<crocoddyl::ActivationModelAbstract> state_activation = | ||
| 259 | ✗ | std::make_shared<crocoddyl::ActivationModelWeightedQuad>(state_weights); | |
| 260 | std::shared_ptr<crocoddyl::CostModelAbstract> state_reg = | ||
| 261 | ✗ | std::make_shared<crocoddyl::CostModelResidual>( | |
| 262 | ✗ | state_, state_activation, | |
| 263 | ✗ | std::make_shared<crocoddyl::ResidualModelState>( | |
| 264 | ✗ | state_, defaultstate_, actuation_->get_nu())); | |
| 265 | std::shared_ptr<crocoddyl::CostModelAbstract> ctrl_reg = | ||
| 266 | ✗ | std::make_shared<crocoddyl::CostModelResidual>( | |
| 267 | ✗ | state_, std::make_shared<crocoddyl::ResidualModelControl>( | |
| 268 | ✗ | state_, actuation_->get_nu())); | |
| 269 | ✗ | cost_model->addCost("stateReg", state_reg, 1e1); | |
| 270 | ✗ | cost_model->addCost("ctrlReg", ctrl_reg, 1e-1); | |
| 271 | |||
| 272 | // Creating the action model for the KKT dynamics with simpletic Euler | ||
| 273 | // integration scheme | ||
| 274 | std::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dmodel = | ||
| 275 | ✗ | std::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>( | |
| 276 | ✗ | state_, actuation_, contact_model, cost_model); | |
| 277 | ✗ | return std::make_shared<crocoddyl::IntegratedActionModelEuler>(dmodel, | |
| 278 | ✗ | timestep); | |
| 279 | ✗ | } | |
| 280 | |||
| 281 | std::shared_ptr<ActionModelAbstract> | ||
| 282 | ✗ | SimpleQuadrupedGaitProblem::createFootSwitchModel( | |
| 283 | const std::vector<pinocchio::FrameIndex>& support_foot_ids, | ||
| 284 | const std::vector<pinocchio::FrameIndex>& id_foot_swing_task, | ||
| 285 | const std::vector<pinocchio::SE3>& ref_foot_swing_task, | ||
| 286 | bool pseudo_impulse) { | ||
| 287 | ✗ | if (pseudo_impulse) { | |
| 288 | return createPseudoImpulseModel(support_foot_ids, id_foot_swing_task, | ||
| 289 | ✗ | ref_foot_swing_task); | |
| 290 | } else { | ||
| 291 | return createImpulseModel(support_foot_ids, id_foot_swing_task, | ||
| 292 | ✗ | ref_foot_swing_task); | |
| 293 | } | ||
| 294 | } | ||
| 295 | |||
| 296 | std::shared_ptr<crocoddyl::ActionModelAbstract> | ||
| 297 | ✗ | SimpleQuadrupedGaitProblem::createPseudoImpulseModel( | |
| 298 | const std::vector<pinocchio::FrameIndex>& support_foot_ids, | ||
| 299 | const std::vector<pinocchio::FrameIndex>& id_foot_swing_task, | ||
| 300 | const std::vector<pinocchio::SE3>& ref_foot_swing_task) { | ||
| 301 | // Creating a 3D multi-contact model, and then including the supporting foot | ||
| 302 | std::shared_ptr<crocoddyl::ContactModelMultiple> contact_model = | ||
| 303 | ✗ | std::make_shared<crocoddyl::ContactModelMultiple>(state_, | |
| 304 | ✗ | actuation_->get_nu()); | |
| 305 | ✗ | for (std::vector<pinocchio::FrameIndex>::const_iterator it = | |
| 306 | ✗ | support_foot_ids.begin(); | |
| 307 | ✗ | it != support_foot_ids.end(); ++it) { | |
| 308 | std::shared_ptr<crocoddyl::ContactModelAbstract> support_contact_model = | ||
| 309 | ✗ | std::make_shared<crocoddyl::ContactModel3D>( | |
| 310 | ✗ | state_, *it, Eigen::Vector3d::Zero(), | |
| 311 | ✗ | pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, | |
| 312 | ✗ | actuation_->get_nu(), Eigen::Vector2d(0., 50.)); | |
| 313 | ✗ | contact_model->addContact(rmodel_.frames[*it].name + "_contact", | |
| 314 | support_contact_model); | ||
| 315 | ✗ | } | |
| 316 | |||
| 317 | // Creating the cost model for a contact phase | ||
| 318 | std::shared_ptr<crocoddyl::CostModelSum> cost_model = | ||
| 319 | ✗ | std::make_shared<crocoddyl::CostModelSum>(state_, actuation_->get_nu()); | |
| 320 | ✗ | if (!id_foot_swing_task.empty() && !ref_foot_swing_task.empty()) { | |
| 321 | ✗ | for (std::size_t i = 0; i < id_foot_swing_task.size(); ++i) { | |
| 322 | ✗ | const pinocchio::FrameIndex id = id_foot_swing_task[i]; | |
| 323 | std::shared_ptr<crocoddyl::CostModelAbstract> foot_track = | ||
| 324 | ✗ | std::make_shared<crocoddyl::CostModelResidual>( | |
| 325 | ✗ | state_, | |
| 326 | ✗ | std::make_shared<crocoddyl::ResidualModelFrameTranslation>( | |
| 327 | ✗ | state_, id, ref_foot_swing_task[i].translation(), | |
| 328 | ✗ | actuation_->get_nu())); | |
| 329 | std::shared_ptr<crocoddyl::CostModelAbstract> impulse_foot_vel = | ||
| 330 | ✗ | std::make_shared<crocoddyl::CostModelResidual>( | |
| 331 | ✗ | state_, | |
| 332 | ✗ | std::make_shared<crocoddyl::ResidualModelFrameVelocity>( | |
| 333 | ✗ | state_, id, pinocchio::Motion::Zero(), | |
| 334 | ✗ | pinocchio::ReferenceFrame::LOCAL, actuation_->get_nu())); | |
| 335 | ✗ | cost_model->addCost(rmodel_.frames[id].name + "_footTrack", foot_track, | |
| 336 | 1e7); | ||
| 337 | ✗ | cost_model->addCost(rmodel_.frames[id].name + "_impulseVel", | |
| 338 | impulse_foot_vel, 1e6); | ||
| 339 | ✗ | } | |
| 340 | } | ||
| 341 | ✗ | Eigen::VectorXd state_weights(2 * rmodel_.nv); | |
| 342 | ✗ | state_weights.head<3>().fill(0.); | |
| 343 | ✗ | state_weights.segment<3>(3).fill(pow(500., 2)); | |
| 344 | ✗ | state_weights.segment(6, rmodel_.nv - 6).fill(pow(0.01, 2)); | |
| 345 | ✗ | state_weights.segment(rmodel_.nv, rmodel_.nv).fill(pow(10., 2)); | |
| 346 | std::shared_ptr<crocoddyl::ActivationModelAbstract> state_activation = | ||
| 347 | ✗ | std::make_shared<crocoddyl::ActivationModelWeightedQuad>(state_weights); | |
| 348 | std::shared_ptr<crocoddyl::CostModelAbstract> state_reg = | ||
| 349 | ✗ | std::make_shared<crocoddyl::CostModelResidual>( | |
| 350 | ✗ | state_, state_activation, | |
| 351 | ✗ | std::make_shared<crocoddyl::ResidualModelState>( | |
| 352 | ✗ | state_, defaultstate_, actuation_->get_nu())); | |
| 353 | std::shared_ptr<crocoddyl::CostModelAbstract> ctrl_reg = | ||
| 354 | ✗ | std::make_shared<crocoddyl::CostModelResidual>( | |
| 355 | ✗ | state_, std::make_shared<crocoddyl::ResidualModelControl>( | |
| 356 | ✗ | state_, actuation_->get_nu())); | |
| 357 | ✗ | cost_model->addCost("stateReg", state_reg, 1e1); | |
| 358 | ✗ | cost_model->addCost("ctrlReg", ctrl_reg, 1e-3); | |
| 359 | |||
| 360 | // Creating the action model for the KKT dynamics with simpletic Euler | ||
| 361 | // integration scheme | ||
| 362 | std::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dmodel = | ||
| 363 | ✗ | std::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>( | |
| 364 | ✗ | state_, actuation_, contact_model, cost_model); | |
| 365 | ✗ | return std::make_shared<crocoddyl::IntegratedActionModelEuler>(dmodel, 0.); | |
| 366 | ✗ | } | |
| 367 | |||
| 368 | std::shared_ptr<ActionModelAbstract> | ||
| 369 | ✗ | SimpleQuadrupedGaitProblem::createImpulseModel( | |
| 370 | const std::vector<pinocchio::FrameIndex>& support_foot_ids, | ||
| 371 | const std::vector<pinocchio::FrameIndex>& id_foot_swing_task, | ||
| 372 | const std::vector<pinocchio::SE3>& ref_foot_swing_task) { | ||
| 373 | // Creating a 3D multi-contact model, and then including the supporting foot | ||
| 374 | std::shared_ptr<crocoddyl::ImpulseModelMultiple> impulse_model = | ||
| 375 | ✗ | std::make_shared<crocoddyl::ImpulseModelMultiple>(state_); | |
| 376 | ✗ | for (std::vector<pinocchio::FrameIndex>::const_iterator it = | |
| 377 | ✗ | support_foot_ids.begin(); | |
| 378 | ✗ | it != support_foot_ids.end(); ++it) { | |
| 379 | std::shared_ptr<crocoddyl::ImpulseModelAbstract> support_contact_model = | ||
| 380 | ✗ | std::make_shared<crocoddyl::ImpulseModel3D>( | |
| 381 | ✗ | state_, *it, pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED); | |
| 382 | ✗ | impulse_model->addImpulse(rmodel_.frames[*it].name + "_impulse", | |
| 383 | support_contact_model); | ||
| 384 | ✗ | } | |
| 385 | |||
| 386 | // Creating the cost model for a contact phase | ||
| 387 | std::shared_ptr<crocoddyl::CostModelSum> cost_model = | ||
| 388 | ✗ | std::make_shared<crocoddyl::CostModelSum>(state_, 0); | |
| 389 | ✗ | if (!id_foot_swing_task.empty() && !ref_foot_swing_task.empty()) { | |
| 390 | ✗ | for (std::size_t i = 0; i < id_foot_swing_task.size(); ++i) { | |
| 391 | ✗ | const pinocchio::FrameIndex id = id_foot_swing_task[i]; | |
| 392 | std::shared_ptr<crocoddyl::CostModelAbstract> foot_track = | ||
| 393 | ✗ | std::make_shared<crocoddyl::CostModelResidual>( | |
| 394 | ✗ | state_, | |
| 395 | ✗ | std::make_shared<crocoddyl::ResidualModelFrameTranslation>( | |
| 396 | ✗ | state_, id, ref_foot_swing_task[i].translation(), 0)); | |
| 397 | ✗ | cost_model->addCost(rmodel_.frames[id].name + "_footTrack", foot_track, | |
| 398 | 1e7); | ||
| 399 | ✗ | } | |
| 400 | } | ||
| 401 | ✗ | Eigen::VectorXd state_weights(2 * rmodel_.nv); | |
| 402 | ✗ | state_weights.head<6>().fill(1.); | |
| 403 | ✗ | state_weights.segment(6, rmodel_.nv - 6).fill(pow(10., 2)); | |
| 404 | ✗ | state_weights.segment(rmodel_.nv, rmodel_.nv).fill(pow(10., 2)); | |
| 405 | std::shared_ptr<crocoddyl::ActivationModelAbstract> state_activation = | ||
| 406 | ✗ | std::make_shared<crocoddyl::ActivationModelWeightedQuad>(state_weights); | |
| 407 | std::shared_ptr<crocoddyl::CostModelAbstract> state_reg = | ||
| 408 | ✗ | std::make_shared<crocoddyl::CostModelResidual>( | |
| 409 | ✗ | state_, state_activation, | |
| 410 | ✗ | std::make_shared<crocoddyl::ResidualModelState>(state_, defaultstate_, | |
| 411 | ✗ | 0)); | |
| 412 | ✗ | cost_model->addCost("stateReg", state_reg, 1e1); | |
| 413 | |||
| 414 | // Creating the action model for the KKT dynamics with simpletic Euler | ||
| 415 | // integration scheme | ||
| 416 | ✗ | return std::make_shared<crocoddyl::ActionModelImpulseFwdDynamics>( | |
| 417 | ✗ | state_, impulse_model, cost_model); | |
| 418 | ✗ | } | |
| 419 | |||
| 420 | ✗ | const Eigen::VectorXd& SimpleQuadrupedGaitProblem::get_defaultState() const { | |
| 421 | ✗ | return defaultstate_; | |
| 422 | } | ||
| 423 | |||
| 424 | } // namespace crocoddyl | ||
| 425 |