GCC Code Coverage Report

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