GCC Code Coverage Report

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