GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/jacobian.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 159 161 98.8%
Branches: 169 549 30.8%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2024 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_jacobian_hxx__
6 #define __pinocchio_algorithm_jacobian_hxx__
7
8 #include "pinocchio/multibody/visitor.hpp"
9 #include "pinocchio/algorithm/check.hpp"
10
11 /// @cond DEV
12
13 namespace pinocchio
14 {
15 namespace impl
16 {
17 template<
18 typename Scalar,
19 int Options,
20 template<typename, int> class JointCollectionTpl,
21 typename ConfigVectorType,
22 typename Matrix6xLike>
23 struct JointJacobiansForwardStep
24 : public fusion::JointUnaryVisitorBase<JointJacobiansForwardStep<
25 Scalar,
26 Options,
27 JointCollectionTpl,
28 ConfigVectorType,
29 Matrix6xLike>>
30 {
31 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
32 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
33
34 typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &, Matrix6xLike &>
35 ArgsType;
36
37 template<typename JointModel>
38 14710 static void algo(
39 const JointModelBase<JointModel> & jmodel,
40 JointDataBase<typename JointModel::JointDataDerived> & jdata,
41 const Model & model,
42 Data & data,
43 const Eigen::MatrixBase<ConfigVectorType> & q,
44 const Eigen::MatrixBase<Matrix6xLike> & J)
45 {
46 typedef typename Model::JointIndex JointIndex;
47
48
1/2
✓ Branch 1 taken 7355 times.
✗ Branch 2 not taken.
 14710 const JointIndex & i = jmodel.id();
49 14710 const JointIndex & parent = model.parents[i];
50
51
1/2
✓ Branch 3 taken 7355 times.
✗ Branch 4 not taken.
 14710 jmodel.calc(jdata.derived(), q.derived());
52
53
6/10
✓ Branch 1 taken 7355 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7069 times.
✓ Branch 5 taken 286 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7069 times.
✓ Branch 9 taken 286 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7069 times.
✗ Branch 13 not taken.
 14710 data.liMi[i] = model.jointPlacements[i] * jdata.M();
54
2/2
✓ Branch 0 taken 7059 times.
✓ Branch 1 taken 296 times.
 14710 if (parent > 0)
55
2/4
✓ Branch 3 taken 7059 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 7059 times.
✗ Branch 8 not taken.
 14118 data.oMi[i] = data.oMi[parent] * data.liMi[i];
56 else
57
1/2
✓ Branch 3 taken 296 times.
✗ Branch 4 not taken.
 592 data.oMi[i] = data.liMi[i];
58
59 14710 Matrix6xLike & J_ = J.const_cast_derived();
60
4/8
✓ Branch 2 taken 7355 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7355 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7355 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7355 times.
✗ Branch 12 not taken.
 14710 jmodel.jointCols(J_) = data.oMi[i].act(jdata.S());
61 }
62 };
63
64 template<
65 typename Scalar,
66 int Options,
67 template<typename, int> class JointCollectionTpl,
68 typename ConfigVectorType>
69 295 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeJointJacobians(
70 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
71 DataTpl<Scalar, Options, JointCollectionTpl> & data,
72 const Eigen::MatrixBase<ConfigVectorType> & q)
73 {
74
1/2
✓ Branch 1 taken 293 times.
✗ Branch 2 not taken.
 295 assert(model.check(data) && "data is not consistent with model.");
75
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 293 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 295 PINOCCHIO_CHECK_ARGUMENT_SIZE(
76 q.size(), model.nq, "The configuration vector is not of right size");
77
78 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
79 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
80 typedef typename Model::JointIndex JointIndex;
81 typedef typename Data::Matrix6x Matrix6x;
82
83 typedef JointJacobiansForwardStep<
84 Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6x>
85 Pass;
86 typedef typename Pass::ArgsType ArgsType;
87
2/2
✓ Branch 0 taken 7340 times.
✓ Branch 1 taken 293 times.
 7637 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
88 {
89
1/2
✓ Branch 1 taken 7340 times.
✗ Branch 2 not taken.
 7342 Pass::run(
90 7342 model.joints[i], data.joints[i],
91 14684 ArgsType(model, data, q.derived(), data.J.const_cast_derived()));
92 }
93
94 295 return data.J;
95 }
96
97 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
98 struct JointJacobiansForwardStep2
99 : public fusion::JointUnaryVisitorBase<
100 JointJacobiansForwardStep2<Scalar, Options, JointCollectionTpl>>
101 {
102 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
103 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
104
105 typedef boost::fusion::vector<Data &> ArgsType;
106
107 template<typename JointModel>
108 54 static void algo(
109 const JointModelBase<JointModel> & jmodel,
110 JointDataBase<typename JointModel::JointDataDerived> & jdata,
111 Data & data)
112 {
113 typedef typename Model::JointIndex JointIndex;
114
115
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 const JointIndex & i = jmodel.id();
116
4/8
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
 54 jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
117 }
118 };
119 } // namespace impl
120 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
121 1 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeJointJacobians(
122 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
123 DataTpl<Scalar, Options, JointCollectionTpl> & data)
124 {
125
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assert(model.check(data) && "data is not consistent with model.");
126
127 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
128 typedef typename Model::JointIndex JointIndex;
129
130 typedef impl::JointJacobiansForwardStep2<Scalar, Options, JointCollectionTpl> Pass;
131
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 28 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
132 {
133
1/2
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
 27 Pass::run(model.joints[i], data.joints[i], typename Pass::ArgsType(data));
134 }
135
136 1 return data.J;
137 }
138
139 namespace details
140 {
141 template<typename Scalar, int Options, typename Matrix6xLikeIn, typename Matrix6xLikeOut>
142 362 void translateJointJacobian(
143 const SE3Tpl<Scalar, Options> & placement,
144 const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
145 const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
146 {
147
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 181 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 362 PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.rows(), 6);
148
1/24
✗ Branch 2 not taken.
✓ Branch 3 taken 181 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 362 PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), Jout.cols());
149
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 181 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 362 PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6);
150
151 362 Matrix6xLikeOut & Jout_ = Jout.const_cast_derived();
152
153 typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn;
154 typedef const MotionRef<ConstColXprIn> MotionIn;
155
156 typedef typename Matrix6xLikeOut::ColXpr ColXprOut;
157 typedef MotionRef<ColXprOut> MotionOut;
158
159
2/2
✓ Branch 1 taken 336 times.
✓ Branch 2 taken 181 times.
 1034 for (Eigen::DenseIndex j = 0; j < Jin.cols(); ++j)
