GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/actions/free-invdyn.hxx
Date: 2025-02-24 23:41:29
Exec Total Coverage
Lines: 129 150 86.0%
Branches: 75 280 26.8%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2024, Heriot-Watt University, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include <pinocchio/algorithm/compute-all-terms.hpp>
10 #include <pinocchio/algorithm/jacobian.hpp>
11 #include <pinocchio/algorithm/kinematics.hpp>
12 #include <pinocchio/algorithm/rnea-derivatives.hpp>
13 #include <pinocchio/algorithm/rnea.hpp>
14
15 #include "crocoddyl/core/constraints/residual.hpp"
16
17 namespace crocoddyl {
18
19 template <typename Scalar>
20 1 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::
21 DifferentialActionModelFreeInvDynamicsTpl(
22 std::shared_ptr<StateMultibody> state,
23 std::shared_ptr<ActuationModelAbstract> actuation,
24 std::shared_ptr<CostModelSum> costs)
25 1 : Base(state, state->get_nv(), costs->get_nr(), 0,
26 1 state->get_nv() - actuation->get_nu()),
27 1 actuation_(actuation),
28 1 costs_(costs),
29 1 constraints_(
30
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::make_shared<ConstraintModelManager>(state, state->get_nv())),
31
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 pinocchio_(*state->get_pinocchio().get()) {
32
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 init(state);
33 1 }
34
35 template <typename Scalar>
36 150 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::
37 DifferentialActionModelFreeInvDynamicsTpl(
38 std::shared_ptr<StateMultibody> state,
39 std::shared_ptr<ActuationModelAbstract> actuation,
40 std::shared_ptr<CostModelSum> costs,
41 std::shared_ptr<ConstraintModelManager> constraints)
42 150 : Base(state, state->get_nv(), costs->get_nr(), constraints->get_ng(),
43 150 constraints->get_nh() + state->get_nv() - actuation->get_nu(),
44 constraints->get_ng_T(), constraints->get_nh_T()),
45 150 actuation_(actuation),
46 150 costs_(costs),
47 150 constraints_(constraints),
48
1/2
✓ Branch 11 taken 150 times.
✗ Branch 12 not taken.
 600 pinocchio_(*state->get_pinocchio().get()) {
49
1/2
✓ Branch 1 taken 150 times.
✗ Branch 2 not taken.
 150 init(state);
50 150 }
51
52 template <typename Scalar>
53 151 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::init(
54 const std::shared_ptr<StateMultibody>& state) {
55
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 151 times.
 151 if (costs_->get_nu() != nu_) {
56 ✗ throw_pretty(
57 "Invalid argument: "
58 << "Costs doesn't have the same control dimension (it should be " +
59 std::to_string(nu_) + ")");
60 }
61
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 151 times.
 151 if (constraints_->get_nu() != nu_) {
62 ✗ throw_pretty("Invalid argument: "
63 << "Constraints doesn't have the same control dimension (it "
64 "should be " +
65 std::to_string(nu_) + ")");
66 }
67
1/2
✓ Branch 1 taken 151 times.
✗ Branch 2 not taken.
 151 VectorXs lb =
68
1/2
✓ Branch 2 taken 151 times.
✗ Branch 3 not taken.
 151 VectorXs::Constant(nu_, -std::numeric_limits<Scalar>::infinity());
69
1/2
✓ Branch 1 taken 151 times.
✗ Branch 2 not taken.
 151 VectorXs ub =
70
1/2
✓ Branch 2 taken 151 times.
✗ Branch 3 not taken.
 151 VectorXs::Constant(nu_, std::numeric_limits<Scalar>::infinity());
71
1/2
✓ Branch 1 taken 151 times.
✗ Branch 2 not taken.
 151 Base::set_u_lb(lb);
72
1/2
✓ Branch 1 taken 151 times.
✗ Branch 2 not taken.
 151 Base::set_u_ub(ub);
73
74
2/2
✓ Branch 4 taken 51 times.
✓ Branch 5 taken 100 times.
 151 if (state->get_nv() - actuation_->get_nu() > 0) {
75
2/4
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
 102 constraints_->addConstraint(
76 "tau",
77 std::make_shared<ConstraintModelResidual>(
78
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 state_,
79 std::make_shared<typename DifferentialActionModelFreeInvDynamicsTpl<
80
1/2
✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
 51 Scalar>::ResidualModelActuation>(state, actuation_->get_nu()),
81 102 false));
82 }
83 151 }
84
85 template <typename Scalar>
86 306 DifferentialActionModelFreeInvDynamicsTpl<
87 306 Scalar>::~DifferentialActionModelFreeInvDynamicsTpl() {}
88
89 template <typename Scalar>
90 7141 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calc(
91 const std::shared_ptr<DifferentialActionDataAbstract>& data,
92 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
93
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 7141 times.
 7141 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
94 ✗ throw_pretty(
95 "Invalid argument: " << "x has wrong dimension (it should be " +
96 std::to_string(state_->get_nx()) + ")");
97 }
98
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7141 times.
 7141 if (static_cast<std::size_t>(u.size()) != nu_) {
99 ✗ throw_pretty(
100 "Invalid argument: " << "u has wrong dimension (it should be " +
101 std::to_string(nu_) + ")");
102 }
103 7141 Data* d = static_cast<Data*>(data.get());
104 7141 const std::size_t nv = state_->get_nv();
105 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
106
1/2
✓ Branch 3 taken 7141 times.
✗ Branch 4 not taken.
 7141 x.head(state_->get_nq());
107 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
108
1/2
✓ Branch 1 taken 7141 times.
✗ Branch 2 not taken.
 7141 x.tail(nv);
109
110
1/2
✓ Branch 1 taken 7141 times.
✗ Branch 2 not taken.
 7141 d->xout = u;
111
1/2
✓ Branch 1 taken 7141 times.
✗ Branch 2 not taken.
 7141 pinocchio::rnea(pinocchio_, d->pinocchio, q, v, u);
112
1/2
✓ Branch 1 taken 7141 times.
✗ Branch 2 not taken.
 7141 pinocchio::updateGlobalPlacements(pinocchio_, d->pinocchio);
113
2/4
✓ Branch 2 taken 7141 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7141 times.
✗ Branch 6 not taken.
 7141 actuation_->commands(d->multibody.actuation, x, d->pinocchio.tau);
114
1/2
✓ Branch 2 taken 7141 times.
✗ Branch 3 not taken.
 7141 d->multibody.joint->a = u;
115
1/2
✓ Branch 3 taken 7141 times.
✗ Branch 4 not taken.
 7141 d->multibody.joint->tau = d->multibody.actuation->u;
116
2/4
✓ Branch 3 taken 7141 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7141 times.
✗ Branch 7 not taken.
 7141 actuation_->calc(d->multibody.actuation, x, d->multibody.joint->tau);
117
1/2
✓ Branch 2 taken 7141 times.
✗ Branch 3 not taken.
 7141 costs_->calc(d->costs, x, u);
118 7141 d->cost = d->costs->cost;
119
1/2
✓ Branch 2 taken 7141 times.
✗ Branch 3 not taken.
 7141 d->constraints->resize(this, d);
120
1/2
✓ Branch 2 taken 7141 times.
✗ Branch 3 not taken.
 7141 constraints_->calc(d->constraints, x, u);
121 7141 }
122
123 template <typename Scalar>
124 571 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calc(
125 const std::shared_ptr<DifferentialActionDataAbstract>& data,
126 const Eigen::Ref<const VectorXs>& x) {
127
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 571 times.
 571 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
128 ✗ throw_pretty(
129 "Invalid argument: " << "x has wrong dimension (it should be " +
130 std::to_string(state_->get_nx()) + ")");
131 }
132
133 571 Data* d = static_cast<Data*>(data.get());
134 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
135
1/2
✓ Branch 3 taken 571 times.
✗ Branch 4 not taken.
 571 x.head(state_->get_nq());
