GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/states/multibody.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 111 151 73.5%
Branches: 158 665 23.8%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include <pinocchio/algorithm/joint-configuration.hpp>
10
11 #include "crocoddyl/multibody/states/multibody.hpp"
12
13 namespace crocoddyl {
14
15 template <typename Scalar>
16 11087 StateMultibodyTpl<Scalar>::StateMultibodyTpl(
17 std::shared_ptr<PinocchioModel> model)
18 11087 : Base(model->nq + model->nv, 2 * model->nv),
19 11087 pinocchio_(model),
20
2/4
✓ Branch 7 taken 11087 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 11087 times.
✗ Branch 11 not taken.
 22174 x0_(VectorXs::Zero(model->nq + model->nv)) {
21
3/6
✓ Branch 2 taken 11087 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11087 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 11087 times.
✗ Branch 9 not taken.
 11087 x0_.head(nq_) = pinocchio::neutral(*pinocchio_.get());
22 // In a multibody system, we could define the first joint using Lie groups.
23 // The current cases are free-flyer (SE3) and spherical (S03).
24 // Instead simple represents any joint that can model within the Euclidean
25 // manifold. The rest of joints use Euclidean algebra. We use this fact for
26 // computing Jdiff.
27 // Define internally the limits of the first joint
28 11087 const std::size_t nq0 =
29
2/4
✓ Branch 3 taken 11087 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11087 times.
✗ Branch 7 not taken.
 22174 model->existJointName("root_joint")
30
9/16
✓ Branch 0 taken 6800 times.
✓ Branch 1 taken 4287 times.
✓ Branch 6 taken 6800 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6800 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 6800 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 6800 times.
✓ Branch 16 taken 4287 times.
✓ Branch 18 taken 6800 times.
✓ Branch 19 taken 4287 times.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
 11087 ? model->joints[model->getJointId("root_joint")].nq()
31 : 0;
32
4/8
✓ Branch 1 taken 11087 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 11087 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 11087 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 11087 times.
✗ Branch 12 not taken.
 11087 lb_.head(nq0) = -std::numeric_limits<Scalar>::max() * VectorXs::Ones(nq0);
33
4/8
✓ Branch 1 taken 11087 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 11087 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 11087 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 11087 times.
✗ Branch 12 not taken.
 11087 ub_.head(nq0) = std::numeric_limits<Scalar>::max() * VectorXs::Ones(nq0);
34
3/6
✓ Branch 2 taken 11087 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11087 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 11087 times.
✗ Branch 9 not taken.
 11087 lb_.segment(nq0, nq_ - nq0) = pinocchio_->lowerPositionLimit.tail(nq_ - nq0);
35
3/6
✓ Branch 2 taken 11087 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11087 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 11087 times.
✗ Branch 9 not taken.
 11087 ub_.segment(nq0, nq_ - nq0) = pinocchio_->upperPositionLimit.tail(nq_ - nq0);
36
3/6
✓ Branch 2 taken 11087 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11087 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 11087 times.
✗ Branch 9 not taken.
 11087 lb_.tail(nv_) = -pinocchio_->velocityLimit;
37
2/4
✓ Branch 2 taken 11087 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11087 times.
✗ Branch 6 not taken.
 11087 ub_.tail(nv_) = pinocchio_->velocityLimit;
38
1/2
✓ Branch 1 taken 11087 times.
✗ Branch 2 not taken.
 11087 Base::update_has_limits();
39 11087 }
40
41 template <typename Scalar>
42 ✗ StateMultibodyTpl<Scalar>::StateMultibodyTpl()
43 ✗ : Base(), x0_(VectorXs::Zero(0)) {}
44
45 template <typename Scalar>
46 22380 StateMultibodyTpl<Scalar>::~StateMultibodyTpl() {}
47
48 template <typename Scalar>
49 5055 typename MathBaseTpl<Scalar>::VectorXs StateMultibodyTpl<Scalar>::zero() const {
50 5055 return x0_;
51 }
52
53 template <typename Scalar>
54 10375 typename MathBaseTpl<Scalar>::VectorXs StateMultibodyTpl<Scalar>::rand() const {
55
1/2
✓ Branch 2 taken 10375 times.
✗ Branch 3 not taken.
 10375 VectorXs xrand = VectorXs::Random(nx_);
56
3/6
✓ Branch 2 taken 10375 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10375 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10375 times.
✗ Branch 9 not taken.
 10375 xrand.head(nq_) = pinocchio::randomConfiguration(*pinocchio_.get());
57 10375 return xrand;
58 }
59
60 template <typename Scalar>
61 107203 void StateMultibodyTpl<Scalar>::diff(const Eigen::Ref<const VectorXs>& x0,
62 const Eigen::Ref<const VectorXs>& x1,
63 Eigen::Ref<VectorXs> dxout) const {
64
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 107203 times.
 107203 if (static_cast<std::size_t>(x0.size()) != nx_) {
65 ✗ throw_pretty(
66 "Invalid argument: " << "x0 has wrong dimension (it should be " +
67 std::to_string(nx_) + ")");
68 }
69
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 107203 times.
 107203 if (static_cast<std::size_t>(x1.size()) != nx_) {
70 ✗ throw_pretty(
71 "Invalid argument: " << "x1 has wrong dimension (it should be " +
72 std::to_string(nx_) + ")");
73 }
74
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 107203 times.
 107203 if (static_cast<std::size_t>(dxout.size()) != ndx_) {
75 ✗ throw_pretty(
76 "Invalid argument: " << "dxout has wrong dimension (it should be " +
77 std::to_string(ndx_) + ")");
78 }
79
80
3/6
✓ Branch 1 taken 107203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107203 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 107203 times.
✗ Branch 9 not taken.
 107203 pinocchio::difference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
81 107203 dxout.head(nv_));
82
4/8
✓ Branch 2 taken 107203 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 107203 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 107203 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 107203 times.
✗ Branch 12 not taken.
 107203 dxout.tail(nv_) = x1.tail(nv_) - x0.tail(nv_);
83 107203 }
84
85 template <typename Scalar>
86 146470 void StateMultibodyTpl<Scalar>::integrate(const Eigen::Ref<const VectorXs>& x,
87 const Eigen::Ref<const VectorXs>& dx,
88 Eigen::Ref<VectorXs> xout) const {
89
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 146470 times.
 146470 if (static_cast<std::size_t>(x.size()) != nx_) {
90 ✗ throw_pretty(
91 "Invalid argument: " << "x has wrong dimension (it should be " +
92 std::to_string(nx_) + ")");
93 }
94
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 146470 times.
 146470 if (static_cast<std::size_t>(dx.size()) != ndx_) {
95 ✗ throw_pretty(
96 "Invalid argument: " << "dx has wrong dimension (it should be " +
97 std::to_string(ndx_) + ")");
98 }
99
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 146470 times.
 146470 if (static_cast<std::size_t>(xout.size()) != nx_) {
100 ✗ throw_pretty(
101 "Invalid argument: " << "xout has wrong dimension (it should be " +
102 std::to_string(nx_) + ")");
103 }
104
105
3/6
✓ Branch 1 taken 146470 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 146470 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 146470 times.
✗ Branch 9 not taken.
 146470 pinocchio::integrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
106 146470 xout.head(nq_));
107
4/8
✓ Branch 2 taken 146470 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 146470 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 146470 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 146470 times.
✗ Branch 12 not taken.
 146470 xout.tail(nv_) = x.tail(nv_) + dx.tail(nv_);
