GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/actions/contact-fwddyn.hpp
Date: 2025-02-24 23:41:29
Exec Total Coverage
Lines: 30 30 100.0%
Branches: 25 48 52.1%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2024, LAAS-CNRS, University of Edinburgh, CTU, INRIA,
5 // University of Oxford, Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #ifndef CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
11 #define CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
12
13 #include <stdexcept>
14
15 #include "crocoddyl/core/actuation-base.hpp"
16 #include "crocoddyl/core/constraints/constraint-manager.hpp"
17 #include "crocoddyl/core/costs/cost-sum.hpp"
18 #include "crocoddyl/core/diff-action-base.hpp"
19 #include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
20 #include "crocoddyl/multibody/data/contacts.hpp"
21 #include "crocoddyl/multibody/fwd.hpp"
22 #include "crocoddyl/multibody/states/multibody.hpp"
23
24 namespace crocoddyl {
25
26 /**
27 * @brief Differential action model for contact forward dynamics in multibody
28 * systems.
29 *
30 * This class implements contact forward dynamics given a stack of
31 * rigid-contacts described in `ContactModelMultipleTpl`, i.e., \f[
32 * \left[\begin{matrix}\dot{\mathbf{v}}
33 * \\ -\boldsymbol{\lambda}\end{matrix}\right] = \left[\begin{matrix}\mathbf{M}
34 * & \mathbf{J}^{\top}_c \\ {\mathbf{J}_{c}} & \mathbf{0}
35 * \end{matrix}\right]^{-1} \left[\begin{matrix}\boldsymbol{\tau}_b
36 * \\ -\mathbf{a}_0 \\\end{matrix}\right], \f] where \f$\mathbf{q}\in Q\f$,
37 * \f$\mathbf{v}\in\mathbb{R}^{nv}\f$ are the configuration point and
38 * generalized velocity (its tangent vector), respectively;
39 * \f$\boldsymbol{\tau}_b=\boldsymbol{\tau} -
40 * \mathbf{h}(\mathbf{q},\mathbf{v})\f$ is the bias forces that depends on the
41 * torque inputs \f$\boldsymbol{\tau}\f$ and the Coriolis effect and gravity
42 * field \f$\mathbf{h}(\mathbf{q},\mathbf{v})\f$;
43 * \f$\mathbf{J}_c\in\mathbb{R}^{nc\times nv}\f$ is the contact Jacobian
44 * expressed in the local frame; and \f$\mathbf{a}_0\in\mathbb{R}^{nc}\f$ is the
45 * desired acceleration in the constraint space. To improve stability in the
46 * numerical integration, we define PD gains that are similar in spirit to
47 * Baumgarte stabilization: \f[ \mathbf{a}_0 = \mathbf{a}_{\lambda(c)} - \alpha
48 * \,^oM^{ref}_{\lambda(c)}\ominus\,^oM_{\lambda(c)}
49 * - \beta\mathbf{v}_{\lambda(c)}, \f] where \f$\mathbf{v}_{\lambda(c)}\f$,
50 * \f$\mathbf{a}_{\lambda(c)}\f$ are the spatial velocity and acceleration at
51 * the parent body of the contact \f$\lambda(c)\f$, respectively; \f$\alpha\f$
52 * and \f$\beta\f$ are the stabilization gains;
53 * \f$\,^oM^{ref}_{\lambda(c)}\ominus\,^oM_{\lambda(c)}\f$ is the
54 * \f$\mathbb{SE}(3)\f$ inverse composition between the reference contact
55 * placement and the current one.
56 *
57 * The derivatives of the system acceleration and contact forces are computed
58 * efficiently based on the analytical derivatives of Recursive Newton Euler
59 * Algorithm (RNEA) as described in \cite mastalli-icra20. Note that the
60 * algorithm for computing the RNEA derivatives is described in \cite
61 * carpentier-rss18.
62 *
63 * The stack of cost and constraint functions are implemented in
64 * `CostModelSumTpl` and `ConstraintModelAbstractTpl`, respectively. The
65 * computation of the contact dynamics and its derivatives are carrying out
66 * inside `calc()` and `calcDiff()` functions, respectively. It is also
67 * important to remark that `calcDiff()` computes the derivatives using the
68 * latest stored values by `calc()`. Thus, we need to run `calc()` first.
69 *
70 * \sa `DifferentialActionModelAbstractTpl`, `calc()`, `calcDiff()`,
71 * `createData()`
72 */
73 template <typename _Scalar>
74 class DifferentialActionModelContactFwdDynamicsTpl
75 : public DifferentialActionModelAbstractTpl<_Scalar> {
76 public:
77 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
78
79 typedef _Scalar Scalar;
80 typedef DifferentialActionModelAbstractTpl<Scalar> Base;
81 typedef DifferentialActionDataContactFwdDynamicsTpl<Scalar> Data;
82 typedef MathBaseTpl<Scalar> MathBase;
83 typedef StateMultibodyTpl<Scalar> StateMultibody;
84 typedef CostModelSumTpl<Scalar> CostModelSum;
85 typedef ConstraintModelManagerTpl<Scalar> ConstraintModelManager;
86 typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple;
87 typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
88 typedef DifferentialActionDataAbstractTpl<Scalar>
89 DifferentialActionDataAbstract;
90 typedef typename MathBase::VectorXs VectorXs;
91 typedef typename MathBase::MatrixXs MatrixXs;
92
93 /**
94 * @brief Initialize the contact forward-dynamics action model
95 *
96 * It describes the dynamics evolution of a multibody system under
97 * rigid-contact constraints defined by `ContactModelMultipleTpl`. It computes
98 * the cost described in `CostModelSumTpl`.
99 *
100 * @param[in] state State of the multibody system
101 * @param[in] actuation Actuation model
102 * @param[in] contacts Stack of rigid contact
103 * @param[in] costs Stack of cost functions
104 * @param[in] JMinvJt_damping Damping term used in operational space inertia
105 * matrix (default 0.)
106 * @param[in] enable_force Enable the computation of the contact force
107 * derivatives (default false)
108 */
109 DifferentialActionModelContactFwdDynamicsTpl(
110 std::shared_ptr<StateMultibody> state,
111 std::shared_ptr<ActuationModelAbstract> actuation,
112 std::shared_ptr<ContactModelMultiple> contacts,
113 std::shared_ptr<CostModelSum> costs,
114 const Scalar JMinvJt_damping = Scalar(0.),
115 const bool enable_force = false);
116
117 /**
118 * @brief Initialize the contact forward-dynamics action model
119 *
120 * It describes the dynamics evolution of a multibody system under
121 * rigid-contact constraints defined by `ContactModelMultipleTpl`. It computes
122 * the cost described in `CostModelSumTpl`.
123 *
124 * @param[in] state State of the multibody system
125 * @param[in] actuation Actuation model
126 * @param[in] contacts Stack of rigid contact
127 * @param[in] costs Stack of cost functions
128 * @param[in] constraints Stack of constraints
129 * @param[in] JMinvJt_damping Damping term used in operational space inertia
130 * matrix (default 0.)
131 * @param[in] enable_force Enable the computation of the contact force
132 * derivatives (default false)
133 */
134 DifferentialActionModelContactFwdDynamicsTpl(
135 std::shared_ptr<StateMultibody> state,
136 std::shared_ptr<ActuationModelAbstract> actuation,
137 std::shared_ptr<ContactModelMultiple> contacts,
138 std::shared_ptr<CostModelSum> costs,
139 std::shared_ptr<ConstraintModelManager> constraints,
140 const Scalar JMinvJt_damping = Scalar(0.),
141 const bool enable_force = false);
142 virtual ~DifferentialActionModelContactFwdDynamicsTpl();
143
144 /**
145 * @brief Compute the system acceleration, and cost value
146 *
147 * It computes the system acceleration using the contact dynamics.
148 *
149 * @param[in] data Contact forward-dynamics data
150 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
151 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
152 */
153 virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
154 const Eigen::Ref<const VectorXs>& x,
155 const Eigen::Ref<const VectorXs>& u);
156
157 /**
158 * @brief Compute the total cost value for nodes that depends only on the
159 * state
160 *
161 * It updates the total cost and the system acceleration is not updated as it
162 * is expected to be zero. Additionally, it does not update the contact
163 * forces. This function is used in the terminal nodes of an optimal control
164 * problem.
165 *
166 * @param[in] data Contact forward-dynamics data
167 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
168 */
169 virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
170 const Eigen::Ref<const VectorXs>& x);
171
172 /**
173 * @brief Compute the derivatives of the contact dynamics, and cost function
174 *
175 * @param[in] data Contact forward-dynamics data
176 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
177 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
178 */
179 virtual void calcDiff(
180 const std::shared_ptr<DifferentialActionDataAbstract>& data,
181 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
182
183 /**
184 * @brief Compute the derivatives of the cost functions with respect to the
185 * state only
186 *
187 * It updates the derivatives of the cost function with respect to the state
188 * only. Additionally, it does not update the contact forces derivatives. This
189 * function is used in the terminal nodes of an optimal control problem.
190 *
191 * @param[in] data Contact forward-dynamics data
192 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
193 */
194 virtual void calcDiff(
195 const std::shared_ptr<DifferentialActionDataAbstract>& data,
196 const Eigen::Ref<const VectorXs>& x);
197
198 /**
199 * @brief Create the contact forward-dynamics data
200 *
201 * @return contact forward-dynamics data
202 */
203 virtual std::shared_ptr<DifferentialActionDataAbstract> createData();
204
205 /**
206 * @brief Check that the given data belongs to the contact forward-dynamics
207 * data
208 */
209 virtual bool checkData(
210 const std::shared_ptr<DifferentialActionDataAbstract>& data);
211
212 /**
213 * @brief @copydoc Base::quasiStatic()
214 */
215 virtual void quasiStatic(
216 const std::shared_ptr<DifferentialActionDataAbstract>& data,
217 Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
218 const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9));
219
220 /**
221 * @brief Return the number of inequality constraints
222 */
223 virtual std::size_t get_ng() const;
224
225 /**
226 * @brief Return the number of equality constraints
227 */
228 virtual std::size_t get_nh() const;
229
230 /**
231 * @brief Return the number of equality terminal constraints
232 */
233 virtual std::size_t get_ng_T() const;
234
235 /**
236 * @brief Return the number of equality terminal constraints
237 */
238 virtual std::size_t get_nh_T() const;
239
240 /**
241 * @brief Return the lower bound of the inequality constraints
242 */
243 virtual const VectorXs& get_g_lb() const;
244
245 /**
246 * @brief Return the upper bound of the inequality constraints
247 */
248 virtual const VectorXs& get_g_ub() const;
249
250 /**
251 * @brief Return the actuation model
252 */
253 const std::shared_ptr<ActuationModelAbstract>& get_actuation() const;
254
255 /**
256 * @brief Return the contact model
257 */
258 const std::shared_ptr<ContactModelMultiple>& get_contacts() const;
259
260 /**
261 * @brief Return the cost model
262 */
263 const std::shared_ptr<CostModelSum>& get_costs() const;
264
265 /**
266 * @brief Return the constraint model manager
267 */
268 const std::shared_ptr<ConstraintModelManager>& get_constraints() const;
269
270 /**
271 * @brief Return the Pinocchio model
272 */
273 pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
274
275 /**
276 * @brief Return the armature vector
277 */
278 const VectorXs& get_armature() const;
279
280 /**
281 * @brief Return the damping factor used in operational space inertia matrix
282 */
283 const Scalar get_damping_factor() const;
284
285 /**
286 * @brief Modify the armature vector
287 */
288 void set_armature(const VectorXs& armature);
289
290 /**
291 * @brief Modify the damping factor used in operational space inertia matrix
292 */
293 void set_damping_factor(const Scalar damping);
294
295 /**
296 * @brief Print relevant information of the contact forward-dynamics model
297 *
298 * @param[out] os Output stream object
299 */
300 virtual void print(std::ostream& os) const;
301
302 protected:
303 using Base::g_lb_; //!< Lower bound of the inequality constraints
304 using Base::g_ub_; //!< Upper bound of the inequality constraints
305 using Base::nu_; //!< Control dimension
306 using Base::state_; //!< Model of the state
307
308 private:
309 void init();
310 std::shared_ptr<ActuationModelAbstract> actuation_; //!< Actuation model
311 std::shared_ptr<ContactModelMultiple> contacts_; //!< Contact model
312 std::shared_ptr<CostModelSum> costs_; //!< Cost model
313 std::shared_ptr<ConstraintModelManager> constraints_; //!< Constraint model
314 pinocchio::ModelTpl<Scalar>& pinocchio_; //!< Pinocchio model
315 bool with_armature_; //!< Indicate if we have defined an armature
316 VectorXs armature_; //!< Armature vector
317 Scalar JMinvJt_damping_; //!< Damping factor used in operational space
318 //!< inertia matrix
319 bool enable_force_; //!< Indicate if we have enabled the computation of the
320 //!< contact-forces derivatives
321 };
322
323 template <typename _Scalar>
324 struct DifferentialActionDataContactFwdDynamicsTpl
325 : public DifferentialActionDataAbstractTpl<_Scalar> {
326 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
327 typedef _Scalar Scalar;
328 typedef MathBaseTpl<Scalar> MathBase;
329 typedef DifferentialActionDataAbstractTpl<Scalar> Base;
330 typedef JointDataAbstractTpl<Scalar> JointDataAbstract;
331 typedef DataCollectorJointActMultibodyInContactTpl<Scalar>
332 DataCollectorJointActMultibodyInContact;
333 typedef typename MathBase::VectorXs VectorXs;
334 typedef typename MathBase::MatrixXs MatrixXs;
335
336 template <template <typename Scalar> class Model>
337 39554 explicit DifferentialActionDataContactFwdDynamicsTpl(
338 Model<Scalar>* const model)
339 : Base(model),
340
1/2
✓ Branch 1 taken 39554 times.
✗ Branch 2 not taken.
 39554 pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
341
3/6
✓ Branch 2 taken 39554 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 39554 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 39554 times.
✗ Branch 10 not taken.
 118662 multibody(
342
1/2
✓ Branch 3 taken 39554 times.
✗ Branch 4 not taken.
 39554 &pinocchio, model->get_actuation()->createData(),
343 std::make_shared<JointDataAbstract>(
344 79108 model->get_state(), model->get_actuation(), model->get_nu()),
345 39554 model->get_contacts()->createData(&pinocchio)),
346
1/2
✓ Branch 3 taken 39554 times.
✗ Branch 4 not taken.
 39554 costs(model->get_costs()->createData(&multibody)),
347
1/2
✓ Branch 3 taken 39554 times.
✗ Branch 4 not taken.
 79108 Kinv(model->get_state()->get_nv() +
348 39554 model->get_contacts()->get_nc_total(),
349 79108 model->get_state()->get_nv() +
350 39554 model->get_contacts()->get_nc_total()),
351
1/2
✓ Branch 3 taken 39554 times.
✗ Branch 4 not taken.
 39554 df_dx(model->get_contacts()->get_nc_total(),
352 39554 model->get_state()->get_ndx()),
353
1/2
✓ Branch 5 taken 39554 times.
✗ Branch 6 not taken.
 39554 df_du(model->get_contacts()->get_nc_total(), model->get_nu()),
354
1/2
✓ Branch 4 taken 39554 times.
✗ Branch 5 not taken.
 39554 tmp_xstatic(model->get_state()->get_nx()),
355
1/2
✓ Branch 3 taken 39554 times.
✗ Branch 4 not taken.
 39554 tmp_Jstatic(model->get_state()->get_nv(),
356 118662 model->get_nu() + model->get_contacts()->get_nc_total()) {
357
2/4
✓ Branch 2 taken 39554 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 39554 times.
✗ Branch 6 not taken.
 39554 multibody.joint->dtau_du.diagonal().setOnes();
358
1/2
✓ Branch 2 taken 39554 times.
✗ Branch 3 not taken.
 39554 costs->shareMemory(this);
359
2/2
✓ Branch 2 taken 1160 times.
✓ Branch 3 taken 38394 times.
 39554 if (model->get_constraints() != nullptr) {
360
1/2
✓ Branch 3 taken 1160 times.
✗ Branch 4 not taken.
 1160 constraints = model->get_constraints()->createData(&multibody);
361
1/2
✓ Branch 2 taken 1160 times.
✗ Branch 3 not taken.
 1160 constraints->shareMemory(this);
362 }
363
1/2
✓ Branch 1 taken 39554 times.
✗ Branch 2 not taken.
 39554 Kinv.setZero();
364
1/2
✓ Branch 1 taken 39554 times.
✗ Branch 2 not taken.
 39554 df_dx.setZero();
365
1/2
✓ Branch 1 taken 39554 times.
✗ Branch 2 not taken.
 39554 df_du.setZero();
366
1/2
✓ Branch 1 taken 39554 times.
✗ Branch 2 not taken.
 39554 tmp_xstatic.setZero();
367
1/2
✓ Branch 1 taken 39554 times.
✗ Branch 2 not taken.
 39554 tmp_Jstatic.setZero();
368
1/2
✓ Branch 4 taken 39554 times.
✗ Branch 5 not taken.
 39554 pinocchio.lambda_c.resize(model->get_contacts()->get_nc_total());
369
1/2
✓ Branch 1 taken 39554 times.
✗ Branch 2 not taken.
 39554 pinocchio.lambda_c.setZero();
370 39554 }
371
372 pinocchio::DataTpl<Scalar> pinocchio;
373 DataCollectorJointActMultibodyInContact multibody;
374 std::shared_ptr<CostDataSumTpl<Scalar> > costs;
375 std::shared_ptr<ConstraintDataManagerTpl<Scalar> > constraints;
376 MatrixXs Kinv;
377 MatrixXs df_dx;
378 MatrixXs df_du;
379 VectorXs tmp_xstatic;
380 MatrixXs tmp_Jstatic;
381
382 using Base::cost;
383 using Base::Fu;
384 using Base::Fx;
385 using Base::Lu;
386 using Base::Luu;
387 using Base::Lx;
388 using Base::Lxu;
389 using Base::Lxx;
390 using Base::r;
391 using Base::xout;
392 };
393
394 } // namespace crocoddyl
395
396 /* --- Details -------------------------------------------------------------- */
397 /* --- Details -------------------------------------------------------------- */
398 /* --- Details -------------------------------------------------------------- */
399 #include <crocoddyl/multibody/actions/contact-fwddyn.hxx>
400
401 #endif // CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
402