160 {
161
2/4
✓ Branch 1 taken 336 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 336 times.
✗ Branch 5 not taken.
 672 MotionIn v_in(Jin.col(j));
162
2/4
✓ Branch 1 taken 336 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 336 times.
✗ Branch 5 not taken.
 672 MotionOut v_out(Jout_.col(j));
163
164
1/2
✓ Branch 1 taken 336 times.
✗ Branch 2 not taken.
 672 v_out = v_in;
165
5/10
✓ Branch 1 taken 336 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 336 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 336 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 336 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 336 times.
✗ Branch 14 not taken.
 672 v_out.linear() -= placement.translation().cross(v_in.angular());
166 }
167 }
168
169 template<
170 typename Scalar,
171 int Options,
172 template<typename, int> class JointCollectionTpl,
173 typename Matrix6xLikeIn,
174 typename Matrix6xLikeOut>
175 462 void translateJointJacobian(
176 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
177 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
178 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex joint_id,
179 const ReferenceFrame rf,
180 const SE3Tpl<Scalar, Options> & placement,
181 const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
182 const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
183 {
184
1/2
✓ Branch 1 taken 462 times.
✗ Branch 2 not taken.
 462 assert(model.check(data) && "data is not consistent with model.");
185
186
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 462 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 462 PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.rows(), 6);
187
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 462 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 462 PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), model.nv);
188
189
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 462 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 462 PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6);
190
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 462 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 462 PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv);
191
192 462 Matrix6xLikeOut & Jout_ = Jout.const_cast_derived();
193
194 typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn;
195 typedef const MotionRef<ConstColXprIn> MotionIn;
196
197 typedef typename Matrix6xLikeOut::ColXpr ColXprOut;
198 typedef MotionRef<ColXprOut> MotionOut;
199
200 462 const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1;
201
3/4
✓ Branch 0 taken 78 times.
✓ Branch 1 taken 63 times.
✓ Branch 2 taken 321 times.
✗ Branch 3 not taken.
 462 switch (rf)
