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