GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/integrator/euler.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 101 120 84.2%
Branches: 93 338 27.5%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5 // University of Oxford, University of Pisa,
6 // Heriot-Watt University
7 // Copyright note valid unless otherwise stated in individual files.
8 // All rights reserved.
9 ///////////////////////////////////////////////////////////////////////////////
10
11 #include <boost/core/demangle.hpp>
12 #include <iostream>
13 #include <typeinfo>
14
15 namespace crocoddyl {
16
17 template <typename Scalar>
18 161 IntegratedActionModelEulerTpl<Scalar>::IntegratedActionModelEulerTpl(
19 std::shared_ptr<DifferentialActionModelAbstract> model,
20 std::shared_ptr<ControlParametrizationModelAbstract> control,
21 const Scalar time_step, const bool with_cost_residual)
22
1/2
✓ Branch 3 taken 161 times.
✗ Branch 4 not taken.
 161 : Base(model, control, time_step, with_cost_residual) {}
23
24 template <typename Scalar>
25 98 IntegratedActionModelEulerTpl<Scalar>::IntegratedActionModelEulerTpl(
26 std::shared_ptr<DifferentialActionModelAbstract> model,
27 const Scalar time_step, const bool with_cost_residual)
28
1/2
✓ Branch 2 taken 98 times.
✗ Branch 3 not taken.
 98 : Base(model, time_step, with_cost_residual) {}
29
30 template <typename Scalar>
31 6350 void IntegratedActionModelEulerTpl<Scalar>::calc(
32 const std::shared_ptr<ActionDataAbstract>& data,
33 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
34
2/4
✓ Branch 3 taken 6350 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 6350 times.
 6350 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
35 ✗ throw_pretty(
36 "Invalid argument: " << "x has wrong dimension (it should be " +
37 std::to_string(state_->get_nx()) + ")");
38 }
39
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6350 times.
 6350 if (static_cast<std::size_t>(u.size()) != nu_) {
40 ✗ throw_pretty(
41 "Invalid argument: " << "u has wrong dimension (it should be " +
42 std::to_string(nu_) + ")");
43 }
44
2/4
✓ Branch 2 taken 6350 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 6350 times.
✗ Branch 7 not taken.
 6350 const std::size_t nv = differential_->get_state()->get_nv();
45 6350 Data* d = static_cast<Data*>(data.get());
46 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
47
1/2
✓ Branch 1 taken 6350 times.
✗ Branch 2 not taken.
 6350 x.tail(nv);
48
49
1/2
✓ Branch 2 taken 6350 times.
✗ Branch 3 not taken.
 6350 control_->calc(d->control, Scalar(0.), u);
50
2/4
✓ Branch 3 taken 6350 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6350 times.
✗ Branch 7 not taken.
 6350 differential_->calc(d->differential, x, d->control->w);
51 6350 const VectorXs& a = d->differential->xout;
52
6/12
✓ Branch 1 taken 6350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6350 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6350 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6350 times.
✗ Branch 17 not taken.
 6350 d->dx.head(nv).noalias() = v * time_step_ + a * time_step2_;
53
4/8
✓ Branch 1 taken 6350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6350 times.
✗ Branch 11 not taken.
 6350 d->dx.tail(nv).noalias() = a * time_step_;
54
4/8
✓ Branch 2 taken 6350 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 6350 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6350 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 6350 times.
✗ Branch 13 not taken.
 6350 differential_->get_state()->integrate(x, d->dx, d->xnext);
55 6350 d->cost = time_step_ * d->differential->cost;
56
1/2
✓ Branch 2 taken 6350 times.
✗ Branch 3 not taken.
 6350 d->g = d->differential->g;
57
1/2
✓ Branch 2 taken 6350 times.
✗ Branch 3 not taken.
 6350 d->h = d->differential->h;
58
1/2
✓ Branch 0 taken 6350 times.
✗ Branch 1 not taken.
 6350 if (with_cost_residual_) {
59
1/2
✓ Branch 2 taken 6350 times.
✗ Branch 3 not taken.
 6350 d->r = d->differential->r;
60 }
61 6350 }
62
63 template <typename Scalar>
64 1192 void IntegratedActionModelEulerTpl<Scalar>::calc(
65 const std::shared_ptr<ActionDataAbstract>& data,
66 const Eigen::Ref<const VectorXs>& x) {
67
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1192 times.
 1192 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
68 ✗ throw_pretty(
69 "Invalid argument: " << "x has wrong dimension (it should be " +
70 std::to_string(state_->get_nx()) + ")");
71 }
72 1192 Data* d = static_cast<Data*>(data.get());
73
74 1192 differential_->calc(d->differential, x);
75 1192 d->dx.setZero();
76 1192 d->xnext = x;
77 1192 d->cost = d->differential->cost;
78 1192 d->g = d->differential->g;
79 1192 d->h = d->differential->h;
80
1/2
✓ Branch 0 taken 1192 times.
✗ Branch 1 not taken.
 1192 if (with_cost_residual_) {
81 1192 d->r = d->differential->r;
82 }
83 1192 }
84
85 template <typename Scalar>
86 2094 void IntegratedActionModelEulerTpl<Scalar>::calcDiff(
87 const std::shared_ptr<ActionDataAbstract>& data,
88 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
89
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 2094 times.
 2094 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
90 ✗ throw_pretty(
91 "Invalid argument: " << "x has wrong dimension (it should be " +
92 std::to_string(state_->get_nx()) + ")");
93 }
94
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2094 times.
 2094 if (static_cast<std::size_t>(u.size()) != nu_) {
95 ✗ throw_pretty(
96 "Invalid argument: " << "u has wrong dimension (it should be " +
97 std::to_string(nu_) + ")");
98 }
99
100 2094 const std::size_t nv = state_->get_nv();
101 2094 Data* d = static_cast<Data*>(data.get());
102
103 2094 control_->calc(d->control, Scalar(0.), u);
104
1/2
✓ Branch 4 taken 2094 times.
✗ Branch 5 not taken.
 2094 differential_->calcDiff(d->differential, x, d->control->w);
