GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/actions/free-fwddyn.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 22 0.0%
Branches: 0 80 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #ifndef CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
11 #define CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
12
13 #include "crocoddyl/core/actuation-base.hpp"
14 #include "crocoddyl/core/constraints/constraint-manager.hpp"
15 #include "crocoddyl/core/costs/cost-sum.hpp"
16 #include "crocoddyl/core/diff-action-base.hpp"
17 #include "crocoddyl/multibody/data/multibody.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 free forward dynamics in multibody
25 * systems.
26 *
27 * This class implements free forward dynamics, i.e.,
28 * \f[
29 * \mathbf{M}\dot{\mathbf{v}} + \mathbf{h}(\mathbf{q},\mathbf{v}) =
30 * \boldsymbol{\tau}, \f] where \f$\mathbf{q}\in Q\f$,
31 * \f$\mathbf{v}\in\mathbb{R}^{nv}\f$ are the configuration point and
32 * generalized velocity (its tangent vector), respectively;
33 * \f$\boldsymbol{\tau}\f$ is the torque inputs and
34 * \f$\mathbf{h}(\mathbf{q},\mathbf{v})\f$ are the Coriolis effect and gravity
35 * field.
36 *
37 * The derivatives of the system acceleration is computed efficiently based on
38 * the analytical derivatives of Articulate Body Algorithm (ABA) as described in
39 * \cite carpentier-rss18.
40 *
41 * The stack of cost functions is implemented in `CostModelSumTpl`. The
42 * computation of the free forward dynamics and its derivatives are carrying out
43 * inside `calc()` and `calcDiff()` functions, respectively. It is also
44 * important to remark that `calcDiff()` computes the derivatives using the
45 * latest stored values by `calc()`. Thus, we need to run `calc()` first.
46 *
47 * \sa `DifferentialActionModelAbstractTpl`, `calc()`, `calcDiff()`,
48 * `createData()`
49 */
50 template <typename _Scalar>
51 class DifferentialActionModelFreeFwdDynamicsTpl
52 : public DifferentialActionModelAbstractTpl<_Scalar> {
53 public:
54 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
55 ✗ CROCODDYL_DERIVED_CAST(DifferentialActionModelBase,
56 DifferentialActionModelFreeFwdDynamicsTpl)
57
58 typedef _Scalar Scalar;
59 typedef DifferentialActionModelAbstractTpl<Scalar> Base;
60 typedef DifferentialActionDataFreeFwdDynamicsTpl<Scalar> Data;
61 typedef DifferentialActionDataAbstractTpl<Scalar>
62 DifferentialActionDataAbstract;
63 typedef StateMultibodyTpl<Scalar> StateMultibody;
64 typedef CostModelSumTpl<Scalar> CostModelSum;
65 typedef ConstraintModelManagerTpl<Scalar> ConstraintModelManager;
66 typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
67 typedef MathBaseTpl<Scalar> MathBase;
68 typedef typename MathBase::VectorXs VectorXs;
69 typedef typename MathBase::MatrixXs MatrixXs;
70
71 DifferentialActionModelFreeFwdDynamicsTpl(
72 std::shared_ptr<StateMultibody> state,
73 std::shared_ptr<ActuationModelAbstract> actuation,
74 std::shared_ptr<CostModelSum> costs,
75 std::shared_ptr<ConstraintModelManager> constraints = nullptr);
76 ✗ virtual ~DifferentialActionModelFreeFwdDynamicsTpl() = default;
77
78 /**
79 * @brief Compute the system acceleration, and cost value
80 *
81 * It computes the system acceleration using the free forward-dynamics.
82 *
83 * @param[in] data Free forward-dynamics data
84 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
85 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
86 */
87 virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
88 const Eigen::Ref<const VectorXs>& x,
89 const Eigen::Ref<const VectorXs>& u) override;
90
91 /**
92 * @brief @copydoc Base::calc(const
93 * std::shared_ptr<DifferentialActionDataAbstract>& data, const
94 * Eigen::Ref<const VectorXs>& x)
95 */
96 virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
97 const Eigen::Ref<const VectorXs>& x) override;
98
99 /**
100 * @brief Compute the derivatives of the contact dynamics, and cost function
101 *
102 * @param[in] data Free forward-dynamics data
103 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
104 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
105 */
106 virtual void calcDiff(
107 const std::shared_ptr<DifferentialActionDataAbstract>& data,
108 const Eigen::Ref<const VectorXs>& x,
109 const Eigen::Ref<const VectorXs>& u) override;
110
111 /**
112 * @brief @copydoc Base::calcDiff(const
113 * std::shared_ptr<DifferentialActionDataAbstract>& data, const
114 * Eigen::Ref<const VectorXs>& x)
115 */
116 virtual void calcDiff(
117 const std::shared_ptr<DifferentialActionDataAbstract>& data,
118 const Eigen::Ref<const VectorXs>& x) override;
119
120 /**
121 * @brief Create the free forward-dynamics data
122 *
123 * @return free forward-dynamics data
124 */
125 virtual std::shared_ptr<DifferentialActionDataAbstract> createData() override;
126
127 /**
128 * @brief Cast the free-fwddyn model to a different scalar type.
129 *
130 * It is useful for operations requiring different precision or scalar types.
131 *
132 * @tparam NewScalar The new scalar type to cast to.
133 * @return DifferentialActionModelFreeFwdDynamicsTpl<NewScalar> A
134 * differential-action model with the new scalar type.
135 */
136 template <typename NewScalar>
137 DifferentialActionModelFreeFwdDynamicsTpl<NewScalar> cast() const;
138
139 /**
140 * @brief Check that the given data belongs to the free forward-dynamics data
141 */
142 virtual bool checkData(
143 const std::shared_ptr<DifferentialActionDataAbstract>& data) override;
144
145 /**
146 * @brief @copydoc Base::quasiStatic()
147 */
148 virtual void quasiStatic(
149 const std::shared_ptr<DifferentialActionDataAbstract>& data,
150 Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
151 const std::size_t maxiter = 100,
152 const Scalar tol = Scalar(1e-9)) override;
153
154 /**
155 * @brief Return the number of inequality constraints
156 */
157 virtual std::size_t get_ng() const override;
158
159 /**
160 * @brief Return the number of equality constraints
161 */
162 virtual std::size_t get_nh() const override;
163
164 /**
165 * @brief Return the number of equality terminal constraints
166 */
167 virtual std::size_t get_ng_T() const override;
168
169 /**
170 * @brief Return the number of equality terminal constraints
171 */
172 virtual std::size_t get_nh_T() const override;
173
174 /**
175 * @brief Return the lower bound of the inequality constraints
176 */
177 virtual const VectorXs& get_g_lb() const override;
178
179 /**
180 * @brief Return the upper bound of the inequality constraints
181 */
182 virtual const VectorXs& get_g_ub() const override;
183
184 /**
185 * @brief Return the actuation model
186 */
187 const std::shared_ptr<ActuationModelAbstract>& get_actuation() const;
188
189 /**
190 * @brief Return the cost model
191 */
192 const std::shared_ptr<CostModelSum>& get_costs() const;
193
194 /**
195 * @brief Return the constraint model manager
196 */
197 const std::shared_ptr<ConstraintModelManager>& get_constraints() const;
198
199 /**
200 * @brief Return the Pinocchio model
201 */
202 pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
203
204 /**
205 * @brief Return the armature vector
206 */
207 const VectorXs& get_armature() const;
208
209 /**
210 * @brief Modify the armature vector
211 */
212 void set_armature(const VectorXs& armature);
213
214 /**
215 * @brief Print relevant information of the free forward-dynamics model
216 *
217 * @param[out] os Output stream object
218 */
219 virtual void print(std::ostream& os) const override;
220
221 protected:
222 using Base::g_lb_; //!< Lower bound of the inequality constraints
223 using Base::g_ub_; //!< Upper bound of the inequality constraints
224 using Base::nu_; //!< Control dimension
225 using Base::state_; //!< Model of the state
226
227 private:
228 std::shared_ptr<ActuationModelAbstract> actuation_; //!< Actuation model
229 std::shared_ptr<CostModelSum> costs_; //!< Cost model
230 std::shared_ptr<ConstraintModelManager> constraints_; //!< Constraint model
231 pinocchio::ModelTpl<Scalar>* pinocchio_; //!< Pinocchio model
232 bool without_armature_; //!< Indicate if we have defined an armature
233 VectorXs armature_; //!< Armature vector
234 };
235
236 template <typename _Scalar>
237 struct DifferentialActionDataFreeFwdDynamicsTpl
238 : public DifferentialActionDataAbstractTpl<_Scalar> {
239 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
240 typedef _Scalar Scalar;
241 typedef MathBaseTpl<Scalar> MathBase;
242 typedef DifferentialActionDataAbstractTpl<Scalar> Base;
243 typedef JointDataAbstractTpl<Scalar> JointDataAbstract;
244 typedef DataCollectorJointActMultibodyTpl<Scalar>
245 DataCollectorJointActMultibody;
246 typedef typename MathBase::VectorXs VectorXs;
247 typedef typename MathBase::MatrixXs MatrixXs;
248
249 template <template <typename Scalar> class Model>
250 ✗ explicit DifferentialActionDataFreeFwdDynamicsTpl(Model<Scalar>* const model)
251 : Base(model),
252 ✗ pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
253 ✗ multibody(
254 ✗ &pinocchio, model->get_actuation()->createData(),
255 std::make_shared<JointDataAbstract>(
256 ✗ model->get_state(), model->get_actuation(), model->get_nu())),
257 ✗ costs(model->get_costs()->createData(&multibody)),
258 ✗ Minv(model->get_state()->get_nv(), model->get_state()->get_nv()),
259 ✗ u_drift(model->get_state()->get_nv()),
260 ✗ dtau_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
261 ✗ tmp_xstatic(model->get_state()->get_nx()) {
262 ✗ multibody.joint->dtau_du.diagonal().setOnes();
263 ✗ costs->shareMemory(this);
264 ✗ if (model->get_constraints() != nullptr) {
265 ✗ constraints = model->get_constraints()->createData(&multibody);
266 ✗ constraints->shareMemory(this);
267 }
268 ✗ Minv.setZero();
269 ✗ u_drift.setZero();
270 ✗ dtau_dx.setZero();
271 ✗ tmp_xstatic.setZero();
272 }
273 ✗ virtual ~DifferentialActionDataFreeFwdDynamicsTpl() = default;
274
275 pinocchio::DataTpl<Scalar> pinocchio;
276 DataCollectorJointActMultibody multibody;
277 std::shared_ptr<CostDataSumTpl<Scalar> > costs;
278 std::shared_ptr<ConstraintDataManagerTpl<Scalar> > constraints;
279 MatrixXs Minv;
280 VectorXs u_drift;
281 MatrixXs dtau_dx;
282 VectorXs tmp_xstatic;
283
284 using Base::cost;
285 using Base::Fu;
286 using Base::Fx;
287 using Base::Lu;
288 using Base::Luu;
289 using Base::Lx;
290 using Base::Lxu;
291 using Base::Lxx;
292 using Base::r;
293 using Base::xout;
294 };
295
296 } // namespace crocoddyl
297
298 /* --- Details -------------------------------------------------------------- */
299 /* --- Details -------------------------------------------------------------- */
300 /* --- Details -------------------------------------------------------------- */
301 #include <crocoddyl/multibody/actions/free-fwddyn.hxx>
302
303 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
304 crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl)
305 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
306 crocoddyl::DifferentialActionDataFreeFwdDynamicsTpl)
307
308 #endif // CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
309