136 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
137
1/2
✓ Branch 3 taken 571 times.
✗ Branch 4 not taken.
 571 x.tail(state_->get_nv());
138
139
1/2
✓ Branch 1 taken 571 times.
✗ Branch 2 not taken.
 571 pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
140
141
1/2
✓ Branch 2 taken 571 times.
✗ Branch 3 not taken.
 571 costs_->calc(d->costs, x);
142 571 d->cost = d->costs->cost;
143
1/2
✓ Branch 2 taken 571 times.
✗ Branch 3 not taken.
 571 d->constraints->resize(this, d, false);
144
1/2
✓ Branch 2 taken 571 times.
✗ Branch 3 not taken.
 571 constraints_->calc(d->constraints, x);
145 571 }
146
147 template <typename Scalar>
148 1881 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calcDiff(
149 const std::shared_ptr<DifferentialActionDataAbstract>& data,
150 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
151
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1881 times.
 1881 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
152 ✗ throw_pretty(
153 "Invalid argument: " << "x has wrong dimension (it should be " +
154 std::to_string(state_->get_nx()) + ")");
155 }
156
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1881 times.
 1881 if (static_cast<std::size_t>(u.size()) != nu_) {
157 ✗ throw_pretty(
158 "Invalid argument: " << "u has wrong dimension (it should be " +
159 std::to_string(nu_) + ")");
160 }
161 1881 Data* d = static_cast<Data*>(data.get());
162 1881 const std::size_t nv = state_->get_nv();
163 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
164
1/2
✓ Branch 3 taken 1881 times.
✗ Branch 4 not taken.
 1881 x.head(state_->get_nq());