108 146470 }
109
110 template <typename Scalar>
111 14297 void StateMultibodyTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>& x0,
112 const Eigen::Ref<const VectorXs>& x1,
113 Eigen::Ref<MatrixXs> Jfirst,
114 Eigen::Ref<MatrixXs> Jsecond,
115 const Jcomponent firstsecond) const {
116
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 14297 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.
 14297 assert_pretty(
117 is_a_Jcomponent(firstsecond),
118 ("firstsecond must be one of the Jcomponent {both, first, second}"));
119
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 14297 times.
 14297 if (static_cast<std::size_t>(x0.size()) != nx_) {
120 ✗ throw_pretty(
121 "Invalid argument: " << "x0 has wrong dimension (it should be " +
122 std::to_string(nx_) + ")");
123 }
124
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 14297 times.
 14297 if (static_cast<std::size_t>(x1.size()) != nx_) {
125 ✗ throw_pretty(
126 "Invalid argument: " << "x1 has wrong dimension (it should be " +
127 std::to_string(nx_) + ")");
128 }
129
130
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 14285 times.
 14297 if (firstsecond == first) {
131
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 12 times.
 24 if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
132
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
 12 static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
133 ✗ throw_pretty(
134 "Invalid argument: " << "Jfirst has wrong dimension (it should be " +
135 std::to_string(ndx_) + "," +
136 std::to_string(ndx_) + ")");
137 }
138
139
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
 12 pinocchio::dDifference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
140 12 Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0);
141
4/8
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
 12 Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)-1;
