GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/actuations/floating-base.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 45 50 90.0%
Branches: 51 154 33.1%
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_ACTUATIONS_FLOATING_BASE_HPP_
11 #define CROCODDYL_MULTIBODY_ACTUATIONS_FLOATING_BASE_HPP_
12
13 #include "crocoddyl/core/actuation-base.hpp"
14 #include "crocoddyl/multibody/fwd.hpp"
15 #include "crocoddyl/multibody/states/multibody.hpp"
16
17 namespace crocoddyl {
18
19 /**
20 * @brief Floating-base actuation model
21 *
22 * It considers the first joint, defined in the Pinocchio model, as the
23 * floating-base joints. Then, this joint (that might have various DoFs) is
24 * unactuated.
25 *
26 * The main computations are carrying out in `calc`, and `calcDiff`, where the
27 * former computes actuation signal \f$\mathbf{a}\f$ from a given joint-torque
28 * input \f$\mathbf{u}\f$ and state point \f$\mathbf{x}\f$, and the latter
29 * computes the Jacobians of the actuation-mapping function. Note that
30 * `calcDiff` requires to run `calc` first.
31 *
32 * \sa `ActuationModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
33 */
34 template <typename _Scalar>
35 class ActuationModelFloatingBaseTpl
36 : public ActuationModelAbstractTpl<_Scalar> {
37 public:
38 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39
1/2
✓ Branch 3 taken 25 times.
✗ Branch 4 not taken.
 100 CROCODDYL_DERIVED_CAST(ActuationModelBase, ActuationModelFloatingBaseTpl)
40
41 typedef _Scalar Scalar;
42 typedef MathBaseTpl<Scalar> MathBase;
43 typedef ActuationModelAbstractTpl<Scalar> Base;
44 typedef ActuationDataAbstractTpl<Scalar> Data;
45 typedef StateMultibodyTpl<Scalar> StateMultibody;
46 typedef typename MathBase::VectorXs VectorXs;
47 typedef typename MathBase::MatrixXs MatrixXs;
48
49 /**
50 * @brief Initialize the floating-base actuation model
51 *
52 * @param[in] state State of a multibody system
53 * @param[in] nu Dimension of joint-torque vector
54 */
55 713 explicit ActuationModelFloatingBaseTpl(std::shared_ptr<StateMultibody> state)
56 : Base(state,
57 713 state->get_nv() -
58 713 state->get_pinocchio()
59 713 ->joints[(
60
4/6
✓ Branch 5 taken 713 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 713 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 587 times.
✓ Branch 11 taken 126 times.
 1426 state->get_pinocchio()->existJointName("root_joint")
61
5/12
✓ Branch 2 taken 587 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 587 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 587 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 587 times.
✓ Branch 14 taken 126 times.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
 1300 ? state->get_pinocchio()->getJointId("root_joint")
62 : 0)]
63
6/10
✓ Branch 2 taken 713 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 713 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 587 times.
✓ Branch 10 taken 126 times.
✓ Branch 12 taken 713 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 713 times.
✗ Branch 16 not taken.
 2139 .nv()) {};
64 1480 virtual ~ActuationModelFloatingBaseTpl() = default;
65
66 /**
67 * @brief Compute the floating-base actuation signal from the joint-torque
68 * input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
69 *
70 * @param[in] data Actuation data
71 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
72 * @param[in] u Joint-torque input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
73 */
74 54017 virtual void calc(const std::shared_ptr<Data>& data,
75 const Eigen::Ref<const VectorXs>& /*x*/,
76 const Eigen::Ref<const VectorXs>& u) override {
77
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 54017 times.
 54017 if (static_cast<std::size_t>(u.size()) != nu_) {
78 ✗ throw_pretty(
79 "Invalid argument: " << "u has wrong dimension (it should be " +
80 std::to_string(nu_) + ")");
81 }
82
1/2
✓ Branch 3 taken 54017 times.
✗ Branch 4 not taken.
 54017 data->tau.tail(nu_) = u;
83 54017 };
84
85 /**
86 * @brief Compute the Jacobians of the floating-base actuation function
87 *
88 * @param[in] data Actuation data
89 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
90 * @param[in] u Joint-torque input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
91 */
92 #ifndef NDEBUG
93 8676 virtual void calcDiff(const std::shared_ptr<Data>& data,
94 const Eigen::Ref<const VectorXs>& /*x*/,
95 const Eigen::Ref<const VectorXs>& /*u*/) override {
96 #else
97 virtual void calcDiff(const std::shared_ptr<Data>&,
98 const Eigen::Ref<const VectorXs>& /*x*/,
99 const Eigen::Ref<const VectorXs>& /*u*/) override {
100 #endif
101 // The derivatives has constant values which were set in createData.
102
2/12
✓ Branch 3 taken 8676 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 8676 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
 8676 assert_pretty(data->dtau_dx.isZero(), "dtau_dx has wrong value");
103
2/12
✓ Branch 4 taken 8676 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 8676 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
 8676 assert_pretty(MatrixXs(data->dtau_du).isApprox(dtau_du_),
104 "dtau_du has wrong value");
105 8676 };
106
107 23380 virtual void commands(const std::shared_ptr<Data>& data,
108 const Eigen::Ref<const VectorXs>&,
109 const Eigen::Ref<const VectorXs>& tau) override {
110
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 23380 times.
 23380 if (static_cast<std::size_t>(tau.size()) != state_->get_nv()) {
111 ✗ throw_pretty(
112 "Invalid argument: " << "tau has wrong dimension (it should be " +
113 std::to_string(state_->get_nv()) + ")");
114 }
115
1/2
✓ Branch 3 taken 23380 times.
✗ Branch 4 not taken.
 23380 data->u = tau.tail(nu_);
116 23380 }
117
118 #ifndef NDEBUG
119 3150 virtual void torqueTransform(const std::shared_ptr<Data>& data,
120 const Eigen::Ref<const VectorXs>&,
121 const Eigen::Ref<const VectorXs>&) override {
122 #else
123 virtual void torqueTransform(const std::shared_ptr<Data>&,
124 const Eigen::Ref<const VectorXs>&,
125 const Eigen::Ref<const VectorXs>&) override {
126 #endif
127 // The torque transform has constant values which were set in createData.
128
2/12
✓ Branch 4 taken 3150 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 3150 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
 3150 assert_pretty(MatrixXs(data->Mtau).isApprox(Mtau_), "Mtau has wrong value");
129 3150 }
130
131 /**
132 * @brief Create the floating-base actuation data
133 *
134 * @return the actuation data
135 */
136 55615 virtual std::shared_ptr<Data> createData() override {
137 typedef StateMultibodyTpl<Scalar> StateMultibody;
138 55615 std::shared_ptr<StateMultibody> state =
139 55615 std::static_pointer_cast<StateMultibody>(state_);
140
1/2
✓ Branch 1 taken 55615 times.
✗ Branch 2 not taken.
 55615 std::shared_ptr<Data> data =
141 55615 std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
142 55615 const std::size_t root_joint_id =
143
3/6
✓ Branch 2 taken 55615 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 55615 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 55615 times.
✗ Branch 11 not taken.
 111230 state->get_pinocchio()->existJointName("root_joint")
144
9/16
✓ Branch 0 taken 51889 times.
✓ Branch 1 taken 3726 times.
✓ Branch 4 taken 51889 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 51889 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 51889 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 51889 times.
✓ Branch 15 taken 3726 times.
✓ Branch 17 taken 51889 times.
✓ Branch 18 taken 3726 times.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
 55615 ? state->get_pinocchio()->getJointId("root_joint")
145 : 0;
146
2/4
✓ Branch 2 taken 55615 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 55615 times.
✗ Branch 8 not taken.
 55615 const std::size_t nfb = state->get_pinocchio()->joints[root_joint_id].nv();
147
2/4
✓ Branch 2 taken 55615 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 55615 times.
✗ Branch 6 not taken.
 55615 data->dtau_du.diagonal(-nfb).setOnes();
148
2/4
✓ Branch 2 taken 55615 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 55615 times.
✗ Branch 6 not taken.
 55615 data->Mtau.diagonal(nfb).setOnes();
149
2/2
✓ Branch 0 taken 315060 times.
✓ Branch 1 taken 55615 times.
 370675 for (std::size_t i = 0; i < nfb; ++i) {
150
1/2
✓ Branch 2 taken 315060 times.
✗ Branch 3 not taken.
 315060 data->tau_set[i] = false;
151 }
152 #ifndef NDEBUG
153
1/2
✓ Branch 2 taken 55615 times.
✗ Branch 3 not taken.
 55615 dtau_du_ = data->dtau_du;
154
1/2
✓ Branch 2 taken 55615 times.
✗ Branch 3 not taken.
 55615 Mtau_ = data->Mtau;
155 #endif
156 111230 return data;
157 55615 };
158
159 template <typename NewScalar>
160 25 ActuationModelFloatingBaseTpl<NewScalar> cast() const {
161 typedef ActuationModelFloatingBaseTpl<NewScalar> ReturnType;
162 typedef StateMultibodyTpl<NewScalar> StateType;
163
1/2
✓ Branch 3 taken 25 times.
✗ Branch 4 not taken.
 50 ReturnType ret(std::static_pointer_cast<StateType>(
164 25 state_->template cast<NewScalar>()));
165 25 return ret;
166 }
167
168 /**
169 * @brief Print relevant information of the joint-effort residual
170 *
171 * @param[out] os Output stream object
172 */
173 ✗ virtual void print(std::ostream& os) const override {
174 ✗ os << "ActuationModelFloatingBase {nu=" << nu_
175 ✗ << ", nv=" << state_->get_nv() << "}";
176 }
177
178 protected:
179 using Base::nu_;
180 using Base::state_;
181
182 #ifndef NDEBUG
183 private:
184 MatrixXs dtau_du_;
185 MatrixXs Mtau_;
186 #endif
187 };
188
189 } // namespace crocoddyl
190
191 extern template class CROCODDYL_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI
192 crocoddyl::ActuationModelFloatingBaseTpl<double>;
193 extern template class CROCODDYL_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI
194 crocoddyl::ActuationModelFloatingBaseTpl<float>;
195
196 #endif // CROCODDYL_MULTIBODY_ACTUATIONS_FLOATING_BASE_HPP_
197