| Directory: | ./ |
|---|---|
| File: | include/crocoddyl/multibody/actions/contact-invdyn.hpp |
| Date: | 2025-01-30 11:01:55 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 107 | 128 | 83.6% |
| Branches: | 66 | 136 | 48.5% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2021-2024, Heriot-Watt University, University of Edinburgh | ||
| 5 | // Copyright note valid unless otherwise stated in individual files. | ||
| 6 | // All rights reserved. | ||
| 7 | /////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | #ifndef CROCODDYL_MULTIBODY_ACTIONS_CONTACT_INVDYN_HPP_ | ||
| 10 | #define CROCODDYL_MULTIBODY_ACTIONS_CONTACT_INVDYN_HPP_ | ||
| 11 | |||
| 12 | #include "crocoddyl/core/actuation-base.hpp" | ||
| 13 | #include "crocoddyl/core/constraints/constraint-manager.hpp" | ||
| 14 | #include "crocoddyl/core/costs/cost-sum.hpp" | ||
| 15 | #include "crocoddyl/core/diff-action-base.hpp" | ||
| 16 | #include "crocoddyl/multibody/contacts/multiple-contacts.hpp" | ||
| 17 | #include "crocoddyl/multibody/data/contacts.hpp" | ||
| 18 | #include "crocoddyl/multibody/fwd.hpp" | ||
| 19 | #include "crocoddyl/multibody/states/multibody.hpp" | ||
| 20 | |||
| 21 | namespace crocoddyl { | ||
| 22 | |||
| 23 | /** | ||
| 24 | * @brief Differential action model for contact inverse dynamics in multibody | ||
| 25 | * systems. | ||
| 26 | * | ||
| 27 | * This class implements forward kinematic with holonomic contact constraints | ||
| 28 | * (defined at the acceleration level) and inverse-dynamics computation using | ||
| 29 | * the Recursive Newton Euler Algorithm (RNEA). The stack of cost and constraint | ||
| 30 | * functions are implemented in `CostModelSumTpl` and | ||
| 31 | * `ConstraintModelManagerTpl`, respectively. The acceleration and contact | ||
| 32 | * forces are decision variables defined as the control inputs, and the | ||
| 33 | * under-actuation and contact constraint are under the name `tau` and its frame | ||
| 34 | * name, thus the user is not allowed to use it. | ||
| 35 | * | ||
| 36 | * Additionally, it is important to note that `calcDiff()` computes the | ||
| 37 | * derivatives using the latest stored values by `calc()`. Thus, we need to | ||
| 38 | * first run `calc()`. | ||
| 39 | * | ||
| 40 | * \sa `DifferentialActionModelAbstractTpl`, `calc()`, `calcDiff()`, | ||
| 41 | * `createData()` | ||
| 42 | */ | ||
| 43 | template <typename _Scalar> | ||
| 44 | class DifferentialActionModelContactInvDynamicsTpl | ||
| 45 | : public DifferentialActionModelAbstractTpl<_Scalar> { | ||
| 46 | public: | ||
| 47 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 48 | |||
| 49 | typedef _Scalar Scalar; | ||
| 50 | typedef DifferentialActionModelAbstractTpl<Scalar> Base; | ||
| 51 | typedef DifferentialActionDataContactInvDynamicsTpl<Scalar> Data; | ||
| 52 | typedef StateMultibodyTpl<Scalar> StateMultibody; | ||
| 53 | typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract; | ||
| 54 | typedef CostModelSumTpl<Scalar> CostModelSum; | ||
| 55 | typedef ConstraintModelManagerTpl<Scalar> ConstraintModelManager; | ||
| 56 | typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple; | ||
| 57 | typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract; | ||
| 58 | typedef DifferentialActionDataAbstractTpl<Scalar> | ||
| 59 | DifferentialActionDataAbstract; | ||
| 60 | typedef ContactItemTpl<Scalar> ContactItem; | ||
| 61 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 62 | typedef typename MathBase::VectorXs VectorXs; | ||
| 63 | typedef typename MathBase::MatrixXs MatrixXs; | ||
| 64 | |||
| 65 | /** | ||
| 66 | * @brief Initialize the contact inverse-dynamics action model | ||
| 67 | * | ||
| 68 | * It describes the kinematic evolution of the multibody system with contacts, | ||
| 69 | * and computes the needed torques using inverse-dynamics. | ||
| 70 | * | ||
| 71 | * @param[in] state State of the multibody system | ||
| 72 | * @param[in] actuation Actuation model | ||
| 73 | * @param[in] contacts Multiple contacts | ||
| 74 | * @param[in] costs Cost model | ||
| 75 | */ | ||
| 76 | DifferentialActionModelContactInvDynamicsTpl( | ||
| 77 | std::shared_ptr<StateMultibody> state, | ||
| 78 | std::shared_ptr<ActuationModelAbstract> actuation, | ||
| 79 | std::shared_ptr<ContactModelMultiple> contacts, | ||
| 80 | std::shared_ptr<CostModelSum> costs); | ||
| 81 | |||
| 82 | /** | ||
| 83 | * @brief Initialize the contact inverse-dynamics action model | ||
| 84 | * | ||
| 85 | * @param[in] state State of the multibody system | ||
| 86 | * @param[in] actuation Actuation model | ||
| 87 | * @param[in] contacts Multiple contacts | ||
| 88 | * @param[in] costs Cost model | ||
| 89 | * @param[in] constraints Constraints model | ||
| 90 | */ | ||
| 91 | DifferentialActionModelContactInvDynamicsTpl( | ||
| 92 | std::shared_ptr<StateMultibody> state, | ||
| 93 | std::shared_ptr<ActuationModelAbstract> actuation, | ||
| 94 | std::shared_ptr<ContactModelMultiple> contacts, | ||
| 95 | std::shared_ptr<CostModelSum> costs, | ||
| 96 | std::shared_ptr<ConstraintModelManager> constraints); | ||
| 97 | virtual ~DifferentialActionModelContactInvDynamicsTpl(); | ||
| 98 | |||
| 99 | /** | ||
| 100 | * @brief Compute the system acceleration, cost value and constraint residuals | ||
| 101 | * | ||
| 102 | * It extracts the acceleration value from control vector and also computes | ||
| 103 | * the cost and constraints. | ||
| 104 | * | ||
| 105 | * @param[in] data Contact inverse-dynamics data | ||
| 106 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 107 | * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ | ||
| 108 | */ | ||
| 109 | virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data, | ||
| 110 | const Eigen::Ref<const VectorXs>& x, | ||
| 111 | const Eigen::Ref<const VectorXs>& u); | ||
| 112 | |||
| 113 | /** | ||
| 114 | * @brief @copydoc Base::calc(const | ||
| 115 | * std::shared_ptr<DifferentialActionDataAbstract>& data, const | ||
| 116 | * Eigen::Ref<const VectorXs>& x) | ||
| 117 | */ | ||
| 118 | virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data, | ||
| 119 | const Eigen::Ref<const VectorXs>& x); | ||
| 120 | |||
| 121 | /** | ||
| 122 | * @brief Compute the derivatives of the dynamics, cost and constraint | ||
| 123 | * functions | ||
| 124 | * | ||
| 125 | * It computes the partial derivatives of the dynamical system and the cost | ||
| 126 | * and contraint functions. It assumes that `calc()` has been run first. This | ||
| 127 | * function builds a quadratic approximation of the time-continuous action | ||
| 128 | * model (i.e., dynamical system, cost and constraint functions). | ||
| 129 | * | ||
| 130 | * @param[in] data Contact inverse-dynamics data | ||
| 131 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 132 | * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ | ||
| 133 | */ | ||
| 134 | virtual void calcDiff( | ||
| 135 | const std::shared_ptr<DifferentialActionDataAbstract>& data, | ||
| 136 | const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u); | ||
| 137 | |||
| 138 | /** | ||
| 139 | * @brief @copydoc Base::calcDiff(const | ||
| 140 | * std::shared_ptr<DifferentialActionDataAbstract>& data, const | ||
| 141 | * Eigen::Ref<const VectorXs>& x) | ||
| 142 | */ | ||
| 143 | virtual void calcDiff( | ||
| 144 | const std::shared_ptr<DifferentialActionDataAbstract>& data, | ||
| 145 | const Eigen::Ref<const VectorXs>& x); | ||
| 146 | |||
| 147 | /** | ||
| 148 | * @brief Create the contact inverse-dynamics data | ||
| 149 | * | ||
| 150 | * @return contact inverse-dynamics data | ||
| 151 | */ | ||
| 152 | virtual std::shared_ptr<DifferentialActionDataAbstract> createData(); | ||
| 153 | |||
| 154 | /** | ||
| 155 | * @brief Checks that a specific data belongs to the contact inverse-dynamics | ||
| 156 | * model | ||
| 157 | */ | ||
| 158 | virtual bool checkData( | ||
| 159 | const std::shared_ptr<DifferentialActionDataAbstract>& data); | ||
| 160 | |||
| 161 | /** | ||
| 162 | * @brief Computes the quasic static commands | ||
| 163 | * | ||
| 164 | * The quasic static commands are the ones produced for a reference posture as | ||
| 165 | * an equilibrium point with zero acceleration, i.e., for | ||
| 166 | * \f$\mathbf{f^q_x}\delta\mathbf{q}+\mathbf{f_u}\delta\mathbf{u}=\mathbf{0}\f$ | ||
| 167 | * | ||
| 168 | * @param[in] data Contact inverse-dynamics data | ||
| 169 | * @param[out] u Quasic-static commands | ||
| 170 | * @param[in] x State point (velocity has to be zero) | ||
| 171 | * @param[in] maxiter Maximum allowed number of iterations (default 100) | ||
| 172 | * @param[in] tol Tolerance (default 1e-9) | ||
| 173 | */ | ||
| 174 | virtual void quasiStatic( | ||
| 175 | const std::shared_ptr<DifferentialActionDataAbstract>& data, | ||
| 176 | Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x, | ||
| 177 | const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9)); | ||
| 178 | |||
| 179 | /** | ||
| 180 | * @brief Return the number of inequality constraints | ||
| 181 | */ | ||
| 182 | virtual std::size_t get_ng() const; | ||
| 183 | |||
| 184 | /** | ||
| 185 | * @brief Return the number of equality constraints | ||
| 186 | */ | ||
| 187 | virtual std::size_t get_nh() const; | ||
| 188 | |||
| 189 | /** | ||
| 190 | * @brief Return the number of equality terminal constraints | ||
| 191 | */ | ||
| 192 | virtual std::size_t get_ng_T() const; | ||
| 193 | |||
| 194 | /** | ||
| 195 | * @brief Return the number of equality terminal constraints | ||
| 196 | */ | ||
| 197 | virtual std::size_t get_nh_T() const; | ||
| 198 | |||
| 199 | /** | ||
| 200 | * @brief Return the lower bound of the inequality constraints | ||
| 201 | */ | ||
| 202 | virtual const VectorXs& get_g_lb() const; | ||
| 203 | |||
| 204 | /** | ||
| 205 | * @brief Return the upper bound of the inequality constraints | ||
| 206 | */ | ||
| 207 | virtual const VectorXs& get_g_ub() const; | ||
| 208 | |||
| 209 | /** | ||
| 210 | * @brief Return the actuation model | ||
| 211 | */ | ||
| 212 | const std::shared_ptr<ActuationModelAbstract>& get_actuation() const; | ||
| 213 | |||
| 214 | /** | ||
| 215 | * @brief Return the contact model | ||
| 216 | */ | ||
| 217 | const std::shared_ptr<ContactModelMultiple>& get_contacts() const; | ||
| 218 | |||
| 219 | /** | ||
| 220 | * @brief Return the cost model | ||
| 221 | */ | ||
| 222 | const std::shared_ptr<CostModelSum>& get_costs() const; | ||
| 223 | |||
| 224 | /** | ||
| 225 | * @brief Return the constraint model manager | ||
| 226 | */ | ||
| 227 | const std::shared_ptr<ConstraintModelManager>& get_constraints() const; | ||
| 228 | |||
| 229 | /** | ||
| 230 | * @brief Return the Pinocchio model | ||
| 231 | */ | ||
| 232 | pinocchio::ModelTpl<Scalar>& get_pinocchio() const; | ||
| 233 | |||
| 234 | /** | ||
| 235 | * @brief Print relevant information of the contact inverse-dynamics model | ||
| 236 | * @param[out] os Output stream object | ||
| 237 | */ | ||
| 238 | virtual void print(std::ostream& os) const; | ||
| 239 | |||
| 240 | protected: | ||
| 241 | using Base::g_lb_; //!< Lower bound of the inequality constraints | ||
| 242 | using Base::g_ub_; //!< Upper bound of the inequality constraints | ||
| 243 | using Base::ng_; //!< Number of inequality constraints | ||
| 244 | using Base::nh_; //!< Number of equality constraints | ||
| 245 | using Base::nu_; //!< Control dimension | ||
| 246 | using Base::state_; //!< Model of the state | ||
| 247 | |||
| 248 | private: | ||
| 249 | void init(const std::shared_ptr<StateMultibody>& state); | ||
| 250 | std::shared_ptr<ActuationModelAbstract> actuation_; //!< Actuation model | ||
| 251 | std::shared_ptr<ContactModelMultiple> contacts_; //!< Contact model | ||
| 252 | std::shared_ptr<CostModelSum> costs_; //!< Cost model | ||
| 253 | std::shared_ptr<ConstraintModelManager> constraints_; //!< Constraint model | ||
| 254 | pinocchio::ModelTpl<Scalar>& pinocchio_; //!< Pinocchio model | ||
| 255 | |||
| 256 | public: | ||
| 257 | /** | ||
| 258 | * @brief Actuation residual | ||
| 259 | * | ||
| 260 | * This residual function enforces the torques of under-actuated joints (e.g., | ||
| 261 | * floating-base joints) to be zero. We compute these torques and their | ||
| 262 | * derivatives using RNEA inside | ||
| 263 | * `DifferentialActionModelContactInvDynamicsTpl`. | ||
| 264 | * | ||
| 265 | * As described in `ResidualModelAbstractTpl`, the residual value and its | ||
| 266 | * Jacobians are calculated by `calc` and `calcDiff`, respectively. | ||
| 267 | * | ||
| 268 | * \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()` | ||
| 269 | */ | ||
| 270 | class ResidualModelActuation : public ResidualModelAbstractTpl<_Scalar> { | ||
| 271 | public: | ||
| 272 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 273 | |||
| 274 | typedef _Scalar Scalar; | ||
| 275 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 276 | typedef ResidualModelAbstractTpl<Scalar> Base; | ||
| 277 | typedef StateMultibodyTpl<Scalar> StateMultibody; | ||
| 278 | typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract; | ||
| 279 | typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract; | ||
| 280 | typedef typename MathBase::VectorXs VectorXs; | ||
| 281 | |||
| 282 | /** | ||
| 283 | * @brief Initialize the actuation residual model | ||
| 284 | * | ||
| 285 | * @param[in] state State of the multibody system | ||
| 286 | * @param[in] nu Dimension of the joint torques | ||
| 287 | * @param[in] nc Dimension of all the contacts | ||
| 288 | */ | ||
| 289 | 251 | ResidualModelActuation(std::shared_ptr<StateMultibody> state, | |
| 290 | const std::size_t nu, const std::size_t nc) | ||
| 291 | 502 | : Base(state, state->get_nv() - nu, state->get_nv() + nc, true, true, | |
| 292 | true), | ||
| 293 | 251 | na_(nu), | |
| 294 |
1/2✓ Branch 4 taken 251 times.
✗ Branch 5 not taken.
|
753 | nc_(nc) {} |
| 295 | 502 | virtual ~ResidualModelActuation() {} | |
| 296 | |||
| 297 | /** | ||
| 298 | * @brief Compute the actuation residual | ||
| 299 | * | ||
| 300 | * @param[in] data Actuation residual data | ||
| 301 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 302 | * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nv+nu}\f$ | ||
| 303 | */ | ||
| 304 | 23365 | virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data, | |
| 305 | const Eigen::Ref<const VectorXs>&, | ||
| 306 | const Eigen::Ref<const VectorXs>&) { | ||
| 307 | typename Data::ResidualDataActuation* d = | ||
| 308 | 23365 | static_cast<typename Data::ResidualDataActuation*>(data.get()); | |
| 309 | // Update the under-actuation set and residual | ||
| 310 | 23365 | std::size_t nrow = 0; | |
| 311 | 665320 | for (std::size_t k = 0; | |
| 312 |
2/2✓ Branch 2 taken 641955 times.
✓ Branch 3 taken 23365 times.
|
665320 | k < static_cast<std::size_t>(d->actuation->tau_set.size()); ++k) { |
| 313 |
2/2✓ Branch 3 taken 127365 times.
✓ Branch 4 taken 514590 times.
|
641955 | if (!d->actuation->tau_set[k]) { |
| 314 | 127365 | data->r(nrow) = d->pinocchio->tau(k); | |
| 315 | 127365 | nrow += 1; | |
| 316 | } | ||
| 317 | } | ||
| 318 | 23365 | } | |
| 319 | |||
| 320 | /** | ||
| 321 | * @brief @copydoc Base::calc(const std::shared_ptr<ResidualDataAbstract>& | ||
| 322 | * data, const Eigen::Ref<const VectorXs>& x) | ||
| 323 | */ | ||
| 324 | ✗ | virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data, | |
| 325 | const Eigen::Ref<const VectorXs>&) { | ||
| 326 | ✗ | data->r.setZero(); | |
| 327 | } | ||
| 328 | |||
| 329 | /** | ||
| 330 | * @brief Compute the derivatives of the actuation residual | ||
| 331 | * | ||
| 332 | * @param[in] data Actuation residual data | ||
| 333 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 334 | * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ | ||
| 335 | */ | ||
| 336 | 3135 | virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data, | |
| 337 | const Eigen::Ref<const VectorXs>&, | ||
| 338 | const Eigen::Ref<const VectorXs>&) { | ||
| 339 | typename Data::ResidualDataActuation* d = | ||
| 340 | 3135 | static_cast<typename Data::ResidualDataActuation*>(data.get()); | |
| 341 | 3135 | std::size_t nrow = 0; | |
| 342 | 3135 | const std::size_t nv = state_->get_nv(); | |
| 343 |
1/2✓ Branch 2 taken 3135 times.
✗ Branch 3 not taken.
|
3135 | d->dtau_dx.leftCols(nv) = d->pinocchio->dtau_dq; |
| 344 |
1/2✓ Branch 2 taken 3135 times.
✗ Branch 3 not taken.
|
3135 | d->dtau_dx.rightCols(nv) = d->pinocchio->dtau_dv; |
| 345 | 3135 | d->dtau_dx -= d->actuation->dtau_dx; | |
| 346 |
1/2✓ Branch 2 taken 3135 times.
✗ Branch 3 not taken.
|
3135 | d->dtau_du.leftCols(nv) = d->pinocchio->M; |
| 347 |
3/6✓ Branch 3 taken 3135 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3135 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3135 times.
✗ Branch 10 not taken.
|
3135 | d->dtau_du.rightCols(nc_) = -d->contact->Jc.transpose(); |
| 348 | 77748 | for (std::size_t k = 0; | |
| 349 |
2/2✓ Branch 2 taken 74613 times.
✓ Branch 3 taken 3135 times.
|
77748 | k < static_cast<std::size_t>(d->actuation->tau_set.size()); ++k) { |
| 350 |
2/2✓ Branch 3 taken 15675 times.
✓ Branch 4 taken 58938 times.
|
74613 | if (!d->actuation->tau_set[k]) { |
| 351 |
2/4✓ Branch 2 taken 15675 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15675 times.
✗ Branch 6 not taken.
|
15675 | d->Rx.row(nrow) = d->dtau_dx.row(k); |
| 352 |
2/4✓ Branch 2 taken 15675 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15675 times.
✗ Branch 6 not taken.
|
15675 | d->Ru.row(nrow) = d->dtau_du.row(k); |
| 353 | 15675 | nrow += 1; | |
| 354 | } | ||
| 355 | } | ||
| 356 | 3135 | } | |
| 357 | |||
| 358 | /** | ||
| 359 | * @brief @copydoc Base::calcDiff(const | ||
| 360 | * std::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const | ||
| 361 | * VectorXs>& x) | ||
| 362 | */ | ||
| 363 | ✗ | virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data, | |
| 364 | const Eigen::Ref<const VectorXs>&) { | ||
| 365 | ✗ | data->Rx.setZero(); | |
| 366 | ✗ | data->Ru.setZero(); | |
| 367 | } | ||
| 368 | |||
| 369 | /** | ||
| 370 | * @brief Create the actuation residual data | ||
| 371 | * | ||
| 372 | * @return Actuation residual data | ||
| 373 | */ | ||
| 374 | 27948 | virtual std::shared_ptr<ResidualDataAbstract> createData( | |
| 375 | DataCollectorAbstract* const data) { | ||
| 376 | return std::allocate_shared<typename Data::ResidualDataActuation>( | ||
| 377 | ✗ | Eigen::aligned_allocator<typename Data::ResidualDataActuation>(), | |
| 378 |
1/2✓ Branch 2 taken 27948 times.
✗ Branch 3 not taken.
|
27948 | this, data); |
| 379 | } | ||
| 380 | |||
| 381 | /** | ||
| 382 | * @brief Print relevant information of the actuation residual model | ||
| 383 | * | ||
| 384 | * @param[out] os Output stream object | ||
| 385 | */ | ||
| 386 | ✗ | virtual void print(std::ostream& os) const { | |
| 387 | ✗ | os << "ResidualModelActuation {nx=" << state_->get_nx() | |
| 388 | ✗ | << ", ndx=" << state_->get_ndx() << ", nu=" << nu_ << ", na=" << na_ | |
| 389 | ✗ | << "}"; | |
| 390 | } | ||
| 391 | |||
| 392 | protected: | ||
| 393 | using Base::nu_; | ||
| 394 | using Base::state_; | ||
| 395 | |||
| 396 | private: | ||
| 397 | std::size_t na_; //!< Number of actuated joints | ||
| 398 | std::size_t nc_; //!< Number of contacts | ||
| 399 | }; | ||
| 400 | |||
| 401 | public: | ||
| 402 | /** | ||
| 403 | * @brief Contact-acceleration residual | ||
| 404 | * | ||
| 405 | * This residual function is defined as \f$\mathbf{r} = \mathbf{a_0}\f$, where | ||
| 406 | * \f$\mathbf{a_0}\f$ defines the desired contact acceleration, which might | ||
| 407 | * also include the Baumgarte stabilization gains. Furthermore, the Jacobians | ||
| 408 | * of the residual function are computed analytically. This is used by | ||
| 409 | * `ConstraintModelManagerTpl` inside parent | ||
| 410 | * `DifferentialActionModelContactInvDynamicsTpl` class. | ||
| 411 | * | ||
| 412 | * As described in `ResidualModelAbstractTpl`, the residual value and its | ||
| 413 | * Jacobians are calculated by `calc` and `calcDiff`, respectively. | ||
| 414 | * | ||
| 415 | * \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()` | ||
| 416 | */ | ||
| 417 | class ResidualModelContact : public ResidualModelAbstractTpl<_Scalar> { | ||
| 418 | public: | ||
| 419 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 420 | |||
| 421 | typedef _Scalar Scalar; | ||
| 422 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 423 | typedef ResidualModelAbstractTpl<Scalar> Base; | ||
| 424 | typedef StateMultibodyTpl<Scalar> StateMultibody; | ||
| 425 | typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract; | ||
| 426 | typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract; | ||
| 427 | typedef typename MathBase::VectorXs VectorXs; | ||
| 428 | typedef typename MathBase::MatrixXs MatrixXs; | ||
| 429 | |||
| 430 | /** | ||
| 431 | * @brief Initialize the contact-acceleration residual model | ||
| 432 | * | ||
| 433 | * @param[in] state State of the multibody system | ||
| 434 | * @param[in] id Contact frame id | ||
| 435 | * @param[in] nr Dimension of the contact-acceleration residual | ||
| 436 | * @param[in] nc Dimension of all contacts | ||
| 437 | */ | ||
| 438 | 700 | ResidualModelContact(std::shared_ptr<StateMultibody> state, | |
| 439 | const pinocchio::FrameIndex id, const std::size_t nr, | ||
| 440 | const std::size_t nc) | ||
| 441 | 700 | : Base(state, nr, state->get_nv() + nc, true, true, true), | |
| 442 | 700 | id_(id), | |
| 443 |
2/4✓ Branch 3 taken 700 times.
✗ Branch 4 not taken.
✓ Branch 11 taken 700 times.
✗ Branch 12 not taken.
|
1400 | frame_name_(state->get_pinocchio()->frames[id].name) {} |
| 444 | 1400 | virtual ~ResidualModelContact() {} | |
| 445 | |||
| 446 | /** | ||
| 447 | * @brief Compute the contact-acceleration residual | ||
| 448 | * | ||
| 449 | * @param[in] data Contact-acceleration residual data | ||
| 450 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 451 | * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nv+nu}\f$ | ||
| 452 | */ | ||
| 453 | 63366 | void calc(const std::shared_ptr<ResidualDataAbstract>& data, | |
| 454 | const Eigen::Ref<const VectorXs>&, | ||
| 455 | const Eigen::Ref<const VectorXs>&) { | ||
| 456 | typename Data::ResidualDataContact* d = | ||
| 457 | 63366 | static_cast<typename Data::ResidualDataContact*>(data.get()); | |
| 458 | 63366 | d->r = d->contact->a0; | |
| 459 | 63366 | } | |
| 460 | |||
| 461 | /** | ||
| 462 | * @brief @copydoc Base::calc(const std::shared_ptr<ResidualDataAbstract>& | ||
| 463 | * data, const Eigen::Ref<const VectorXs>& x) | ||
| 464 | */ | ||
| 465 | ✗ | virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data, | |
| 466 | const Eigen::Ref<const VectorXs>&) { | ||
| 467 | ✗ | data->r.setZero(); | |
| 468 | } | ||
| 469 | |||
| 470 | /** | ||
| 471 | * @brief Compute the derivatives of the contact-acceleration residual | ||
| 472 | * | ||
| 473 | * @param[in] data Contact-acceleration residual data | ||
| 474 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 475 | * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ | ||
| 476 | */ | ||
| 477 | 8778 | void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data, | |
| 478 | const Eigen::Ref<const VectorXs>&, | ||
| 479 | const Eigen::Ref<const VectorXs>&) { | ||
| 480 | typename Data::ResidualDataContact* d = | ||
| 481 | 8778 | static_cast<typename Data::ResidualDataContact*>(data.get()); | |
| 482 | 8778 | d->Rx = d->contact->da0_dx; | |
| 483 |
1/2✓ Branch 4 taken 8778 times.
✗ Branch 5 not taken.
|
8778 | d->Ru.leftCols(state_->get_nv()) = d->contact->Jc; |
| 484 | 8778 | } | |
| 485 | |||
| 486 | /** | ||
| 487 | * @brief @copydoc Base::calcDiff(const | ||
| 488 | * std::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const | ||
| 489 | * VectorXs>& x) | ||
| 490 | */ | ||
| 491 | ✗ | virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data, | |
| 492 | const Eigen::Ref<const VectorXs>&) { | ||
| 493 | ✗ | data->Rx.setZero(); | |
| 494 | ✗ | data->Ru.setZero(); | |
| 495 | } | ||
| 496 | |||
| 497 | /** | ||
| 498 | * @brief Create the contact-acceleration residual data | ||
| 499 | * | ||
| 500 | * @return contact-acceleration residual data | ||
| 501 | */ | ||
| 502 | 76196 | virtual std::shared_ptr<ResidualDataAbstract> createData( | |
| 503 | DataCollectorAbstract* const data) { | ||
| 504 | return std::allocate_shared<typename Data::ResidualDataContact>( | ||
| 505 | ✗ | Eigen::aligned_allocator<typename Data::ResidualDataContact>(), this, | |
| 506 |
1/2✓ Branch 2 taken 76196 times.
✗ Branch 3 not taken.
|
76196 | data, id_); |
| 507 | } | ||
| 508 | |||
| 509 | /** | ||
| 510 | * @brief Print relevant information of the contact-acceleration residual | ||
| 511 | * model | ||
| 512 | * | ||
| 513 | * @param[out] os Output stream object | ||
| 514 | */ | ||
| 515 | ✗ | virtual void print(std::ostream& os) const { | |
| 516 | ✗ | os << "ResidualModelContact {frame=" << frame_name_ << ", nr=" << nr_ | |
| 517 | ✗ | << "}"; | |
| 518 | } | ||
| 519 | |||
| 520 | protected: | ||
| 521 | using Base::nr_; | ||
| 522 | using Base::nu_; | ||
| 523 | using Base::state_; | ||
| 524 | |||
| 525 | private: | ||
| 526 | pinocchio::FrameIndex id_; //!< Reference frame id | ||
| 527 | std::string frame_name_; //!< Reference frame name | ||
| 528 | }; | ||
| 529 | }; | ||
| 530 | template <typename _Scalar> | ||
| 531 | struct DifferentialActionDataContactInvDynamicsTpl | ||
| 532 | : public DifferentialActionDataAbstractTpl<_Scalar> { | ||
| 533 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 534 | typedef _Scalar Scalar; | ||
| 535 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 536 | typedef DifferentialActionDataAbstractTpl<Scalar> Base; | ||
| 537 | typedef JointDataAbstractTpl<Scalar> JointDataAbstract; | ||
| 538 | typedef DataCollectorJointActMultibodyInContactTpl<Scalar> | ||
| 539 | DataCollectorJointActMultibodyInContact; | ||
| 540 | typedef CostDataSumTpl<Scalar> CostDataSum; | ||
| 541 | typedef ConstraintDataManagerTpl<Scalar> ConstraintDataManager; | ||
| 542 | typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple; | ||
| 543 | typedef typename MathBase::VectorXs VectorXs; | ||
| 544 | typedef typename MathBase::MatrixXs MatrixXs; | ||
| 545 | |||
| 546 | template <template <typename Scalar> class Model> | ||
| 547 | 31430 | explicit DifferentialActionDataContactInvDynamicsTpl( | |
| 548 | Model<Scalar>* const model) | ||
| 549 | : Base(model), | ||
| 550 |
1/2✓ Branch 1 taken 31430 times.
✗ Branch 2 not taken.
|
31430 | pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())), |
| 551 |
3/6✓ Branch 2 taken 31430 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 31430 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 31430 times.
✗ Branch 10 not taken.
|
94290 | multibody( |
| 552 |
1/2✓ Branch 3 taken 31430 times.
✗ Branch 4 not taken.
|
31430 | &pinocchio, model->get_actuation()->createData(), |
| 553 | std::make_shared<JointDataAbstract>( | ||
| 554 | 62860 | model->get_state(), model->get_actuation(), model->get_nu()), | |
| 555 | 31430 | model->get_contacts()->createData(&pinocchio)), | |
| 556 |
1/2✓ Branch 4 taken 31430 times.
✗ Branch 5 not taken.
|
31430 | tmp_xstatic(model->get_state()->get_nx()), |
| 557 |
1/2✓ Branch 4 taken 31430 times.
✗ Branch 5 not taken.
|
62860 | tmp_rstatic(model->get_actuation()->get_nu() + |
| 558 | 31430 | model->get_contacts()->get_nc()), | |
| 559 |
1/2✓ Branch 3 taken 31430 times.
✗ Branch 4 not taken.
|
31430 | tmp_Jstatic(model->get_state()->get_nv(), |
| 560 | 62860 | model->get_actuation()->get_nu() + | |
| 561 | 94290 | model->get_contacts()->get_nc()) { | |
| 562 | // Set constant values for Fu, df_dx, and df_du | ||
| 563 | 31430 | const std::size_t nv = model->get_state()->get_nv(); | |
| 564 | 31430 | const std::size_t nc = model->get_contacts()->get_nc_total(); | |
| 565 |
3/6✓ Branch 1 taken 31430 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31430 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31430 times.
✗ Branch 8 not taken.
|
31430 | Fu.leftCols(nv).diagonal().setOnes(); |
| 566 |
3/6✓ Branch 2 taken 31430 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31430 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31430 times.
✗ Branch 9 not taken.
|
31430 | multibody.joint->da_du.leftCols(nv).diagonal().setOnes(); |
| 567 |
2/4✓ Branch 4 taken 31430 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31430 times.
✗ Branch 8 not taken.
|
31430 | MatrixXs df_dx = MatrixXs::Zero(nc, model->get_state()->get_ndx()); |
| 568 |
2/4✓ Branch 2 taken 31430 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31430 times.
✗ Branch 6 not taken.
|
31430 | MatrixXs df_du = MatrixXs::Zero(nc, model->get_nu()); |
| 569 | 31430 | std::size_t fid = 0; | |
| 570 | 31430 | for (typename ContactModelMultiple::ContactModelContainer::const_iterator | |
| 571 | 31430 | it = model->get_contacts()->get_contacts().begin(); | |
| 572 |
2/2✓ Branch 5 taken 76196 times.
✓ Branch 6 taken 31430 times.
|
107626 | it != model->get_contacts()->get_contacts().end(); ++it) { |
| 573 | 76196 | const std::size_t nc = it->second->contact->get_nc(); | |
| 574 |
3/6✓ Branch 1 taken 76196 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76196 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 76196 times.
✗ Branch 8 not taken.
|
76196 | df_du.block(fid, nv + fid, nc, nc).diagonal().setOnes(); |
| 575 | 76196 | fid += nc; | |
| 576 | } | ||
| 577 |
1/2✓ Branch 3 taken 31430 times.
✗ Branch 4 not taken.
|
31430 | model->get_contacts()->updateForceDiff(multibody.contacts, df_dx, df_du); |
| 578 |
1/2✓ Branch 3 taken 31430 times.
✗ Branch 4 not taken.
|
31430 | costs = model->get_costs()->createData(&multibody); |
| 579 |
1/2✓ Branch 3 taken 31430 times.
✗ Branch 4 not taken.
|
31430 | constraints = model->get_constraints()->createData(&multibody); |
| 580 |
1/2✓ Branch 2 taken 31430 times.
✗ Branch 3 not taken.
|
31430 | costs->shareMemory(this); |
| 581 |
1/2✓ Branch 2 taken 31430 times.
✗ Branch 3 not taken.
|
31430 | constraints->shareMemory(this); |
| 582 | |||
| 583 |
1/2✓ Branch 1 taken 31430 times.
✗ Branch 2 not taken.
|
31430 | tmp_xstatic.setZero(); |
| 584 |
1/2✓ Branch 1 taken 31430 times.
✗ Branch 2 not taken.
|
31430 | tmp_rstatic.setZero(); |
| 585 |
1/2✓ Branch 1 taken 31430 times.
✗ Branch 2 not taken.
|
31430 | tmp_Jstatic.setZero(); |
| 586 | 31430 | } | |
| 587 | |||
| 588 | pinocchio::DataTpl<Scalar> pinocchio; //!< Pinocchio data | ||
| 589 | DataCollectorJointActMultibodyInContact multibody; //!< Multibody data | ||
| 590 | std::shared_ptr<CostDataSum> costs; //!< Costs data | ||
| 591 | std::shared_ptr<ConstraintDataManager> constraints; //!< Constraints data | ||
| 592 | VectorXs | ||
| 593 | tmp_xstatic; //!< State point used for computing the quasi-static input | ||
| 594 | VectorXs | ||
| 595 | tmp_rstatic; //!< Factorization used for computing the quasi-static input | ||
| 596 | MatrixXs tmp_Jstatic; //!< Jacobian used for computing the quasi-static input | ||
| 597 | |||
| 598 | using Base::cost; | ||
| 599 | using Base::Fu; | ||
| 600 | using Base::Fx; | ||
| 601 | using Base::Lu; | ||
| 602 | using Base::Luu; | ||
| 603 | using Base::Lx; | ||
| 604 | using Base::Lxu; | ||
| 605 | using Base::Lxx; | ||
| 606 | using Base::r; | ||
| 607 | using Base::xout; | ||
| 608 | |||
| 609 | struct ResidualDataActuation : public ResidualDataAbstractTpl<_Scalar> { | ||
| 610 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 611 | |||
| 612 | typedef _Scalar Scalar; | ||
| 613 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 614 | typedef ResidualDataAbstractTpl<Scalar> Base; | ||
| 615 | typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract; | ||
| 616 | typedef DataCollectorActMultibodyInContactTpl<Scalar> | ||
| 617 | DataCollectorActMultibodyInContact; | ||
| 618 | typedef ActuationDataAbstractTpl<Scalar> ActuationDataAbstract; | ||
| 619 | typedef ContactDataMultipleTpl<Scalar> ContactDataMultiple; | ||
| 620 | typedef typename MathBase::MatrixXs MatrixXs; | ||
| 621 | |||
| 622 | template <template <typename Scalar> class Model> | ||
| 623 | 27948 | ResidualDataActuation(Model<Scalar>* const model, | |
| 624 | DataCollectorAbstract* const data) | ||
| 625 | : Base(model, data), | ||
| 626 |
1/2✓ Branch 7 taken 27948 times.
✗ Branch 8 not taken.
|
27948 | dtau_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()), |
| 627 |
1/2✓ Branch 8 taken 27948 times.
✗ Branch 9 not taken.
|
55896 | dtau_du(model->get_state()->get_nv(), model->get_nu()) { |
| 628 | // Check that proper shared data has been passed | ||
| 629 | 27948 | DataCollectorActMultibodyInContact* d = | |
| 630 |
1/2✓ Branch 0 taken 27948 times.
✗ Branch 1 not taken.
|
27948 | dynamic_cast<DataCollectorActMultibodyInContact*>(shared); |
| 631 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 27948 times.
|
27948 | if (d == NULL) { |
| 632 | ✗ | throw_pretty( | |
| 633 | "Invalid argument: the shared data should be derived from " | ||
| 634 | "DataCollectorActMultibodyInContact"); | ||
| 635 | } | ||
| 636 | // Avoids data casting at runtime | ||
| 637 | 27948 | pinocchio = d->pinocchio; | |
| 638 | 27948 | actuation = d->actuation; | |
| 639 | 27948 | contact = d->contacts; | |
| 640 |
1/2✓ Branch 1 taken 27948 times.
✗ Branch 2 not taken.
|
27948 | dtau_dx.setZero(); |
| 641 |
1/2✓ Branch 1 taken 27948 times.
✗ Branch 2 not taken.
|
27948 | dtau_du.setZero(); |
| 642 | 27948 | } | |
| 643 | |||
| 644 | pinocchio::DataTpl<Scalar>* pinocchio; //!< Pinocchio data | ||
| 645 | std::shared_ptr<ActuationDataAbstract> actuation; //!< Actuation data | ||
| 646 | std::shared_ptr<ContactDataMultiple> contact; //!< Contact data | ||
| 647 | MatrixXs dtau_dx; | ||
| 648 | MatrixXs dtau_du; | ||
| 649 | using Base::r; | ||
| 650 | using Base::Ru; | ||
| 651 | using Base::Rx; | ||
| 652 | using Base::shared; | ||
| 653 | }; | ||
| 654 | |||
| 655 | struct ResidualDataContact : public ResidualDataAbstractTpl<_Scalar> { | ||
| 656 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 657 | |||
| 658 | typedef _Scalar Scalar; | ||
| 659 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 660 | typedef ResidualDataAbstractTpl<Scalar> Base; | ||
| 661 | typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract; | ||
| 662 | typedef DataCollectorMultibodyInContactTpl<Scalar> | ||
| 663 | DataCollectorMultibodyInContact; | ||
| 664 | typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple; | ||
| 665 | |||
| 666 | template <template <typename Scalar> class Model> | ||
| 667 | 76196 | ResidualDataContact(Model<Scalar>* const model, | |
| 668 | DataCollectorAbstract* const data, const std::size_t id) | ||
| 669 | 76196 | : Base(model, data) { | |
| 670 | 76196 | DataCollectorMultibodyInContact* d = | |
| 671 |
1/2✓ Branch 0 taken 76196 times.
✗ Branch 1 not taken.
|
76196 | dynamic_cast<DataCollectorMultibodyInContact*>(shared); |
| 672 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 76196 times.
|
76196 | if (d == NULL) { |
| 673 | ✗ | throw_pretty( | |
| 674 | "Invalid argument: the shared data should be derived from " | ||
| 675 | "DataCollectorMultibodyInContact"); | ||
| 676 | } | ||
| 677 | 76196 | typename ContactModelMultiple::ContactDataContainer::iterator it, end; | |
| 678 | 152392 | for (it = d->contacts->contacts.begin(), | |
| 679 | 76196 | end = d->contacts->contacts.end(); | |
| 680 |
1/2✓ Branch 2 taken 151420 times.
✗ Branch 3 not taken.
|
151420 | it != end; ++it) { |
| 681 |
2/2✓ Branch 2 taken 76196 times.
✓ Branch 3 taken 75224 times.
|
151420 | if (id == it->second->frame) { // TODO(cmastalli): use model->get_id() |
| 682 | // and avoid to pass id in constructor | ||
| 683 | 76196 | contact = it->second.get(); | |
| 684 | 76196 | break; | |
| 685 | } | ||
| 686 | } | ||
| 687 | 76196 | } | |
| 688 | |||
| 689 | ContactDataAbstractTpl<Scalar>* contact; //!< Contact force data | ||
| 690 | using Base::r; | ||
| 691 | using Base::Ru; | ||
| 692 | using Base::Rx; | ||
| 693 | using Base::shared; | ||
| 694 | }; | ||
| 695 | }; | ||
| 696 | } // namespace crocoddyl | ||
| 697 | |||
| 698 | /* --- Details -------------------------------------------------------------- */ | ||
| 699 | /* --- Details -------------------------------------------------------------- */ | ||
| 700 | /* --- Details -------------------------------------------------------------- */ | ||
| 701 | #include <crocoddyl/multibody/actions/contact-invdyn.hxx> | ||
| 702 | |||
| 703 | #endif // CROCODDYL_MULTIBODY_ACTIONS_CONTACT_INVDYN_HPP_ | ||
| 704 |