142
2/2
✓ Branch 0 taken 14258 times.
✓ Branch 1 taken 27 times.
 14285 } else if (firstsecond == second) {
143
2/4
✓ Branch 1 taken 14258 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 14258 times.
 28516 if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
144
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 14258 times.
 14258 static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
145 ✗ throw_pretty(
146 "Invalid argument: " << "Jsecond has wrong dimension (it should be " +
147 std::to_string(ndx_) + "," +
148 std::to_string(ndx_) + ")");
149 }
150
3/6
✓ Branch 1 taken 14258 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14258 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 14258 times.
✗ Branch 9 not taken.
 14258 pinocchio::dDifference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
151 14258 Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1);
152
4/8
✓ Branch 1 taken 14258 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14258 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14258 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14258 times.
✗ Branch 11 not taken.
 14258 Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
153 } else { // computing both
154
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 27 times.
 54 if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
155
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 27 times.
 27 static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
156 ✗ throw_pretty(
157 "Invalid argument: " << "Jfirst has wrong dimension (it should be " +
158 std::to_string(ndx_) + "," +
159 std::to_string(ndx_) + ")");
160 }
161
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 27 times.
 54 if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
162
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 27 times.
 27 static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
163 ✗ throw_pretty(
164 "Invalid argument: " << "Jsecond has wrong dimension (it should be " +
165 std::to_string(ndx_) + "," +
166 std::to_string(ndx_) + ")");
167 }
168
3/6
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
 27 pinocchio::dDifference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
169 27 Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0);
170
3/6
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
 27 pinocchio::dDifference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
171 27 Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1);
172
4/8
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 27 times.
✗ Branch 11 not taken.
 27 Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)-1;
173
4/8
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 27 times.
✗ Branch 11 not taken.
 27 Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
174 }
175 14297 }
176
177 template <typename Scalar>
178 13429 void StateMultibodyTpl<Scalar>::Jintegrate(const Eigen::Ref<const VectorXs>& x,
179 const Eigen::Ref<const VectorXs>& dx,
180 Eigen::Ref<MatrixXs> Jfirst,
181 Eigen::Ref<MatrixXs> Jsecond,
182 const Jcomponent firstsecond,
183 const AssignmentOp op) const {
184
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 13429 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.
 13429 assert_pretty(
185 is_a_Jcomponent(firstsecond),
186 ("firstsecond must be one of the Jcomponent {both, first, second}"));
187
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 13429 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.
 13429 assert_pretty(is_a_AssignmentOp(op),
188 ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
189
4/4
✓ Branch 0 taken 200 times.
✓ Branch 1 taken 13229 times.
✓ Branch 2 taken 194 times.
✓ Branch 3 taken 6 times.
 13429 if (firstsecond == first || firstsecond == both) {
190
2/4
✓ Branch 1 taken 13423 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 13423 times.
 26846 if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
191
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 13423 times.
 13423 static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
192 ✗ throw_pretty(
193 "Invalid argument: " << "Jfirst has wrong dimension (it should be " +
194 std::to_string(ndx_) + "," +
195 std::to_string(ndx_) + ")");
196 }
197
2/4
✓ Branch 0 taken 200 times.
✓ Branch 1 taken 13223 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 13423 switch (op) {
198 200 case setto:
199
3/6
✓ Branch 1 taken 200 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 200 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 200 times.
✗ Branch 9 not taken.
 200 pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
200 200 Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
201 pinocchio::SETTO);
202
4/8
✓ Branch 1 taken 200 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 200 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 200 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 200 times.
✗ Branch 11 not taken.
 200 Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
203 200 break;
204 13223 case addto:
205
3/6
✓ Branch 1 taken 13223 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13223 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 13223 times.
✗ Branch 9 not taken.
 13223 pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
206 13223 Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
207 pinocchio::ADDTO);
208
4/8
✓ Branch 1 taken 13223 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13223 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13223 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13223 times.
✗ Branch 11 not taken.
 13223 Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() += (Scalar)1;
209 13223 break;
210 ✗ case rmfrom:
211 ✗ pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
212 ✗ Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
213 pinocchio::RMTO);
214 ✗ Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() -= (Scalar)1;
215 ✗ break;
216 ✗ default:
217 ✗ throw_pretty(
218 "Invalid argument: allowed operators: setto, addto, rmfrom");
219 break;
220 }
221 }
222
4/4
✓ Branch 0 taken 13423 times.
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 194 times.
✓ Branch 3 taken 13229 times.
 13429 if (firstsecond == second || firstsecond == both) {
223
2/4
✓ Branch 1 taken 200 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 200 times.
 400 if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
224
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 200 times.
 200 static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
225 ✗ throw_pretty(
226 "Invalid argument: " << "Jsecond has wrong dimension (it should be " +
227 std::to_string(ndx_) + "," +
228 std::to_string(ndx_) + ")");
229 }
230
1/4
✓ Branch 0 taken 200 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 200 switch (op) {
231 200 case setto:
232
3/6
✓ Branch 1 taken 200 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 200 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 200 times.
✗ Branch 9 not taken.
 200 pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
233 200 Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
234 pinocchio::SETTO);
235
4/8
✓ Branch 1 taken 200 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 200 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 200 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 200 times.
✗ Branch 11 not taken.
 200 Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
