GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/states/euclidean.hxx
Date: 2025-01-30 11:01:55
Exec Total Coverage
Lines: 57 81 70.4%
Branches: 63 368 17.1%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 namespace crocoddyl {
10
11 template <typename Scalar>
12 484 StateVectorTpl<Scalar>::StateVectorTpl(const std::size_t nx)
13 484 : StateAbstractTpl<Scalar>(nx, nx) {}
14
15 template <typename Scalar>
16 972 StateVectorTpl<Scalar>::~StateVectorTpl() {}
17
18 template <typename Scalar>
19 2348 typename MathBaseTpl<Scalar>::VectorXs StateVectorTpl<Scalar>::zero() const {
20
1/2
✓ Branch 2 taken 2348 times.
✗ Branch 3 not taken.
 2348 return VectorXs::Zero(nx_);
21 }
22
23 template <typename Scalar>
24 2031 typename MathBaseTpl<Scalar>::VectorXs StateVectorTpl<Scalar>::rand() const {
25
1/2
✓ Branch 2 taken 2031 times.
✗ Branch 3 not taken.
 2031 return VectorXs::Random(nx_);
26 }
27
28 template <typename Scalar>
29 9432 void StateVectorTpl<Scalar>::diff(const Eigen::Ref<const VectorXs>& x0,
30 const Eigen::Ref<const VectorXs>& x1,
31 Eigen::Ref<VectorXs> dxout) const {
32
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 9432 times.
 9432 if (static_cast<std::size_t>(x0.size()) != nx_) {
33 ✗ throw_pretty(
34 "Invalid argument: " << "x0 has wrong dimension (it should be " +
35 std::to_string(nx_) + ")");
36 }
37
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 9432 times.
 9432 if (static_cast<std::size_t>(x1.size()) != nx_) {
38 ✗ throw_pretty(
39 "Invalid argument: " << "x1 has wrong dimension (it should be " +
40 std::to_string(nx_) + ")");
41 }
42
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 9432 times.
 9432 if (static_cast<std::size_t>(dxout.size()) != ndx_) {
43 ✗ throw_pretty(
44 "Invalid argument: " << "dxout has wrong dimension (it should be " +
45 std::to_string(ndx_) + ")");
46 }
47
1/2
✓ Branch 2 taken 9432 times.
✗ Branch 3 not taken.
 9432 dxout = x1 - x0;
48 9432 }
49
50 template <typename Scalar>
51 25238 void StateVectorTpl<Scalar>::integrate(const Eigen::Ref<const VectorXs>& x,
52 const Eigen::Ref<const VectorXs>& dx,
53 Eigen::Ref<VectorXs> xout) const {
54
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 25238 times.
 25238 if (static_cast<std::size_t>(x.size()) != nx_) {
55 ✗ throw_pretty(
56 "Invalid argument: " << "x has wrong dimension (it should be " +
57 std::to_string(nx_) + ")");
58 }
59
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 25238 times.
 25238 if (static_cast<std::size_t>(dx.size()) != ndx_) {
60 ✗ throw_pretty(
61 "Invalid argument: " << "dx has wrong dimension (it should be " +
62 std::to_string(ndx_) + ")");
63 }
64
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 25238 times.
 25238 if (static_cast<std::size_t>(xout.size()) != nx_) {
65 ✗ throw_pretty(
66 "Invalid argument: " << "xout has wrong dimension (it should be " +
67 std::to_string(nx_) + ")");
68 }
69
1/2
✓ Branch 2 taken 25238 times.
✗ Branch 3 not taken.
 25238 xout = x + dx;
70 25238 }
71
72 template <typename Scalar>
73 196 void StateVectorTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>&,
74 const Eigen::Ref<const VectorXs>&,
75 Eigen::Ref<MatrixXs> Jfirst,
76 Eigen::Ref<MatrixXs> Jsecond,
77 const Jcomponent firstsecond) const {
78
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 196 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.
 196 assert_pretty(
79 is_a_Jcomponent(firstsecond),
80 ("firstsecond must be one of the Jcomponent {both, first, second}"));
81
4/4
✓ Branch 0 taken 177 times.
✓ Branch 1 taken 19 times.
✓ Branch 2 taken 175 times.
✓ Branch 3 taken 2 times.
 196 if (firstsecond == first || firstsecond == both) {
82
2/4
✓ Branch 1 taken 194 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 194 times.
 388 if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
83
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 194 times.
 194 static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
84 ✗ throw_pretty(
85 "Invalid argument: " << "Jfirst has wrong dimension (it should be " +
86 std::to_string(ndx_) + "," +
87 std::to_string(ndx_) + ")");
88 }
89 194 Jfirst.setZero();
90
3/6
✓ Branch 1 taken 194 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 194 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 194 times.
✗ Branch 8 not taken.
 194 Jfirst.diagonal() = MathBase::VectorXs::Constant(ndx_, -1.);
91 }
92
4/4
✓ Branch 0 taken 194 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 175 times.
✓ Branch 3 taken 19 times.
 196 if (firstsecond == second || firstsecond == both) {
93
2/4
✓ Branch 1 taken 177 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 177 times.
 354 if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
94
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 177 times.
 177 static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
95 ✗ throw_pretty(
96 "Invalid argument: " << "Jsecond has wrong dimension (it should be " +
97 std::to_string(ndx_) + "," +
98 std::to_string(ndx_) + ")");
99 }
100 177 Jsecond.setZero();
101
3/6
✓ Branch 1 taken 177 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 177 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 177 times.
✗ Branch 8 not taken.
 177 Jsecond.diagonal() = MathBase::VectorXs::Constant(ndx_, 1.);
