GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/state.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 82 108 75.9%
Branches: 125 502 24.9%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh, New York
5 // University,
6 // Max Planck Gesellschaft, Heriot-Watt University
7 // Copyright note valid unless otherwise stated in individual files.
8 // All rights reserved.
9 ///////////////////////////////////////////////////////////////////////////////
10
11 #include "crocoddyl/core/utils/exception.hpp"
12
13 namespace crocoddyl {
14
15 template <typename Scalar>
16 29 StateNumDiffTpl<Scalar>::StateNumDiffTpl(boost::shared_ptr<Base> state)
17 : Base(state->get_nx(), state->get_ndx()),
18 29 state_(state),
19 29 e_jac_(std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon())) {}
20
21 template <typename Scalar>
22 62 StateNumDiffTpl<Scalar>::~StateNumDiffTpl() {}
23
24 template <typename Scalar>
25 ✗ typename MathBaseTpl<Scalar>::VectorXs StateNumDiffTpl<Scalar>::zero() const {
26 ✗ return state_->zero();
27 }
28
29 template <typename Scalar>
30 ✗ typename MathBaseTpl<Scalar>::VectorXs StateNumDiffTpl<Scalar>::rand() const {
31 ✗ return state_->rand();
32 }
33
34 template <typename Scalar>
35 3580 void StateNumDiffTpl<Scalar>::diff(const Eigen::Ref<const VectorXs>& x0,
36 const Eigen::Ref<const VectorXs>& x1,
37 Eigen::Ref<VectorXs> dxout) const {
38
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3580 times.
 3580 if (static_cast<std::size_t>(x0.size()) != nx_) {
39 ✗ throw_pretty(
40 "Invalid argument: " << "x0 has wrong dimension (it should be " +
41 std::to_string(nx_) + ")");
42 }
43
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3580 times.
 3580 if (static_cast<std::size_t>(x1.size()) != nx_) {
44 ✗ throw_pretty(
45 "Invalid argument: " << "x1 has wrong dimension (it should be " +
46 std::to_string(nx_) + ")");
47 }
48
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3580 times.
 3580 if (static_cast<std::size_t>(dxout.size()) != ndx_) {
49 ✗ throw_pretty(
50 "Invalid argument: " << "dxout has wrong dimension (it should be " +
51 std::to_string(ndx_) + ")");
52 }
53
1/2
✓ Branch 3 taken 3580 times.
✗ Branch 4 not taken.
 3580 state_->diff(x0, x1, dxout);
54 3580 }
55
56 template <typename Scalar>
57 4468 void StateNumDiffTpl<Scalar>::integrate(const Eigen::Ref<const VectorXs>& x,
58 const Eigen::Ref<const VectorXs>& dx,
59 Eigen::Ref<VectorXs> xout) const {
60
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4468 times.
 4468 if (static_cast<std::size_t>(x.size()) != nx_) {
61 ✗ throw_pretty(
62 "Invalid argument: " << "x has wrong dimension (it should be " +
63 std::to_string(nx_) + ")");
64 }
65
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4468 times.
 4468 if (static_cast<std::size_t>(dx.size()) != ndx_) {
66 ✗ throw_pretty(
67 "Invalid argument: " << "dx has wrong dimension (it should be " +
68 std::to_string(ndx_) + ")");
69 }
70
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4468 times.
 4468 if (static_cast<std::size_t>(xout.size()) != nx_) {
71 ✗ throw_pretty(
72 "Invalid argument: " << "xout has wrong dimension (it should be " +
73 std::to_string(nx_) + ")");
74 }
75
1/2
✓ Branch 3 taken 4468 times.
✗ Branch 4 not taken.
 4468 state_->integrate(x, dx, xout);
76 4468 }
77
78 template <typename Scalar>
79 28 void StateNumDiffTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>& x0,
80 const Eigen::Ref<const VectorXs>& x1,
81 Eigen::Ref<MatrixXs> Jfirst,
82 Eigen::Ref<MatrixXs> Jsecond,
83 Jcomponent firstsecond) const {
84
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 28 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
 28 assert_pretty(
85 is_a_Jcomponent(firstsecond),
86 ("firstsecond must be one of the Jcomponent {both, first, second}"));
87
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 28 times.
 28 if (static_cast<std::size_t>(x0.size()) != nx_) {
88 ✗ throw_pretty(
89 "Invalid argument: " << "x0 has wrong dimension (it should be " +
90 std::to_string(nx_) + ")");
91 }
92
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 28 times.
 28 if (static_cast<std::size_t>(x1.size()) != nx_) {
93 ✗ throw_pretty(
94 "Invalid argument: " << "x1 has wrong dimension (it should be " +
95 std::to_string(nx_) + ")");
96 }
97
2/4
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
 28 VectorXs tmp_x_ = VectorXs::Zero(nx_);
98
2/4
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
 28 VectorXs dx_ = VectorXs::Zero(ndx_);
99
2/4
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
 28 VectorXs dx0_ = VectorXs::Zero(ndx_);
100
101
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 28 dx_.setZero();
102
2/4
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
 28 diff(x0, x1, dx0_);
103
4/4
✓ Branch 0 taken 21 times.
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 14 times.
✓ Branch 3 taken 7 times.
 28 if (firstsecond == first || firstsecond == both) {
104
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 const Scalar x0h_jac = e_jac_ * std::max(1., x0.norm());
105
2/4
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 21 times.
 42 if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
106
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
 21 static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
107 ✗ throw_pretty(
108 "Invalid argument: " << "Jfirst has wrong dimension (it should be " +
109 std::to_string(ndx_) + "," +
110 std::to_string(ndx_) + ")");
111 }
112
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 Jfirst.setZero();
113
2/2
✓ Branch 0 taken 888 times.
✓ Branch 1 taken 21 times.
 909 for (std::size_t i = 0; i < ndx_; ++i) {
114
1/2
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
 888 dx_(i) = x0h_jac;
115
3/6
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 888 times.
✗ Branch 8 not taken.
 888 integrate(x0, dx_, tmp_x_);
116
4/8
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 888 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 888 times.
✗ Branch 11 not taken.
 888 diff(tmp_x_, x1, Jfirst.col(i));
117
2/4
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 888 times.
✗ Branch 5 not taken.
 888 Jfirst.col(i) -= dx0_;
118
1/2
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
 888 dx_(i) = 0.0;
119 }
120
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 Jfirst /= x0h_jac;
121 }
122
4/4
✓ Branch 0 taken 21 times.
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 14 times.
✓ Branch 3 taken 7 times.
 28 if (firstsecond == second || firstsecond == both) {
123
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 const Scalar x1h_jac = e_jac_ * std::max(1., x1.norm());
124
2/4
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 21 times.
 42 if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
125
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
 21 static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
126 ✗ throw_pretty(
127 "Invalid argument: " << "Jsecond has wrong dimension (it should be " +
128 std::to_string(ndx_) + "," +
129 std::to_string(ndx_) + ")");
130 }
131
132
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 Jsecond.setZero();
133
2/2
✓ Branch 0 taken 888 times.
✓ Branch 1 taken 21 times.
 909 for (std::size_t i = 0; i < ndx_; ++i) {
134
1/2
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
 888 dx_(i) = x1h_jac;
135
3/6
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 888 times.
✗ Branch 8 not taken.
 888 integrate(x1, dx_, tmp_x_);
136
4/8
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 888 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 888 times.
✗ Branch 11 not taken.
 888 diff(x0, tmp_x_, Jsecond.col(i));
137
2/4
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 888 times.
✗ Branch 5 not taken.
 888 Jsecond.col(i) -= dx0_;
138
1/2
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
 888 dx_(i) = 0.0;
139 }
140
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 Jsecond /= x1h_jac;
141 }
142 28 }
143
144 template <typename Scalar>
145 28 void StateNumDiffTpl<Scalar>::Jintegrate(const Eigen::Ref<const VectorXs>& x,
146 const Eigen::Ref<const VectorXs>& dx,
147 Eigen::Ref<MatrixXs> Jfirst,
148 Eigen::Ref<MatrixXs> Jsecond,
149 const Jcomponent firstsecond,
150 const AssignmentOp) const {
151
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 28 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
 28 assert_pretty(
152 is_a_Jcomponent(firstsecond),
153 ("firstsecond must be one of the Jcomponent {both, first, second}"));
154
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 28 times.
 28 if (static_cast<std::size_t>(x.size()) != nx_) {
155 ✗ throw_pretty(
156 "Invalid argument: " << "x has wrong dimension (it should be " +
157 std::to_string(nx_) + ")");
158 }
159
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 28 times.
 28 if (static_cast<std::size_t>(dx.size()) != ndx_) {
160 ✗ throw_pretty(
161 "Invalid argument: " << "dx has wrong dimension (it should be " +
162 std::to_string(ndx_) + ")");
163 }
164
2/4
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
 28 VectorXs tmp_x_ = VectorXs::Zero(nx_);
165
2/4
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
 28 VectorXs dx_ = VectorXs::Zero(ndx_);
166
2/4
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
 28 VectorXs x0_ = VectorXs::Zero(nx_);
167
168 // x0_ = integrate(x, dx)
169
2/4
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
 28 integrate(x, dx, x0_);
170
171
4/4
✓ Branch 0 taken 21 times.
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 14 times.
✓ Branch 3 taken 7 times.
 28 if (firstsecond == first || firstsecond == both) {
172
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 const Scalar xh_jac = e_jac_ * std::max(1., x.norm());
173
2/4
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 21 times.
 42 if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
174
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
 21 static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
175 ✗ throw_pretty(
176 "Invalid argument: " << "Jfirst has wrong dimension (it should be " +
177 std::to_string(ndx_) + "," +
178 std::to_string(ndx_) + ")");
179 }
180
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 Jfirst.setZero();
181
2/2
✓ Branch 0 taken 888 times.
✓ Branch 1 taken 21 times.
 909 for (std::size_t i = 0; i < ndx_; ++i) {
182
1/2
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
 888 dx_(i) = xh_jac;
183
3/6
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 888 times.
✗ Branch 8 not taken.
 888 integrate(x, dx_, tmp_x_);
184
3/6
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 888 times.
✗ Branch 8 not taken.
 888 integrate(tmp_x_, dx, tmp_x_);
185
5/10
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 888 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 888 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 888 times.
✗ Branch 14 not taken.
 888 diff(x0_, tmp_x_, Jfirst.col(i));
186
1/2
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
 888 dx_(i) = 0.;
187 }
188
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 Jfirst /= xh_jac;
189 }
190
4/4
✓ Branch 0 taken 21 times.
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 14 times.
✓ Branch 3 taken 7 times.
 28 if (firstsecond == second || firstsecond == both) {
191
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 const Scalar dxh_jac = e_jac_ * std::max(1., dx.norm());
192
2/4
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 21 times.
 42 if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
193
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
 21 static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
194 ✗ throw_pretty(
195 "Invalid argument: " << "Jsecond has wrong dimension (it should be " +
196 std::to_string(ndx_) + "," +
197 std::to_string(ndx_) + ")");
198 }
199
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 Jsecond.setZero();
200
2/2
✓ Branch 0 taken 888 times.
✓ Branch 1 taken 21 times.
 909 for (std::size_t i = 0; i < ndx_; ++i) {
201
1/2
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
 888 dx_(i) = dxh_jac;
202
4/8
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 888 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 888 times.
✗ Branch 11 not taken.
 888 integrate(x, dx + dx_, tmp_x_);
203
5/10
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 888 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 888 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 888 times.
✗ Branch 14 not taken.
 888 diff(x0_, tmp_x_, Jsecond.col(i));
204
1/2
✓ Branch 1 taken 888 times.
✗ Branch 2 not taken.
 888 dx_(i) = 0.;
205 }
206
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 Jsecond /= dxh_jac;
207 }
208 28 }
209
210 template <typename Scalar>
211 ✗ void StateNumDiffTpl<Scalar>::JintegrateTransport(
212 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&,
213 ✗ Eigen::Ref<MatrixXs>, const Jcomponent) const {}
214
215 template <typename Scalar>
216 ✗ const Scalar StateNumDiffTpl<Scalar>::get_disturbance() const {
217 ✗ return e_jac_;
218 }
219
220 template <typename Scalar>
221 ✗ void StateNumDiffTpl<Scalar>::set_disturbance(Scalar disturbance) {
222 ✗ if (disturbance < 0.) {
223 ✗ throw_pretty("Invalid argument: " << "Disturbance constant is positive");
224 }
225 ✗ e_jac_ = disturbance;
226 }
227
228 } // namespace crocoddyl
229