236 200 break;
237 ✗ case addto:
238 ✗ pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
239 ✗ Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
240 pinocchio::ADDTO);
241 ✗ Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() += (Scalar)1;
242 ✗ break;
243 ✗ case rmfrom:
244 ✗ pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
245 ✗ Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
246 pinocchio::RMTO);
247 ✗ Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() -= (Scalar)1;
248 ✗ break;
249 ✗ default:
250 ✗ throw_pretty(
251 "Invalid argument: allowed operators: setto, addto, rmfrom");
252 break;
253 }
254 }
255 13429 }
256
257 template <typename Scalar>
258 26458 void StateMultibodyTpl<Scalar>::JintegrateTransport(
259 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
260 Eigen::Ref<MatrixXs> Jin, const Jcomponent firstsecond) const {
261
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 26458 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.
 26458 assert_pretty(
262 is_a_Jcomponent(firstsecond),
263 ("firstsecond must be one of the Jcomponent {both, first, second}"));
264
265
2/3
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 26452 times.
✗ Branch 2 not taken.
 26458 switch (firstsecond) {
266 6 case first:
267
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 6 pinocchio::dIntegrateTransport(*pinocchio_.get(), x.head(nq_),
268
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 6 dx.head(nv_), Jin.topRows(nv_),
269 pinocchio::ARG0);
270 6 break;
271 26452 case second:
272
2/4
✓ Branch 1 taken 26452 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26452 times.
✗ Branch 6 not taken.
 26452 pinocchio::dIntegrateTransport(*pinocchio_.get(), x.head(nq_),
273
1/2
✓ Branch 2 taken 26452 times.
✗ Branch 3 not taken.
 26452 dx.head(nv_), Jin.topRows(nv_),
274 pinocchio::ARG1);
275 26452 break;
276 ✗ default:
277 ✗ throw_pretty(
278 "Invalid argument: firstsecond must be either first or second. both "
279 "not supported for this operation.");
280 break;
281 }
282 26458 }
283
284 template <typename Scalar>
285 const std::shared_ptr<pinocchio::ModelTpl<Scalar> >&
286 1513507 StateMultibodyTpl<Scalar>::get_pinocchio() const {
287 1513507 return pinocchio_;
288 }
289
290 template <typename Scalar>
291 template <typename NewScalar>
292 101 StateMultibodyTpl<NewScalar> StateMultibodyTpl<Scalar>::cast() const {
293 typedef StateMultibodyTpl<NewScalar> ReturnType;
294 typedef pinocchio::ModelTpl<NewScalar> ModelType;
295
2/4
✓ Branch 2 taken 101 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 101 times.
✗ Branch 6 not taken.
 202 ReturnType res(
296 101 std::make_shared<ModelType>(pinocchio_->template cast<NewScalar>()));
297 101 return res;
298 }
299
300 template <typename Scalar>
301 ✗ void StateMultibodyTpl<Scalar>::print(std::ostream& os) const {
302 ✗ os << "StateMultibody {nx=" << nx_ << ", ndx=" << ndx_
303 ✗ << ", pinocchio=" << *pinocchio_.get() << "}";
304 }
305
306 } // namespace crocoddyl
307