GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/actions/free-invdyn.hxx
Date: 2025-01-16 08:47:40
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 boost::shared_ptr<StateMultibody> state,
23 boost::shared_ptr<ActuationModelAbstract> actuation,
24 boost::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 boost::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 boost::shared_ptr<StateMultibody> state,
39 boost::shared_ptr<ActuationModelAbstract> actuation,
40 boost::shared_ptr<CostModelSum> costs,
41 boost::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 boost::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", boost::make_shared<ConstraintModelResidual>(
77
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 state_,
78 boost::make_shared<
79 typename DifferentialActionModelFreeInvDynamicsTpl<
80 Scalar>::ResidualModelActuation>(
81
1/2
✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
 51 state, actuation_->get_nu()),
82 102 false));
83 }
84 151 }
85
86 template <typename Scalar>
87 306 DifferentialActionModelFreeInvDynamicsTpl<
88 306 Scalar>::~DifferentialActionModelFreeInvDynamicsTpl() {}
89
90 template <typename Scalar>
91 7141 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calc(
92 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
93 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
94
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 7141 times.
 7141 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
95 ✗ throw_pretty(
96 "Invalid argument: " << "x has wrong dimension (it should be " +
97 std::to_string(state_->get_nx()) + ")");
98 }
99
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7141 times.
 7141 if (static_cast<std::size_t>(u.size()) != nu_) {
100 ✗ throw_pretty(
101 "Invalid argument: " << "u has wrong dimension (it should be " +
102 std::to_string(nu_) + ")");
103 }
104 7141 Data* d = static_cast<Data*>(data.get());
105 7141 const std::size_t nv = state_->get_nv();
106 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
107
1/2
✓ Branch 3 taken 7141 times.
✗ Branch 4 not taken.
 7141 x.head(state_->get_nq());
108 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
109
1/2
✓ Branch 1 taken 7141 times.
✗ Branch 2 not taken.
 7141 x.tail(nv);
110
111
1/2
✓ Branch 1 taken 7141 times.
✗ Branch 2 not taken.
 7141 d->xout = u;
112
1/2
✓ Branch 1 taken 7141 times.
✗ Branch 2 not taken.
 7141 pinocchio::rnea(pinocchio_, d->pinocchio, q, v, u);
113
1/2
✓ Branch 1 taken 7141 times.
✗ Branch 2 not taken.
 7141 pinocchio::updateGlobalPlacements(pinocchio_, d->pinocchio);
114
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);
115
1/2
✓ Branch 2 taken 7141 times.
✗ Branch 3 not taken.
 7141 d->multibody.joint->a = u;
116
1/2
✓ Branch 3 taken 7141 times.
✗ Branch 4 not taken.
 7141 d->multibody.joint->tau = d->multibody.actuation->u;
117
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);
118
1/2
✓ Branch 2 taken 7141 times.
✗ Branch 3 not taken.
 7141 costs_->calc(d->costs, x, u);
119 7141 d->cost = d->costs->cost;
120
1/2
✓ Branch 2 taken 7141 times.
✗ Branch 3 not taken.
 7141 d->constraints->resize(this, d);
121
1/2
✓ Branch 2 taken 7141 times.
✗ Branch 3 not taken.
 7141 constraints_->calc(d->constraints, x, u);
122 7141 }
123
124 template <typename Scalar>
125 571 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calc(
126 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
127 const Eigen::Ref<const VectorXs>& x) {
128
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 571 times.
 571 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
129 ✗ throw_pretty(
130 "Invalid argument: " << "x has wrong dimension (it should be " +
131 std::to_string(state_->get_nx()) + ")");
132 }
133
134 571 Data* d = static_cast<Data*>(data.get());
135 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
136
1/2
✓ Branch 3 taken 571 times.
✗ Branch 4 not taken.
 571 x.head(state_->get_nq());
137 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
138
1/2
✓ Branch 3 taken 571 times.
✗ Branch 4 not taken.
 571 x.tail(state_->get_nv());
139
140
1/2
✓ Branch 1 taken 571 times.
✗ Branch 2 not taken.
 571 pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
141
142
1/2
✓ Branch 2 taken 571 times.
✗ Branch 3 not taken.
 571 costs_->calc(d->costs, x);
143 571 d->cost = d->costs->cost;
144
1/2
✓ Branch 2 taken 571 times.
✗ Branch 3 not taken.
 571 d->constraints->resize(this, d, false);
145
1/2
✓ Branch 2 taken 571 times.
✗ Branch 3 not taken.
 571 constraints_->calc(d->constraints, x);
146 571 }
147
148 template <typename Scalar>
149 1881 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calcDiff(
150 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
151 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
152
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1881 times.
 1881 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
153 ✗ throw_pretty(
154 "Invalid argument: " << "x has wrong dimension (it should be " +
155 std::to_string(state_->get_nx()) + ")");
156 }
157
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1881 times.
 1881 if (static_cast<std::size_t>(u.size()) != nu_) {
158 ✗ throw_pretty(
159 "Invalid argument: " << "u has wrong dimension (it should be " +
160 std::to_string(nu_) + ")");
161 }
162 1881 Data* d = static_cast<Data*>(data.get());
163 1881 const std::size_t nv = state_->get_nv();
164 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
165
1/2
✓ Branch 3 taken 1881 times.
✗ Branch 4 not taken.
 1881 x.head(state_->get_nq());
166 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
167
1/2
✓ Branch 1 taken 1881 times.
✗ Branch 2 not taken.
 1881 x.tail(nv);