202 {
203 78 case WORLD: {
204
2/2
✓ Branch 1 taken 848 times.
✓ Branch 2 taken 78 times.
 926 for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
205 {
206
2/4
✓ Branch 1 taken 848 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 848 times.
✗ Branch 5 not taken.
 848 MotionIn v_in(Jin.col(j));
207
2/4
✓ Branch 1 taken 848 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 848 times.
✗ Branch 5 not taken.
 848 MotionOut v_out(Jout_.col(j));
208
209
1/2
✓ Branch 1 taken 848 times.
✗ Branch 2 not taken.
 848 v_out = v_in;
210 }
211 78 break;
212 }
213 63 case LOCAL_WORLD_ALIGNED: {
214
2/2
✓ Branch 0 taken 680 times.
✓ Branch 1 taken 63 times.
 743 for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
215 {
216
2/4
✓ Branch 1 taken 680 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 680 times.
✗ Branch 5 not taken.
 680 MotionIn v_in(Jin.col(j));
217
2/4
✓ Branch 1 taken 680 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 680 times.
✗ Branch 5 not taken.
 680 MotionOut v_out(Jout_.col(j));
218
219
1/2
✓ Branch 1 taken 680 times.
✗ Branch 2 not taken.
 680 v_out = v_in;
220
5/10
✓ Branch 1 taken 680 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 680 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 680 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 680 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 680 times.
✗ Branch 14 not taken.
 680 v_out.linear() -= placement.translation().cross(v_in.angular());
221 }
222 63 break;
223 }
224 321 case LOCAL: {
225
2/2
✓ Branch 0 taken 3493 times.
✓ Branch 1 taken 321 times.
 3814 for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
226 {
227
2/4
✓ Branch 1 taken 3493 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3493 times.
✗ Branch 5 not taken.
 3493 MotionIn v_in(Jin.col(j));
228
2/4
✓ Branch 1 taken 3493 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3493 times.
✗ Branch 5 not taken.
 3493 MotionOut v_out(Jout_.col(j));
229
230
2/4
✓ Branch 1 taken 3493 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3493 times.
✗ Branch 5 not taken.
 3493 v_out = placement.actInv(v_in);
231 }
232 321 break;
233 }
234 ✗ default:
235 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "must never happened");
236 break;
237 }
238 462 }
239
240 template<
241 typename Scalar,
242 int Options,
243 template<typename, int> class JointCollectionTpl,
244 typename Matrix6xLikeIn,
245 typename Matrix6xLikeOut>
246 415 void translateJointJacobian(
247 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
248 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
249 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex joint_id,
250 const ReferenceFrame rf,
251 const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
252 const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
253 {
254 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
255 415 const typename Data::SE3 & oMjoint = data.oMi[joint_id];
256
257 415 translateJointJacobian(model, data, joint_id, rf, oMjoint, Jin, Jout);
258 415 }
259 } // namespace details
260 namespace impl
261 {
262 /* Return the jacobian of the output frame attached to joint <jointId> in the
263 world frame or in the local frame depending on the template argument. The
264 function computeJacobians should have been called first. */
265 template<
266 typename Scalar,
267 int Options,
268 template<typename, int> class JointCollectionTpl,
269 typename Matrix6xLike>
270 408 void getJointJacobian(
271 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
272 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
273 const JointIndex joint_id,
274 const ReferenceFrame reference_frame,
275 const Eigen::MatrixBase<Matrix6xLike> & J)
276 {
277
1/2
✓ Branch 1 taken 408 times.
✗ Branch 2 not taken.
 408 assert(model.check(data) && "data is not consistent with model.");
278
279 408 ::pinocchio::details::translateJointJacobian(
280 408 model, data, joint_id, reference_frame, data.J, J.const_cast_derived());
281 408 }
282
283 template<
284 typename Scalar,
285 int Options,
286 template<typename, int> class JointCollectionTpl,
287 typename ConfigVectorType,
288 typename Matrix6xLike>
289 struct JointJacobianForwardStep
290 : public fusion::JointUnaryVisitorBase<JointJacobianForwardStep<
291 Scalar,
292 Options,
293 JointCollectionTpl,
294 ConfigVectorType,
295 Matrix6xLike>>
296 {
297 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
298 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
299
300 typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &, Matrix6xLike &>
301 ArgsType;
302
303 template<typename JointModel>
304 4656 static void algo(
305 const JointModelBase<JointModel> & jmodel,
306 JointDataBase<typename JointModel::JointDataDerived> & jdata,
307 const Model & model,
308 Data & data,
309 const Eigen::MatrixBase<ConfigVectorType> & q,
310 const Eigen::MatrixBase<Matrix6xLike> & J)
311 {
312 typedef typename Model::JointIndex JointIndex;
313
1/2
✓ Branch 1 taken 2328 times.
✗ Branch 2 not taken.
 4656 const JointIndex & i = jmodel.id();
314 4656 const JointIndex & parent = model.parents[i];
315
316
1/2
✓ Branch 3 taken 2328 times.
✗ Branch 4 not taken.
 4656 jmodel.calc(jdata.derived(), q.derived());
317
318
6/10
✓ Branch 1 taken 2328 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2306 times.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2306 times.
✓ Branch 9 taken 22 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2306 times.
✗ Branch 13 not taken.
 4656 data.liMi[i] = model.jointPlacements[i] * jdata.M();
319
2/4
✓ Branch 3 taken 2328 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 2328 times.
✗ Branch 8 not taken.
 4656 data.iMf[parent] = data.liMi[i] * data.iMf[i];
320
321 4656 Matrix6xLike & J_ = J.const_cast_derived();
322
4/8
✓ Branch 2 taken 2328 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2328 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2328 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2328 times.
✗ Branch 12 not taken.
 4656 jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S());
323 }
324 };
325
326 template<
327 typename Scalar,
328 int Options,
329 template<typename, int> class JointCollectionTpl,
330 typename ConfigVectorType,
331 typename Matrix6xLike>
332 379 void computeJointJacobian(
333 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
334 DataTpl<Scalar, Options, JointCollectionTpl> & data,
335 const Eigen::MatrixBase<ConfigVectorType> & q,
336 const JointIndex jointId,
337 const Eigen::MatrixBase<Matrix6xLike> & J)
338 {
339
1/2
✓ Branch 1 taken 379 times.
✗ Branch 2 not taken.
 379 assert(model.check(data) && "data is not consistent with model.");
340
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 379 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 379 PINOCCHIO_CHECK_ARGUMENT_SIZE(
341 q.size(), model.nq, "The configuration vector is not of right size");
342
343 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
344 typedef typename Model::JointIndex JointIndex;
345
346 379 data.iMf[jointId].setIdentity();
347 typedef JointJacobianForwardStep<
348 Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike>
349 Pass;
350
2/2
✓ Branch 0 taken 2292 times.
✓ Branch 1 taken 379 times.
 2671 for (JointIndex i = jointId; i > 0; i = model.parents[i])
351 {
352
1/2
✓ Branch 1 taken 2292 times.
✗ Branch 2 not taken.
 2292 Pass::run(
353 2292 model.joints[i], data.joints[i],
354 4584 typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived()));
355 }
356 379 }
357
358 template<
359 typename Scalar,
360 int Options,
361 template<typename, int> class JointCollectionTpl,
362 typename ConfigVectorType,
363 typename TangentVectorType>
364 struct JointJacobiansTimeVariationForwardStep
365 : public fusion::JointUnaryVisitorBase<JointJacobiansTimeVariationForwardStep<
366 Scalar,
367 Options,
368 JointCollectionTpl,
369 ConfigVectorType,
370 TangentVectorType>>
371 {
372 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
373 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
374
375 typedef boost::fusion::
376 vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &>
377 ArgsType;
378
379 template<typename JointModel>
380 864 static void algo(
381 const JointModelBase<JointModel> & jmodel,
382 JointDataBase<typename JointModel::JointDataDerived> & jdata,
383 const Model & model,
384 Data & data,
385 const Eigen::MatrixBase<ConfigVectorType> & q,
386 const Eigen::MatrixBase<TangentVectorType> & v)
387 {
388 typedef typename Model::JointIndex JointIndex;
389 typedef typename Data::SE3 SE3;
390 typedef typename Data::Motion Motion;
391
392
1/2
✓ Branch 1 taken 432 times.
✗ Branch 2 not taken.
 864 const JointIndex & i = (JointIndex)jmodel.id();
393 864 const JointIndex & parent = model.parents[i];
394
395 864 SE3 & oMi = data.oMi[i];
396 864 Motion & vJ = data.v[i];
397
398
1/2
✓ Branch 4 taken 432 times.
✗ Branch 5 not taken.
 864 jmodel.calc(jdata.derived(), q.derived(), v.derived());
399
400
2/4
✓ Branch 1 taken 432 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 432 times.
✗ Branch 5 not taken.
 864 vJ = jdata.v();
401
402
6/10
✓ Branch 1 taken 432 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 416 times.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 416 times.
✓ Branch 9 taken 16 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 416 times.
✗ Branch 13 not taken.
 864 data.liMi[i] = model.jointPlacements[i] * jdata.M();
403
2/2
✓ Branch 0 taken 416 times.
✓ Branch 1 taken 16 times.
 864 if (parent > 0)
404 {
405
2/4
✓ Branch 3 taken 416 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 416 times.
✗ Branch 7 not taken.
 832 oMi = data.oMi[parent] * data.liMi[i];
406
2/4
✓ Branch 3 taken 416 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 416 times.
✗ Branch 7 not taken.
 832 vJ += data.liMi[i].actInv(data.v[parent]);
407 }
408 else
409 {
410
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 32 oMi = data.liMi[i];
411 }
412
413
4/8
✓ Branch 1 taken 432 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 432 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 432 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 432 times.
✗ Branch 11 not taken.
 864 jmodel.jointCols(data.J) = oMi.act(jdata.S());
414
415 // Spatial velocity of joint i expressed in the global frame o
416
2/4
✓ Branch 1 taken 432 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 432 times.
✗ Branch 6 not taken.
 864 data.ov[i] = oMi.act(vJ);
417
418 typedef
419 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
420 ColsBlock;
421
1/2
✓ Branch 1 taken 432 times.
✗ Branch 2 not taken.
 864 ColsBlock dJcols = jmodel.jointCols(data.dJ);
422
1/2
✓ Branch 1 taken 432 times.
✗ Branch 2 not taken.
 864 ColsBlock Jcols = jmodel.jointCols(data.J);
423
424
1/2
✓ Branch 2 taken 432 times.
✗ Branch 3 not taken.
 864 motionSet::motionAction(data.ov[i], Jcols, dJcols);
425 }
426 };
427
428 template<
429 typename Scalar,
430 int Options,
431 template<typename, int> class JointCollectionTpl,
432 typename ConfigVectorType,
433 typename TangentVectorType>
434 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x &
435 16 computeJointJacobiansTimeVariation(
436 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
437 DataTpl<Scalar, Options, JointCollectionTpl> & data,
438 const Eigen::MatrixBase<ConfigVectorType> & q,
439 const Eigen::MatrixBase<TangentVectorType> & v)
440 {
441
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 assert(model.check(data) && "data is not consistent with model.");
442
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 16 PINOCCHIO_CHECK_ARGUMENT_SIZE(
443 q.size(), model.nq, "The configuration vector is not of right size");
444
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 16 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
445
446 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
447 typedef typename Model::JointIndex JointIndex;
448
449 typedef JointJacobiansTimeVariationForwardStep<
450 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType>
451 Pass;
452
2/2
✓ Branch 0 taken 432 times.
✓ Branch 1 taken 16 times.
 448 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
453 {
454
1/2
✓ Branch 1 taken 432 times.
✗ Branch 2 not taken.
 432 Pass::run(
455 432 model.joints[i], data.joints[i],
456 864 typename Pass::ArgsType(model, data, q.derived(), v.derived()));
457 }
458
459 16 return data.dJ;
460 }
461
462 template<
463 typename Scalar,
464 int Options,
465 template<typename, int> class JointCollectionTpl,
466 typename Matrix6xLike>
467 7 void getJointJacobianTimeVariation(
468 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
469 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
470 const JointIndex jointId,
471 const ReferenceFrame rf,
472 const Eigen::MatrixBase<Matrix6xLike> & dJ_)
473 {
474 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
475 typedef typename Data::SE3 SE3;
476 typedef typename SE3::Vector3 Vector3;
477 typedef typename Data::Motion Motion;
478
479
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 7 PINOCCHIO_CHECK_INPUT_ARGUMENT(
480 jointId < JointIndex(model.njoints)
481 && "jointId is larger than the number of joints contained in the model");
482
483 7 Matrix6xLike & dJ = dJ_.const_cast_derived();
484 7 ::pinocchio::details::translateJointJacobian(model, data, jointId, rf, data.dJ, dJ);
485
486 // Add contribution for LOCAL and LOCAL_WORLD_ALIGNED
487
3/3
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 3 times.
 7 switch (rf)
488 {
489 2 case LOCAL: {
490 2 const SE3 & oMjoint = data.oMi[jointId];
491 2 const Motion & v_joint = data.v[jointId];
492 2 const int colRef = nv(model.joints[jointId]) + idx_v(model.joints[jointId]) - 1;
493
2/2
✓ Branch 0 taken 28 times.
✓ Branch 1 taken 2 times.
 30 for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
494 {
495 typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn;
496 typedef const MotionRef<ConstColXprIn> MotionIn;
497
498 typedef typename Matrix6xLike::ColXpr ColXprOut;
499 typedef MotionRef<ColXprOut> MotionOut;
500
2/4
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
 28 MotionIn v_in(data.J.col(j));
501
2/4
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
 28 MotionOut v_out(dJ.col(j));
502
503
3/6
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28 times.
✗ Branch 8 not taken.
 28 v_out -= v_joint.cross(oMjoint.actInv(v_in));
504 }
505 2 break;
506 }
507 2 case LOCAL_WORLD_ALIGNED: {
508 2 const Motion & ov_joint = data.ov[jointId];
509 2 const SE3 & oMjoint = data.oMi[jointId];
510 2 const int colRef = nv(model.joints[jointId]) + idx_v(model.joints[jointId]) - 1;
511
2/2
✓ Branch 0 taken 28 times.
✓ Branch 1 taken 2 times.
 30 for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
512 {
513 typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn;
514 typedef const MotionRef<ConstColXprIn> MotionIn;
515
516 typedef typename Matrix6xLike::ColXpr ColXprOut;
517 typedef MotionRef<ColXprOut> MotionOut;
518
2/4
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
 28 MotionIn v_in(data.J.col(j));
519
2/4
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
 28 MotionOut v_out(dJ.col(j));
520
521
3/6
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28 times.
✗ Branch 8 not taken.
 28 v_out.linear() -=
522
6/12
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 28 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 28 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 28 times.
✗ Branch 17 not taken.
 28 Vector3(ov_joint.linear() + ov_joint.angular().cross(oMjoint.translation()))
523
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 56 .cross(v_in.angular());
524 }
525 2 break;
526 }
527
528 3 case WORLD:
529 default:
530 3 break;
531 }
532 7 }
533 } // namespace impl
534
535 template<
536 typename Scalar,
537 int Options,
538 template<typename, int> class JointCollectionTpl,
539 typename ConfigVectorType>
540 299 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeJointJacobians(
541 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
542 DataTpl<Scalar, Options, JointCollectionTpl> & data,
543 const Eigen::MatrixBase<ConfigVectorType> & q)
544 {
545
1/2
✓ Branch 2 taken 293 times.
✗ Branch 3 not taken.
 299 return impl::computeJointJacobians(model, data, make_const_ref(q));
546 }
547
548 template<
549 typename Scalar,
550 int Options,
551 template<typename, int> class JointCollectionTpl,
552 typename Matrix6Like>
553 437 void getJointJacobian(
554 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
555 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
556 const JointIndex joint_id,
557 const ReferenceFrame reference_frame,
558 const Eigen::MatrixBase<Matrix6Like> & J)
559 {
560
1/2
✓ Branch 2 taken 408 times.
✗ Branch 3 not taken.
 437 impl::getJointJacobian(model, data, joint_id, reference_frame, make_ref(J));
561 437 }
562
563 template<
564 typename Scalar,
565 int Options,
566 template<typename, int> class JointCollectionTpl,
567 typename ConfigVectorType,
568 typename Matrix6Like>
569 379 void computeJointJacobian(
570 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
571 DataTpl<Scalar, Options, JointCollectionTpl> & data,
572 const Eigen::MatrixBase<ConfigVectorType> & q,
573 const JointIndex joint_id,
574 const Eigen::MatrixBase<Matrix6Like> & J)
575 {
576
2/4
✓ Branch 2 taken 379 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 379 times.
✗ Branch 6 not taken.
 379 impl::computeJointJacobian(model, data, make_const_ref(q), joint_id, make_ref(J));
577 379 }
578
579 template<
580 typename Scalar,
581 int Options,
582 template<typename, int> class JointCollectionTpl,
583 typename ConfigVectorType,
584 typename TangentVectorType>
585 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x &
586 16 computeJointJacobiansTimeVariation(
587 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
588 DataTpl<Scalar, Options, JointCollectionTpl> & data,
589 const Eigen::MatrixBase<ConfigVectorType> & q,
590 const Eigen::MatrixBase<TangentVectorType> & v)
591
592 {
593
2/4
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
 32 return impl::computeJointJacobiansTimeVariation(
594 32 model, data, make_const_ref(q), make_const_ref(v));
595 }
596
597 template<
598 typename Scalar,
599 int Options,
600 template<typename, int> class JointCollectionTpl,
601 typename Matrix6Like>
602 7 void getJointJacobianTimeVariation(
603 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
604 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
605 const JointIndex joint_id,
606 const ReferenceFrame reference_frame,
607 const Eigen::MatrixBase<Matrix6Like> & dJ)
608 {
609
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 impl::getJointJacobianTimeVariation(model, data, joint_id, reference_frame, make_ref(dJ));
610 7 }
611
612 } // namespace pinocchio
613
614 /// @endcond
615
616 #endif // ifndef __pinocchio_algorithm_jacobian_hxx__
617