105 2094 const MatrixXs& da_dx = d->differential->Fx;
106 2094 const MatrixXs& da_du = d->differential->Fu;
107
2/4
✓ Branch 3 taken 2094 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2094 times.
✗ Branch 7 not taken.
 2094 control_->multiplyByJacobian(d->control, da_du, d->da_du);
108
3/6
✓ Branch 2 taken 2094 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2094 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2094 times.
✗ Branch 9 not taken.
 2094 d->Fx.topRows(nv).noalias() = da_dx * time_step2_;
109
3/6
✓ Branch 2 taken 2094 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2094 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2094 times.
✗ Branch 9 not taken.
 2094 d->Fx.bottomRows(nv).noalias() = da_dx * time_step_;
110
4/8
✓ Branch 1 taken 2094 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2094 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2094 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2094 times.
✗ Branch 11 not taken.
 2094 d->Fx.topRightCorner(nv, nv).diagonal().array() += Scalar(time_step_);
111
3/6
✓ Branch 2 taken 2094 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2094 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2094 times.
✗ Branch 9 not taken.
 2094 d->Fu.topRows(nv).noalias() = time_step2_ * d->da_du;
112
3/6
✓ Branch 2 taken 2094 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2094 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2094 times.
✗ Branch 9 not taken.
 2094 d->Fu.bottomRows(nv).noalias() = time_step_ * d->da_du;
113
2/4
✓ Branch 3 taken 2094 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2094 times.
✗ Branch 7 not taken.
 2094 state_->JintegrateTransport(x, d->dx, d->Fx, second);
114
3/6
✓ Branch 3 taken 2094 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2094 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2094 times.
✗ Branch 10 not taken.
 2094 state_->Jintegrate(x, d->dx, d->Fx, d->Fx, first, addto);
115
2/4
✓ Branch 3 taken 2094 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2094 times.
✗ Branch 7 not taken.
 2094 state_->JintegrateTransport(x, d->dx, d->Fu, second);
116
117
2/4
✓ Branch 3 taken 2094 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2094 times.
✗ Branch 7 not taken.
 2094 d->Lx.noalias() = time_step_ * d->differential->Lx;
118
2/4
✓ Branch 4 taken 2094 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2094 times.
✗ Branch 8 not taken.
 2094 control_->multiplyJacobianTransposeBy(d->control, d->differential->Lu, d->Lu);
119 2094 d->Lu *= time_step_;
120
2/4
✓ Branch 3 taken 2094 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2094 times.
✗ Branch 7 not taken.
 2094 d->Lxx.noalias() = time_step_ * d->differential->Lxx;
121
2/4
✓ Branch 4 taken 2094 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2094 times.
✗ Branch 8 not taken.
 2094 control_->multiplyByJacobian(d->control, d->differential->Lxu, d->Lxu);
122 2094 d->Lxu *= time_step_;
123
2/4
✓ Branch 4 taken 2094 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2094 times.
✗ Branch 8 not taken.
 2094 control_->multiplyByJacobian(d->control, d->differential->Luu, d->Lwu);
124
2/4
✓ Branch 3 taken 2094 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2094 times.
✗ Branch 7 not taken.
 2094 control_->multiplyJacobianTransposeBy(d->control, d->Lwu, d->Luu);
125 2094 d->Luu *= time_step_;
126 2094 d->Gx = d->differential->Gx;
127 2094 d->Hx = d->differential->Hx;
128 2094 d->Gu.conservativeResize(differential_->get_ng(), nu_);
129 2094 d->Hu.conservativeResize(differential_->get_nh(), nu_);
130
2/4
✓ Branch 4 taken 2094 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2094 times.
✗ Branch 8 not taken.
 2094 control_->multiplyByJacobian(d->control, d->differential->Gu, d->Gu);
