| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh, | ||
| 5 | // University of Oxford, Heriot-Watt University | ||
| 6 | // Copyright note valid unless otherwise stated in individual files. | ||
| 7 | // All rights reserved. | ||
| 8 | /////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | namespace crocoddyl { | ||
| 11 | |||
| 12 | template <typename Scalar> | ||
| 13 | ✗ | ActionModelImpulseFwdDynamicsTpl<Scalar>::ActionModelImpulseFwdDynamicsTpl( | |
| 14 | std::shared_ptr<StateMultibody> state, | ||
| 15 | std::shared_ptr<ImpulseModelMultiple> impulses, | ||
| 16 | std::shared_ptr<CostModelSum> costs, const Scalar r_coeff, | ||
| 17 | const Scalar JMinvJt_damping, const bool enable_force) | ||
| 18 | : Base(state, 0, costs->get_nr(), 0, 0), | ||
| 19 | ✗ | impulses_(impulses), | |
| 20 | ✗ | costs_(costs), | |
| 21 | ✗ | constraints_(nullptr), | |
| 22 | ✗ | pinocchio_(state->get_pinocchio().get()), | |
| 23 | ✗ | with_armature_(true), | |
| 24 | ✗ | armature_(VectorXs::Zero(state->get_nv())), | |
| 25 | ✗ | r_coeff_(r_coeff), | |
| 26 | ✗ | JMinvJt_damping_(JMinvJt_damping), | |
| 27 | ✗ | enable_force_(enable_force), | |
| 28 | ✗ | gravity_(state->get_pinocchio()->gravity) { | |
| 29 | ✗ | init(); | |
| 30 | ✗ | } | |
| 31 | |||
| 32 | template <typename Scalar> | ||
| 33 | ✗ | ActionModelImpulseFwdDynamicsTpl<Scalar>::ActionModelImpulseFwdDynamicsTpl( | |
| 34 | std::shared_ptr<StateMultibody> state, | ||
| 35 | std::shared_ptr<ImpulseModelMultiple> impulses, | ||
| 36 | std::shared_ptr<CostModelSum> costs, | ||
| 37 | std::shared_ptr<ConstraintModelManager> constraints, const Scalar r_coeff, | ||
| 38 | const Scalar JMinvJt_damping, const bool enable_force) | ||
| 39 | : Base(state, 0, costs->get_nr(), constraints->get_ng(), | ||
| 40 | constraints->get_nh(), constraints->get_ng_T(), | ||
| 41 | constraints->get_nh_T()), | ||
| 42 | ✗ | impulses_(impulses), | |
| 43 | ✗ | costs_(costs), | |
| 44 | ✗ | constraints_(constraints), | |
| 45 | ✗ | pinocchio_(state->get_pinocchio().get()), | |
| 46 | ✗ | with_armature_(true), | |
| 47 | ✗ | armature_(VectorXs::Zero(state->get_nv())), | |
| 48 | ✗ | r_coeff_(r_coeff), | |
| 49 | ✗ | JMinvJt_damping_(JMinvJt_damping), | |
| 50 | ✗ | enable_force_(enable_force), | |
| 51 | ✗ | gravity_(state->get_pinocchio()->gravity) { | |
| 52 | ✗ | init(); | |
| 53 | ✗ | } | |
| 54 | |||
| 55 | template <typename Scalar> | ||
| 56 | ✗ | void ActionModelImpulseFwdDynamicsTpl<Scalar>::init() { | |
| 57 | ✗ | if (r_coeff_ < Scalar(0.)) { | |
| 58 | ✗ | r_coeff_ = Scalar(0.); | |
| 59 | ✗ | throw_pretty("Invalid argument: " | |
| 60 | << "The restitution coefficient has to be positive, set to 0"); | ||
| 61 | } | ||
| 62 | ✗ | if (JMinvJt_damping_ < Scalar(0.)) { | |
| 63 | ✗ | JMinvJt_damping_ = Scalar(0.); | |
| 64 | ✗ | throw_pretty("Invalid argument: " | |
| 65 | << "The damping factor has to be positive, set to 0"); | ||
| 66 | } | ||
| 67 | ✗ | } | |
| 68 | |||
| 69 | template <typename Scalar> | ||
| 70 | ✗ | void ActionModelImpulseFwdDynamicsTpl<Scalar>::calc( | |
| 71 | const std::shared_ptr<ActionDataAbstract>& data, | ||
| 72 | const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) { | ||
| 73 | ✗ | Data* d = static_cast<Data*>(data.get()); | |
| 74 | |||
| 75 | // Computing impulse dynamics and forces | ||
| 76 | ✗ | initCalc(d, x); | |
| 77 | |||
| 78 | // Computing the cost and constraints | ||
| 79 | ✗ | costs_->calc(d->costs, x, u); | |
| 80 | ✗ | d->cost = d->costs->cost; | |
| 81 | ✗ | if (constraints_ != nullptr) { | |
| 82 | ✗ | d->constraints->resize(this, d); | |
| 83 | ✗ | constraints_->calc(d->constraints, x, u); | |
| 84 | } | ||
| 85 | ✗ | } | |
| 86 | |||
| 87 | template <typename Scalar> | ||
| 88 | ✗ | void ActionModelImpulseFwdDynamicsTpl<Scalar>::calc( | |
| 89 | const std::shared_ptr<ActionDataAbstract>& data, | ||
| 90 | const Eigen::Ref<const VectorXs>& x) { | ||
| 91 | ✗ | Data* d = static_cast<Data*>(data.get()); | |
| 92 | |||
| 93 | // Computing impulse dynamics and forces | ||
| 94 | ✗ | initCalc(d, x); | |
| 95 | |||
| 96 | // Computing the cost and constraints | ||
| 97 | ✗ | costs_->calc(d->costs, x); | |
| 98 | ✗ | d->cost = d->costs->cost; | |
| 99 | ✗ | if (constraints_ != nullptr) { | |
| 100 | ✗ | d->constraints->resize(this, d, false); | |
| 101 | ✗ | constraints_->calc(d->constraints, x); | |
| 102 | } | ||
| 103 | ✗ | } | |
| 104 | |||
| 105 | template <typename Scalar> | ||
| 106 | ✗ | void ActionModelImpulseFwdDynamicsTpl<Scalar>::calcDiff( | |
| 107 | const std::shared_ptr<ActionDataAbstract>& data, | ||
| 108 | const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) { | ||
| 109 | ✗ | Data* d = static_cast<Data*>(data.get()); | |
| 110 | |||
| 111 | // Computing derivatives of impulse dynamics and forces | ||
| 112 | ✗ | initCalcDiff(d, x); | |
| 113 | |||
| 114 | // Computing derivatives of cost and constraints | ||
| 115 | ✗ | costs_->calcDiff(d->costs, x, u); | |
| 116 | ✗ | if (constraints_ != nullptr) { | |
| 117 | ✗ | constraints_->calcDiff(d->constraints, x, u); | |
| 118 | } | ||
| 119 | ✗ | } | |
| 120 | |||
| 121 | template <typename Scalar> | ||
| 122 | ✗ | void ActionModelImpulseFwdDynamicsTpl<Scalar>::calcDiff( | |
| 123 | const std::shared_ptr<ActionDataAbstract>& data, | ||
| 124 | const Eigen::Ref<const VectorXs>& x) { | ||
| 125 | ✗ | Data* d = static_cast<Data*>(data.get()); | |
| 126 | |||
| 127 | // Computing derivatives of impulse dynamics and forces | ||
| 128 | ✗ | initCalcDiff(d, x); | |
| 129 | |||
| 130 | // Computing derivatives of cost and constraints | ||
| 131 | ✗ | costs_->calcDiff(d->costs, x); | |
| 132 | ✗ | if (constraints_ != nullptr) { | |
| 133 | ✗ | constraints_->calcDiff(d->constraints, x); | |
| 134 | } | ||
| 135 | ✗ | } | |
| 136 | |||
| 137 | template <typename Scalar> | ||
| 138 | std::shared_ptr<ActionDataAbstractTpl<Scalar> > | ||
| 139 | ✗ | ActionModelImpulseFwdDynamicsTpl<Scalar>::createData() { | |
| 140 | ✗ | return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this); | |
| 141 | } | ||
| 142 | |||
| 143 | template <typename Scalar> | ||
| 144 | template <typename NewScalar> | ||
| 145 | ActionModelImpulseFwdDynamicsTpl<NewScalar> | ||
| 146 | ✗ | ActionModelImpulseFwdDynamicsTpl<Scalar>::cast() const { | |
| 147 | typedef ActionModelImpulseFwdDynamicsTpl<NewScalar> ReturnType; | ||
| 148 | typedef StateMultibodyTpl<NewScalar> StateType; | ||
| 149 | typedef ImpulseModelMultipleTpl<NewScalar> ImpulseType; | ||
| 150 | typedef CostModelSumTpl<NewScalar> CostType; | ||
| 151 | typedef ConstraintModelManagerTpl<NewScalar> ConstraintType; | ||
| 152 | ✗ | if (constraints_) { | |
| 153 | ✗ | ReturnType ret( | |
| 154 | ✗ | std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()), | |
| 155 | ✗ | std::make_shared<ImpulseType>(impulses_->template cast<NewScalar>()), | |
| 156 | ✗ | std::make_shared<CostType>(costs_->template cast<NewScalar>()), | |
| 157 | std::make_shared<ConstraintType>( | ||
| 158 | ✗ | constraints_->template cast<NewScalar>()), | |
| 159 | ✗ | scalar_cast<NewScalar>(r_coeff_), | |
| 160 | ✗ | scalar_cast<NewScalar>(JMinvJt_damping_), enable_force_); | |
| 161 | ✗ | return ret; | |
| 162 | ✗ | } else { | |
| 163 | ✗ | ReturnType ret( | |
| 164 | ✗ | std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()), | |
| 165 | ✗ | std::make_shared<ImpulseType>(impulses_->template cast<NewScalar>()), | |
| 166 | ✗ | std::make_shared<CostType>(costs_->template cast<NewScalar>()), | |
| 167 | ✗ | scalar_cast<NewScalar>(r_coeff_), | |
| 168 | ✗ | scalar_cast<NewScalar>(JMinvJt_damping_), enable_force_); | |
| 169 | ✗ | return ret; | |
| 170 | ✗ | } | |
| 171 | } | ||
| 172 | |||
| 173 | template <typename Scalar> | ||
| 174 | ✗ | bool ActionModelImpulseFwdDynamicsTpl<Scalar>::checkData( | |
| 175 | const std::shared_ptr<ActionDataAbstract>& data) { | ||
| 176 | ✗ | std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data); | |
| 177 | ✗ | if (d != NULL) { | |
| 178 | ✗ | return true; | |
| 179 | } else { | ||
| 180 | ✗ | return false; | |
| 181 | } | ||
| 182 | ✗ | } | |
| 183 | |||
| 184 | template <typename Scalar> | ||
| 185 | ✗ | void ActionModelImpulseFwdDynamicsTpl<Scalar>::quasiStatic( | |
| 186 | const std::shared_ptr<ActionDataAbstract>&, Eigen::Ref<VectorXs>, | ||
| 187 | const Eigen::Ref<const VectorXs>&, const std::size_t, const Scalar) { | ||
| 188 | // do nothing | ||
| 189 | ✗ | } | |
| 190 | |||
| 191 | template <typename Scalar> | ||
| 192 | ✗ | void ActionModelImpulseFwdDynamicsTpl<Scalar>::initCalc( | |
| 193 | Data* data, const Eigen::Ref<const VectorXs>& x) { | ||
| 194 | ✗ | if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { | |
| 195 | ✗ | throw_pretty( | |
| 196 | "Invalid argument: " << "x has wrong dimension (it should be " + | ||
| 197 | std::to_string(state_->get_nx()) + ")"); | ||
| 198 | } | ||
| 199 | |||
| 200 | ✗ | const std::size_t nq = state_->get_nq(); | |
| 201 | ✗ | const std::size_t nv = state_->get_nv(); | |
| 202 | ✗ | const std::size_t nc = impulses_->get_nc(); | |
| 203 | const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = | ||
| 204 | ✗ | x.head(nq); | |
| 205 | const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = | ||
| 206 | ✗ | x.tail(nv); | |
| 207 | |||
| 208 | // Computing the forward dynamics with the holonomic constraints defined by | ||
| 209 | // the contact model | ||
| 210 | ✗ | pinocchio::computeAllTerms(*pinocchio_, data->pinocchio, q, v); | |
| 211 | ✗ | pinocchio::computeCentroidalMomentum(*pinocchio_, data->pinocchio); | |
| 212 | |||
| 213 | ✗ | if (!with_armature_) { | |
| 214 | ✗ | data->pinocchio.M.diagonal() += armature_; | |
| 215 | } | ||
| 216 | ✗ | impulses_->calc(data->multibody.impulses, x); | |
| 217 | |||
| 218 | #ifndef NDEBUG | ||
| 219 | ✗ | Eigen::FullPivLU<MatrixXs> Jc_lu(data->multibody.impulses->Jc.topRows(nc)); | |
| 220 | |||
| 221 | ✗ | if (Jc_lu.rank() < data->multibody.impulses->Jc.topRows(nc).rows() && | |
| 222 | ✗ | JMinvJt_damping_ == Scalar(0.)) { | |
| 223 | ✗ | throw_pretty( | |
| 224 | "It is needed a damping factor since the contact Jacobian is not " | ||
| 225 | "full-rank"); | ||
| 226 | } | ||
| 227 | #endif | ||
| 228 | |||
| 229 | ✗ | pinocchio::impulseDynamics(*pinocchio_, data->pinocchio, v, | |
| 230 | ✗ | data->multibody.impulses->Jc.topRows(nc), r_coeff_, | |
| 231 | JMinvJt_damping_); | ||
| 232 | ✗ | data->xnext.head(nq) = q; | |
| 233 | ✗ | data->xnext.tail(nv) = data->pinocchio.dq_after; | |
| 234 | ✗ | impulses_->updateVelocity(data->multibody.impulses, data->pinocchio.dq_after); | |
| 235 | ✗ | impulses_->updateForce(data->multibody.impulses, data->pinocchio.impulse_c); | |
| 236 | ✗ | } | |
| 237 | |||
| 238 | template <typename Scalar> | ||
| 239 | ✗ | void ActionModelImpulseFwdDynamicsTpl<Scalar>::initCalcDiff( | |
| 240 | Data* data, const Eigen::Ref<const VectorXs>& x) { | ||
| 241 | ✗ | if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { | |
| 242 | ✗ | throw_pretty( | |
| 243 | "Invalid argument: " << "x has wrong dimension (it should be " + | ||
| 244 | std::to_string(state_->get_nx()) + ")"); | ||
| 245 | } | ||
| 246 | |||
| 247 | ✗ | const std::size_t nv = state_->get_nv(); | |
| 248 | ✗ | const std::size_t nc = impulses_->get_nc(); | |
| 249 | const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = | ||
| 250 | ✗ | x.head(state_->get_nq()); | |
| 251 | const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = | ||
| 252 | ✗ | x.tail(nv); | |
| 253 | |||
| 254 | // Computing the dynamics derivatives | ||
| 255 | // We resize the Kinv matrix because Eigen cannot call block operations | ||
| 256 | // recursively: https://eigen.tuxfamily.org/bz/show_bug.cgi?id=408. Therefore, | ||
| 257 | // it is not possible to pass d->Kinv.topLeftCorner(nv + nc, nv + nc) | ||
| 258 | ✗ | data->Kinv.resize(nv + nc, nv + nc); | |
| 259 | ✗ | pinocchio::computeRNEADerivatives(*pinocchio_, data->pinocchio, q, | |
| 260 | ✗ | data->vnone, data->pinocchio.dq_after - v, | |
| 261 | ✗ | data->multibody.impulses->fext); | |
| 262 | ✗ | pinocchio::computeGeneralizedGravityDerivatives(*pinocchio_, data->pinocchio, | |
| 263 | ✗ | q, data->dgrav_dq); | |
| 264 | ✗ | pinocchio::getKKTContactDynamicMatrixInverse( | |
| 265 | ✗ | *pinocchio_, data->pinocchio, data->multibody.impulses->Jc.topRows(nc), | |
| 266 | ✗ | data->Kinv); | |
| 267 | |||
| 268 | ✗ | pinocchio::computeForwardKinematicsDerivatives( | |
| 269 | ✗ | *pinocchio_, data->pinocchio, q, data->pinocchio.dq_after, data->vnone); | |
| 270 | ✗ | impulses_->calcDiff(data->multibody.impulses, x); | |
| 271 | ✗ | impulses_->updateRneaDiff(data->multibody.impulses, data->pinocchio); | |
| 272 | |||
| 273 | ✗ | Eigen::Block<MatrixXs> a_partial_dtau = data->Kinv.topLeftCorner(nv, nv); | |
| 274 | ✗ | Eigen::Block<MatrixXs> a_partial_da = data->Kinv.topRightCorner(nv, nc); | |
| 275 | ✗ | Eigen::Block<MatrixXs> f_partial_dtau = data->Kinv.bottomLeftCorner(nc, nv); | |
| 276 | ✗ | Eigen::Block<MatrixXs> f_partial_da = data->Kinv.bottomRightCorner(nc, nc); | |
| 277 | |||
| 278 | ✗ | data->pinocchio.dtau_dq -= data->dgrav_dq; | |
| 279 | ✗ | data->pinocchio.M.template triangularView<Eigen::StrictlyLower>() = | |
| 280 | ✗ | data->pinocchio.M.transpose() | |
| 281 | ✗ | .template triangularView<Eigen::StrictlyLower>(); | |
| 282 | ✗ | data->Fx.topLeftCorner(nv, nv).setIdentity(); | |
| 283 | ✗ | data->Fx.topRightCorner(nv, nv).setZero(); | |
| 284 | ✗ | data->Fx.bottomLeftCorner(nv, nv).noalias() = | |
| 285 | ✗ | -a_partial_dtau * data->pinocchio.dtau_dq; | |
| 286 | ✗ | data->Fx.bottomLeftCorner(nv, nv).noalias() -= | |
| 287 | ✗ | a_partial_da * data->multibody.impulses->dv0_dq.topRows(nc); | |
| 288 | ✗ | data->Fx.bottomRightCorner(nv, nv).noalias() = | |
| 289 | ✗ | a_partial_dtau * data->pinocchio.M; | |
| 290 | |||
| 291 | // Computing the cost derivatives | ||
| 292 | ✗ | if (enable_force_) { | |
| 293 | ✗ | data->df_dx.topLeftCorner(nc, nv).noalias() = | |
| 294 | ✗ | f_partial_dtau * data->pinocchio.dtau_dq; | |
| 295 | ✗ | data->df_dx.topLeftCorner(nc, nv).noalias() += | |
| 296 | ✗ | f_partial_da * data->multibody.impulses->dv0_dq.topRows(nc); | |
| 297 | ✗ | data->df_dx.topRightCorner(nc, nv).noalias() = | |
| 298 | ✗ | f_partial_da * data->multibody.impulses->Jc.topRows(nc); | |
| 299 | ✗ | impulses_->updateVelocityDiff(data->multibody.impulses, | |
| 300 | ✗ | data->Fx.bottomRows(nv)); | |
| 301 | ✗ | impulses_->updateForceDiff(data->multibody.impulses, | |
| 302 | ✗ | data->df_dx.topRows(nc)); | |
| 303 | } | ||
| 304 | ✗ | } | |
| 305 | |||
| 306 | template <typename Scalar> | ||
| 307 | ✗ | std::size_t ActionModelImpulseFwdDynamicsTpl<Scalar>::get_ng() const { | |
| 308 | ✗ | if (constraints_ != nullptr) { | |
| 309 | ✗ | return constraints_->get_ng(); | |
| 310 | } else { | ||
| 311 | ✗ | return Base::get_ng(); | |
| 312 | } | ||
| 313 | } | ||
| 314 | |||
| 315 | template <typename Scalar> | ||
| 316 | ✗ | std::size_t ActionModelImpulseFwdDynamicsTpl<Scalar>::get_nh() const { | |
| 317 | ✗ | if (constraints_ != nullptr) { | |
| 318 | ✗ | return constraints_->get_nh(); | |
| 319 | } else { | ||
| 320 | ✗ | return Base::get_nh(); | |
| 321 | } | ||
| 322 | } | ||
| 323 | |||
| 324 | template <typename Scalar> | ||
| 325 | ✗ | std::size_t ActionModelImpulseFwdDynamicsTpl<Scalar>::get_ng_T() const { | |
| 326 | ✗ | if (constraints_ != nullptr) { | |
| 327 | ✗ | return constraints_->get_ng_T(); | |
| 328 | } else { | ||
| 329 | ✗ | return Base::get_ng_T(); | |
| 330 | } | ||
| 331 | } | ||
| 332 | |||
| 333 | template <typename Scalar> | ||
| 334 | ✗ | std::size_t ActionModelImpulseFwdDynamicsTpl<Scalar>::get_nh_T() const { | |
| 335 | ✗ | if (constraints_ != nullptr) { | |
| 336 | ✗ | return constraints_->get_nh_T(); | |
| 337 | } else { | ||
| 338 | ✗ | return Base::get_nh_T(); | |
| 339 | } | ||
| 340 | } | ||
| 341 | |||
| 342 | template <typename Scalar> | ||
| 343 | const typename MathBaseTpl<Scalar>::VectorXs& | ||
| 344 | ✗ | ActionModelImpulseFwdDynamicsTpl<Scalar>::get_g_lb() const { | |
| 345 | ✗ | if (constraints_ != nullptr) { | |
| 346 | ✗ | return constraints_->get_lb(); | |
| 347 | } else { | ||
| 348 | ✗ | return g_lb_; | |
| 349 | } | ||
| 350 | } | ||
| 351 | |||
| 352 | template <typename Scalar> | ||
| 353 | const typename MathBaseTpl<Scalar>::VectorXs& | ||
| 354 | ✗ | ActionModelImpulseFwdDynamicsTpl<Scalar>::get_g_ub() const { | |
| 355 | ✗ | if (constraints_ != nullptr) { | |
| 356 | ✗ | return constraints_->get_ub(); | |
| 357 | } else { | ||
| 358 | ✗ | return g_lb_; | |
| 359 | } | ||
| 360 | } | ||
| 361 | |||
| 362 | template <typename Scalar> | ||
| 363 | ✗ | void ActionModelImpulseFwdDynamicsTpl<Scalar>::print(std::ostream& os) const { | |
| 364 | ✗ | os << "ActionModelImpulseFwdDynamics {nx=" << state_->get_nx() | |
| 365 | ✗ | << ", ndx=" << state_->get_ndx() << ", nc=" << impulses_->get_nc() << "}"; | |
| 366 | ✗ | } | |
| 367 | |||
| 368 | template <typename Scalar> | ||
| 369 | pinocchio::ModelTpl<Scalar>& | ||
| 370 | ✗ | ActionModelImpulseFwdDynamicsTpl<Scalar>::get_pinocchio() const { | |
| 371 | ✗ | return *pinocchio_; | |
| 372 | } | ||
| 373 | |||
| 374 | template <typename Scalar> | ||
| 375 | const std::shared_ptr<ImpulseModelMultipleTpl<Scalar> >& | ||
| 376 | ✗ | ActionModelImpulseFwdDynamicsTpl<Scalar>::get_impulses() const { | |
| 377 | ✗ | return impulses_; | |
| 378 | } | ||
| 379 | |||
| 380 | template <typename Scalar> | ||
| 381 | const std::shared_ptr<CostModelSumTpl<Scalar> >& | ||
| 382 | ✗ | ActionModelImpulseFwdDynamicsTpl<Scalar>::get_costs() const { | |
| 383 | ✗ | return costs_; | |
| 384 | } | ||
| 385 | |||
| 386 | template <typename Scalar> | ||
| 387 | const std::shared_ptr<ConstraintModelManagerTpl<Scalar> >& | ||
| 388 | ✗ | ActionModelImpulseFwdDynamicsTpl<Scalar>::get_constraints() const { | |
| 389 | ✗ | return constraints_; | |
| 390 | } | ||
| 391 | |||
| 392 | template <typename Scalar> | ||
| 393 | const typename MathBaseTpl<Scalar>::VectorXs& | ||
| 394 | ✗ | ActionModelImpulseFwdDynamicsTpl<Scalar>::get_armature() const { | |
| 395 | ✗ | return armature_; | |
| 396 | } | ||
| 397 | |||
| 398 | template <typename Scalar> | ||
| 399 | const Scalar | ||
| 400 | ✗ | ActionModelImpulseFwdDynamicsTpl<Scalar>::get_restitution_coefficient() const { | |
| 401 | ✗ | return r_coeff_; | |
| 402 | } | ||
| 403 | |||
| 404 | template <typename Scalar> | ||
| 405 | ✗ | const Scalar ActionModelImpulseFwdDynamicsTpl<Scalar>::get_damping_factor() | |
| 406 | const { | ||
| 407 | ✗ | return JMinvJt_damping_; | |
| 408 | } | ||
| 409 | |||
| 410 | template <typename Scalar> | ||
| 411 | ✗ | void ActionModelImpulseFwdDynamicsTpl<Scalar>::set_armature( | |
| 412 | const VectorXs& armature) { | ||
| 413 | ✗ | if (static_cast<std::size_t>(armature.size()) != state_->get_nv()) { | |
| 414 | ✗ | throw_pretty("Invalid argument: " | |
| 415 | << "The armature dimension is wrong (it should be " + | ||
| 416 | std::to_string(state_->get_nv()) + ")"); | ||
| 417 | } | ||
| 418 | ✗ | armature_ = armature; | |
| 419 | ✗ | with_armature_ = false; | |
| 420 | ✗ | } | |
| 421 | |||
| 422 | template <typename Scalar> | ||
| 423 | ✗ | void ActionModelImpulseFwdDynamicsTpl<Scalar>::set_restitution_coefficient( | |
| 424 | const Scalar r_coeff) { | ||
| 425 | ✗ | if (r_coeff < 0.) { | |
| 426 | ✗ | throw_pretty("Invalid argument: " | |
| 427 | << "The restitution coefficient has to be positive"); | ||
| 428 | } | ||
| 429 | ✗ | r_coeff_ = r_coeff; | |
| 430 | ✗ | } | |
| 431 | |||
| 432 | template <typename Scalar> | ||
| 433 | ✗ | void ActionModelImpulseFwdDynamicsTpl<Scalar>::set_damping_factor( | |
| 434 | const Scalar damping) { | ||
| 435 | ✗ | if (damping < 0.) { | |
| 436 | ✗ | throw_pretty( | |
| 437 | "Invalid argument: " << "The damping factor has to be positive"); | ||
| 438 | } | ||
| 439 | ✗ | JMinvJt_damping_ = damping; | |
| 440 | ✗ | } | |
| 441 | |||
| 442 | } // namespace crocoddyl | ||
| 443 |