165 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
166
1/2
✓ Branch 1 taken 1881 times.
✗ Branch 2 not taken.
 1881 x.tail(nv);
167
168
1/2
✓ Branch 1 taken 1881 times.
✗ Branch 2 not taken.
 1881 pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, v, u);
169
2/4
✓ Branch 1 taken 1881 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1881 times.
✗ Branch 5 not taken.
 1881 d->pinocchio.M.template triangularView<Eigen::StrictlyLower>() =
170
1/2
✓ Branch 1 taken 1881 times.
✗ Branch 2 not taken.
 1881 d->pinocchio.M.template triangularView<Eigen::StrictlyUpper>()
171
1/2
✓ Branch 1 taken 1881 times.
✗ Branch 2 not taken.
 1881 .transpose();
172
2/4
✓ Branch 3 taken 1881 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1881 times.
✗ Branch 7 not taken.
 1881 actuation_->calcDiff(d->multibody.actuation, x, d->multibody.joint->tau);
173
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 3762 actuation_->torqueTransform(d->multibody.actuation, x,
174
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 d->multibody.joint->tau);
175
3/6
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1881 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1881 times.
✗ Branch 9 not taken.
 1881 d->multibody.joint->dtau_dx.leftCols(nv).noalias() =
176
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 d->multibody.actuation->Mtau * d->pinocchio.dtau_dq;
177
3/6
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1881 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1881 times.
✗ Branch 9 not taken.
 1881 d->multibody.joint->dtau_dx.rightCols(nv).noalias() =
178
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 d->multibody.actuation->Mtau * d->pinocchio.dtau_dv;
179
2/4
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1881 times.
✗ Branch 6 not taken.
 1881 d->multibody.joint->dtau_du.noalias() =