131
2/4
✓ Branch 4 taken 2094 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2094 times.
✗ Branch 8 not taken.
 2094 control_->multiplyByJacobian(d->control, d->differential->Hu, d->Hu);
132 2094 }
133
134 template <typename Scalar>
135 159 void IntegratedActionModelEulerTpl<Scalar>::calcDiff(
136 const std::shared_ptr<ActionDataAbstract>& data,
137 const Eigen::Ref<const VectorXs>& x) {
138
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 159 times.
 159 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
139 ✗ throw_pretty(
140 "Invalid argument: " << "x has wrong dimension (it should be " +
141 std::to_string(state_->get_nx()) + ")");
142 }
143 159 Data* d = static_cast<Data*>(data.get());
144
145 159 differential_->calcDiff(d->differential, x);
146
3/6
✓ Branch 3 taken 159 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 159 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 159 times.
✗ Branch 10 not taken.
 159 state_->Jintegrate(x, d->dx, d->Fx, d->Fx);
147 159 d->Lx = d->differential->Lx;
148 159 d->Lxx = d->differential->Lxx;
149 159 d->Gx = d->differential->Gx;
150 159 d->Hx = d->differential->Hx;
151 159 }
152
153 template <typename Scalar>
154 std::shared_ptr<ActionDataAbstractTpl<Scalar> >
155 7685 IntegratedActionModelEulerTpl<Scalar>::createData() {
156
2/2
✓ Branch 4 taken 69 times.
✓ Branch 5 taken 7616 times.
 7685 if (control_->get_nu() > differential_->get_nu())
157 69 std::cerr << "Warning: It is useless to use an Euler integrator with a "
158 "control parametrization larger than PolyZero"
159 69 << std::endl;
160
1/2
✓ Branch 2 taken 7685 times.
✗ Branch 3 not taken.
 7685 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
161 }
162
163 template <typename Scalar>
164 template <typename NewScalar>
165 IntegratedActionModelEulerTpl<NewScalar>
166 ✗ IntegratedActionModelEulerTpl<Scalar>::cast() const {
167 typedef IntegratedActionModelEulerTpl<NewScalar> ReturnType;
168 ✗ if (control_) {
169 ✗ ReturnType ret(differential_->template cast<NewScalar>(),
170 ✗ control_->template cast<NewScalar>(),
171 ✗ scalar_cast<NewScalar>(time_step_), with_cost_residual_);
172 ✗ return ret;
173 ✗ } else {
174 ✗ ReturnType ret(differential_->template cast<NewScalar>(),
175 ✗ scalar_cast<NewScalar>(time_step_), with_cost_residual_);
176 ✗ return ret;
177 }
178 }
179
180 template <typename Scalar>
181 1472 bool IntegratedActionModelEulerTpl<Scalar>::checkData(
182 const std::shared_ptr<ActionDataAbstract>& data) {
183 1472 std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
184
1/2
✓ Branch 1 taken 1472 times.
✗ Branch 2 not taken.
 1472 if (data != NULL) {
185
1/2
✓ Branch 3 taken 1472 times.
✗ Branch 4 not taken.
 1472 return differential_->checkData(d->differential);
186 } else {
187 ✗ return false;
188 }
189 1472 }
190
191 template <typename Scalar>
192 1840 void IntegratedActionModelEulerTpl<Scalar>::quasiStatic(
193 const std::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
194 const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter,
195 const Scalar tol) {
196
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1840 times.
 1840 if (static_cast<std::size_t>(u.size()) != nu_) {
197 ✗ throw_pretty(
198 "Invalid argument: " << "u has wrong dimension (it should be " +
199 std::to_string(nu_) + ")");
200 }
201
2/4
✓ Branch 3 taken 1840 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1840 times.
 1840 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
202 ✗ throw_pretty(
203 "Invalid argument: " << "x has wrong dimension (it should be " +
204 std::to_string(state_->get_nx()) + ")");
205 }
206
207 1840 const std::shared_ptr<Data>& d = std::static_pointer_cast<Data>(data);
208
209
1/2
✓ Branch 3 taken 1840 times.
✗ Branch 4 not taken.
 1840 d->control->w.setZero();
210
2/4
✓ Branch 4 taken 1840 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1840 times.
✗ Branch 9 not taken.
 1840 differential_->quasiStatic(d->differential, d->control->w, x, maxiter, tol);
211
2/4
✓ Branch 4 taken 1840 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1840 times.
✗ Branch 9 not taken.
 1840 control_->params(d->control, Scalar(0.), d->control->w);
212
1/2
✓ Branch 3 taken 1840 times.
✗ Branch 4 not taken.
 1840 u = d->control->u;
213 1840 }
214
215 template <typename Scalar>
216 23 void IntegratedActionModelEulerTpl<Scalar>::print(std::ostream& os) const {
217 23 os << "IntegratedActionModelEuler {dt=" << time_step_ << ", "
218 23 << *differential_ << "}";
219 23 }
220
221 } // namespace crocoddyl
222