GCC Code Coverage Report

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