180
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 d->multibody.actuation->Mtau * d->pinocchio.M;
181
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 costs_->calcDiff(d->costs, x, u);
182
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 constraints_->calcDiff(d->constraints, x, u);
183 1881 }
184
185 template <typename Scalar>
186 66 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calcDiff(
187 const std::shared_ptr<DifferentialActionDataAbstract>& data,
188 const Eigen::Ref<const VectorXs>& x) {
189
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 66 times.
 66 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
190 ✗ throw_pretty(
191 "Invalid argument: " << "x has wrong dimension (it should be " +
192 std::to_string(state_->get_nx()) + ")");
193 }
194 66 Data* d = static_cast<Data*>(data.get());
195
196 66 costs_->calcDiff(d->costs, x);
197 66 constraints_->calcDiff(d->constraints, x);
198 66 }
199
200 template <typename Scalar>
201 std::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
202 9894 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::createData() {
203
1/2
✓ Branch 2 taken 9894 times.
✗ Branch 3 not taken.
 9894 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
204 }
205
206 template <typename Scalar>
207 1971 bool DifferentialActionModelFreeInvDynamicsTpl<Scalar>::checkData(
208 const std::shared_ptr<DifferentialActionDataAbstract>& data) {
209 1971 std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
210
1/2
✓ Branch 1 taken 1971 times.
✗ Branch 2 not taken.
 1971 if (d != NULL) {
211 1971 return true;
212 } else {
213 ✗ return false;
214 }
215 1971 }
216
217 template <typename Scalar>
218 963 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::quasiStatic(
219 const std::shared_ptr<DifferentialActionDataAbstract>&,
220 Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>&,
221 const std::size_t, const Scalar) {
222 963 u.setZero();
223 963 }
224
225 template <typename Scalar>
226 30 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::print(
227 std::ostream& os) const {
228 30 os << "DifferentialActionModelFreeInvDynamics {nx=" << state_->get_nx()
229 30 << ", ndx=" << state_->get_ndx() << ", nu=" << nu_ << "}";
230 30 }
231
232 template <typename Scalar>
233 49135 std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_ng() const {
234
1/2
✓ Branch 1 taken 49135 times.
✗ Branch 2 not taken.
 49135 if (constraints_ != nullptr) {
235 49135 return constraints_->get_ng();
236 } else {
237 ✗ return Base::get_ng();
238 }
239 }
240
241 template <typename Scalar>
242 62545 std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_nh() const {
243
1/2
✓ Branch 1 taken 62545 times.
✗ Branch 2 not taken.
 62545 if (constraints_ != nullptr) {
244 62545 return constraints_->get_nh();
245 } else {
246 ✗ return Base::get_nh();
247 }
248 }
249
250 template <typename Scalar>
251 82723 std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_ng_T()
252 const {
253
1/2
✓ Branch 1 taken 82723 times.
✗ Branch 2 not taken.
 82723 if (constraints_ != nullptr) {
254 82723 return constraints_->get_ng_T();
255 } else {
256 ✗ return Base::get_ng_T();
257 }
258 }
259
260 template <typename Scalar>
261 69313 std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_nh_T()
262 const {
263
1/2
✓ Branch 1 taken 69313 times.
✗ Branch 2 not taken.
 69313 if (constraints_ != nullptr) {
264 69313 return constraints_->get_nh_T();
265 } else {
266 ✗ return Base::get_nh_T();
267 }
268 }
269
270 template <typename Scalar>
271 const typename MathBaseTpl<Scalar>::VectorXs&
272 ✗ DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_g_lb() const {
273 ✗ if (constraints_ != nullptr) {
274 ✗ return constraints_->get_lb();
275 } else {
276 ✗ return g_lb_;
277 }
278 }
279
280 template <typename Scalar>
281 const typename MathBaseTpl<Scalar>::VectorXs&
282 ✗ DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_g_ub() const {
283 ✗ if (constraints_ != nullptr) {
284 ✗ return constraints_->get_ub();
285 } else {
286 ✗ return g_lb_;
287 }
288 }
289
290 template <typename Scalar>
291 const std::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
292 19788 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_actuation() const {
293 19788 return actuation_;
294 }
295
296 template <typename Scalar>
297 const std::shared_ptr<CostModelSumTpl<Scalar> >&
298 9894 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_costs() const {
299 9894 return costs_;
300 }
301
302 template <typename Scalar>
303 const std::shared_ptr<ConstraintModelManagerTpl<Scalar> >&
304 9894 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_constraints() const {
305 9894 return constraints_;
306 }
307
308 template <typename Scalar>
309 pinocchio::ModelTpl<Scalar>&
310 9894 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_pinocchio() const {
311 9894 return pinocchio_;
312 }
313
314 } // namespace crocoddyl
315