102 }
103 196 }
104
105 template <typename Scalar>
106 2519 void StateVectorTpl<Scalar>::Jintegrate(const Eigen::Ref<const VectorXs>&,
107 const Eigen::Ref<const VectorXs>&,
108 Eigen::Ref<MatrixXs> Jfirst,
109 Eigen::Ref<MatrixXs> Jsecond,
110 const Jcomponent firstsecond,
111 const AssignmentOp op) const {
112
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 2519 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.
 2519 assert_pretty(
113 is_a_Jcomponent(firstsecond),
114 ("firstsecond must be one of the Jcomponent {both, first, second}"));
115
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 2519 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.
 2519 assert_pretty(is_a_AssignmentOp(op),
116 ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
117
4/4
✓ Branch 0 taken 640 times.
✓ Branch 1 taken 1879 times.
✓ Branch 2 taken 18 times.
✓ Branch 3 taken 622 times.
 2519 if (firstsecond == first || firstsecond == both) {
118
2/4
✓ Branch 1 taken 1897 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1897 times.
 3794 if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
119
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1897 times.
 1897 static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
120 ✗ throw_pretty(
121 "Invalid argument: " << "Jfirst has wrong dimension (it should be " +
122 std::to_string(ndx_) + "," +
123 std::to_string(ndx_) + ")");
124 }
125
2/4
✓ Branch 0 taken 19 times.
✓ Branch 1 taken 1878 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 1897 switch (op) {
126 19 case setto:
127
3/6
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19 times.
✗ Branch 8 not taken.
 19 Jfirst.diagonal().array() = Scalar(1.);
128 19 break;
129 1878 case addto:
130
3/6
✓ Branch 1 taken 1878 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1878 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1878 times.
✗ Branch 8 not taken.
 1878 Jfirst.diagonal().array() += Scalar(1.);
131 1878 break;
132 ✗ case rmfrom:
133 ✗ Jfirst.diagonal().array() -= Scalar(1.);
134 ✗ break;
135 ✗ default:
136 ✗ throw_pretty(
137 "Invalid argument: allowed operators: setto, addto, rmfrom");
138 break;
139 }
140 }
141
4/4
✓ Branch 0 taken 1897 times.
✓ Branch 1 taken 622 times.
✓ Branch 2 taken 18 times.
✓ Branch 3 taken 1879 times.
 2519 if (firstsecond == second || firstsecond == both) {
142
2/4
✓ Branch 1 taken 640 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 640 times.
 1280 if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
143
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 640 times.
 640 static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
144 ✗ throw_pretty(
145 "Invalid argument: " << "Jsecond has wrong dimension (it should be " +
146 std::to_string(ndx_) + "," +
147 std::to_string(ndx_) + ")");
148 }
149
1/4
✓ Branch 0 taken 640 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 640 switch (op) {
150 640 case setto:
151
3/6
✓ Branch 1 taken 640 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 640 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 640 times.
✗ Branch 8 not taken.
 640 Jsecond.diagonal().array() = Scalar(1.);
152 640 break;
153 ✗ case addto:
154 ✗ Jsecond.diagonal().array() += Scalar(1.);
155 ✗ break;
156 ✗ case rmfrom:
157 ✗ Jsecond.diagonal().array() -= Scalar(1.);
158 ✗ break;
159 ✗ default:
160 ✗ throw_pretty(
161 "Invalid argument: allowed operators: setto, addto, rmfrom");
162 break;
163 }
164 }
165 2519 }
166
167 template <typename Scalar>
168 3758 void StateVectorTpl<Scalar>::JintegrateTransport(
169 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&,
170 Eigen::Ref<MatrixXs>, const Jcomponent firstsecond) const {
171
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 3758 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.
 3758 assert_pretty(is_a_Jcomponent(firstsecond), (""));
172
3/4
✓ Branch 0 taken 3757 times.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3757 times.
 3758 if (firstsecond != first && firstsecond != second) {
173 ✗ throw_pretty(
174 "Invalid argument: firstsecond must be either first or second. both "
175 "not supported for this operation.");
176 }
177 3758 }
178
179 } // namespace crocoddyl
180