168
169
1/2
✓ Branch 1 taken 1881 times.
✗ Branch 2 not taken.
 1881 pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, v, u);
170
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>() =
171
1/2
✓ Branch 1 taken 1881 times.
✗ Branch 2 not taken.
 1881 d->pinocchio.M.template triangularView<Eigen::StrictlyUpper>()
172
1/2
✓ Branch 1 taken 1881 times.
✗ Branch 2 not taken.
 1881 .transpose();
173
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);
174
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 3762 actuation_->torqueTransform(d->multibody.actuation, x,
175
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 d->multibody.joint->tau);
176
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() =
177
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 d->multibody.actuation->Mtau * d->pinocchio.dtau_dq;
178
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() =
179
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 d->multibody.actuation->Mtau * d->pinocchio.dtau_dv;
180
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() =
181
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 d->multibody.actuation->Mtau * d->pinocchio.M;
182
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 costs_->calcDiff(d->costs, x, u);
183
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 constraints_->calcDiff(d->constraints, x, u);
184 1881 }
185
186 template <typename Scalar>
187 66 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calcDiff(
188 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
189 const Eigen::Ref<const VectorXs>& x) {
190
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 66 times.
 66 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
191 ✗ throw_pretty(
192 "Invalid argument: " << "x has wrong dimension (it should be " +
193 std::to_string(state_->get_nx()) + ")");
194 }
195 66 Data* d = static_cast<Data*>(data.get());
196
197 66 costs_->calcDiff(d->costs, x);
198 66 constraints_->calcDiff(d->constraints, x);
199 66 }
200
201 template <typename Scalar>
202 boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
203 9894 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::createData() {
204
1/2
✓ Branch 2 taken 9894 times.
✗ Branch 3 not taken.
 9894 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
205 }
206
207 template <typename Scalar>
208 1971 bool DifferentialActionModelFreeInvDynamicsTpl<Scalar>::checkData(
209 const boost::shared_ptr<DifferentialActionDataAbstract>& data) {
210 1971 boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
211
1/2
✓ Branch 1 taken 1971 times.
✗ Branch 2 not taken.
 1971 if (d != NULL) {
212 1971 return true;
213 } else {
214 ✗ return false;
215 }
216 1971 }
217
218 template <typename Scalar>
219 963 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::quasiStatic(
220 const boost::shared_ptr<DifferentialActionDataAbstract>&,
221 Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>&,
222 const std::size_t, const Scalar) {
223 963 u.setZero();
224 963 }
225
226 template <typename Scalar>
227 30 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::print(
228 std::ostream& os) const {
229 30 os << "DifferentialActionModelFreeInvDynamics {nx=" << state_->get_nx()
230 30 << ", ndx=" << state_->get_ndx() << ", nu=" << nu_ << "}";
231 30 }
232
233 template <typename Scalar>
234 49135 std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_ng() const {
235
1/2
✓ Branch 1 taken 49135 times.
✗ Branch 2 not taken.
 49135 if (constraints_ != nullptr) {
236 49135 return constraints_->get_ng();
237 } else {
238 ✗ return Base::get_ng();
239 }
240 }
241
242 template <typename Scalar>
243 62545 std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_nh() const {
244
1/2
✓ Branch 1 taken 62545 times.
✗ Branch 2 not taken.
 62545 if (constraints_ != nullptr) {
245 62545 return constraints_->get_nh();
246 } else {
247 ✗ return Base::get_nh();
248 }
249 }
250
251 template <typename Scalar>
252 82723 std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_ng_T()
253 const {
254
1/2
✓ Branch 1 taken 82723 times.
✗ Branch 2 not taken.
 82723 if (constraints_ != nullptr) {
255 82723 return constraints_->get_ng_T();
256 } else {
257 ✗ return Base::get_ng_T();
258 }
259 }
260
261 template <typename Scalar>
262 69313 std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_nh_T()
263 const {
264
1/2
✓ Branch 1 taken 69313 times.
✗ Branch 2 not taken.
 69313 if (constraints_ != nullptr) {
265 69313 return constraints_->get_nh_T();
266 } else {
267 ✗ return Base::get_nh_T();
268 }
269 }
270
271 template <typename Scalar>
272 const typename MathBaseTpl<Scalar>::VectorXs&
273 ✗ DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_g_lb() const {
274 ✗ if (constraints_ != nullptr) {
275 ✗ return constraints_->get_lb();
276 } else {
277 ✗ return g_lb_;
278 }
279 }
280
281 template <typename Scalar>
282 const typename MathBaseTpl<Scalar>::VectorXs&
283 ✗ DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_g_ub() const {
284 ✗ if (constraints_ != nullptr) {
285 ✗ return constraints_->get_ub();
286 } else {
287 ✗ return g_lb_;
288 }
289 }
290
291 template <typename Scalar>
292 const boost::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
293 19788 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_actuation() const {
294 19788 return actuation_;
295 }
296
297 template <typename Scalar>
298 const boost::shared_ptr<CostModelSumTpl<Scalar> >&
299 9894 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_costs() const {
300 9894 return costs_;
301 }
302
303 template <typename Scalar>
304 const boost::shared_ptr<ConstraintModelManagerTpl<Scalar> >&
305 9894 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_constraints() const {
306 9894 return constraints_;
307 }
308
309 template <typename Scalar>
310 pinocchio::ModelTpl<Scalar>&
311 9894 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_pinocchio() const {
312 9894 return pinocchio_;
313 }
314
315 } // namespace crocoddyl
316