GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/states/multibody.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 1 1 100.0%
Branches: 1 2 50.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_STATES_MULTIBODY_HPP_
11 #define CROCODDYL_MULTIBODY_STATES_MULTIBODY_HPP_
12
13 #include <pinocchio/multibody/model.hpp>
14
15 #include "crocoddyl/core/state-base.hpp"
16 #include "crocoddyl/multibody/fwd.hpp"
17
18 namespace crocoddyl {
19
20 /**
21 * @brief State multibody representation
22 *
23 * A multibody state is described by the configuration point and its tangential
24 * velocity, or in other words, by the generalized position and velocity
25 * coordinates of a rigid-body system. For this state, we describe its
26 * operators: difference, integrates, transport and their derivatives for any
27 * Pinocchio model.
28 *
29 * For more details about these operators, please read the documentation of the
30 * `StateAbstractTpl` class.
31 *
32 * \sa `diff()`, `integrate()`, `Jdiff()`, `Jintegrate()` and
33 * `JintegrateTransport()`
34 */
35 template <typename _Scalar>
36 class StateMultibodyTpl : public StateAbstractTpl<_Scalar> {
37 public:
38 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39
1/2
✓ Branch 3 taken 97 times.
✗ Branch 4 not taken.
 388 CROCODDYL_DERIVED_CAST(StateBase, StateMultibodyTpl)
40
41 typedef _Scalar Scalar;
42 typedef MathBaseTpl<Scalar> MathBase;
43 typedef StateAbstractTpl<Scalar> Base;
44 typedef pinocchio::ModelTpl<Scalar> PinocchioModel;
45 typedef typename MathBase::VectorXs VectorXs;
46 typedef typename MathBase::MatrixXs MatrixXs;
47
48 /**
49 * @brief Initialize the multibody state
50 *
51 * @param[in] model Pinocchio model
52 */
53 explicit StateMultibodyTpl(std::shared_ptr<PinocchioModel> model);
54 StateMultibodyTpl();
55 virtual ~StateMultibodyTpl();
56
57 /**
58 * @brief Generate a zero state.
59 *
60 * Note that the zero configuration is computed using `pinocchio::neutral`.
61 */
62 virtual VectorXs zero() const override;
63
64 /**
65 * @brief Generate a random state
66 *
67 * Note that the random configuration is computed using `pinocchio::random`
68 * which satisfies the manifold definition (e.g., the quaterion definition)
69 */
70 virtual VectorXs rand() const override;
71
72 virtual void diff(const Eigen::Ref<const VectorXs>& x0,
73 const Eigen::Ref<const VectorXs>& x1,
74 Eigen::Ref<VectorXs> dxout) const override;
75 virtual void integrate(const Eigen::Ref<const VectorXs>& x,
76 const Eigen::Ref<const VectorXs>& dx,
77 Eigen::Ref<VectorXs> xout) const override;
78 virtual void Jdiff(const Eigen::Ref<const VectorXs>&,
79 const Eigen::Ref<const VectorXs>&,
80 Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
81 const Jcomponent firstsecond = both) const override;
82
83 virtual void Jintegrate(const Eigen::Ref<const VectorXs>& x,
84 const Eigen::Ref<const VectorXs>& dx,
85 Eigen::Ref<MatrixXs> Jfirst,
86 Eigen::Ref<MatrixXs> Jsecond,
87 const Jcomponent firstsecond = both,
88 const AssignmentOp = setto) const override;
89 virtual void JintegrateTransport(const Eigen::Ref<const VectorXs>& x,
90 const Eigen::Ref<const VectorXs>& dx,
91 Eigen::Ref<MatrixXs> Jin,
92 const Jcomponent firstsecond) const override;
93
94 /**
95 * @brief Return the Pinocchio model (i.e., model of the rigid body system)
96 */
97 const std::shared_ptr<PinocchioModel>& get_pinocchio() const;
98
99 template <typename NewScalar>
100 StateMultibodyTpl<NewScalar> cast() const;
101
102 /**
103 * @brief Print relevant information of the state multibody
104 *
105 * @param[out] os Output stream object
106 */
107 virtual void print(std::ostream& os) const override;
108
109 protected:
110 using Base::has_limits_;
111 using Base::lb_;
112 using Base::ndx_;
113 using Base::nq_;
114 using Base::nv_;
115 using Base::nx_;
116 using Base::ub_;
117
118 private:
119 std::shared_ptr<PinocchioModel> pinocchio_; //!< Pinocchio model
120 VectorXs x0_; //!< Zero state
121 };
122
123 } // namespace crocoddyl
124
125 /* --- Details -------------------------------------------------------------- */
126 /* --- Details -------------------------------------------------------------- */
127 /* --- Details -------------------------------------------------------------- */
128 #include "crocoddyl/multibody/states/multibody.hxx"
129
130 extern template class CROCODDYL_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI
131 crocoddyl::StateMultibodyTpl<double>;
132 extern template class CROCODDYL_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI
133 crocoddyl::StateMultibodyTpl<float>;
134
135 #endif // CROCODDYL_MULTIBODY_STATES_MULTIBODY_HPP_
136