GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/jacobian.hxx
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 244 247 98.8%
Branches: 267 731 36.5%
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 34870 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 17435 times.
✗ Branch 2 not taken.
 34870 const JointIndex & i = jmodel.id();
49 34870 const JointIndex & parent = model.parents[i];
50
51
1/2
✓ Branch 3 taken 17435 times.
✗ Branch 4 not taken.
 34870 jmodel.calc(jdata.derived(), q.derived());
52
53
6/10
✓ Branch 1 taken 17435 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16341 times.
✓ Branch 5 taken 1094 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16341 times.
✓ Branch 9 taken 1094 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 16341 times.
✗ Branch 13 not taken.
 34870 data.liMi[i] = model.jointPlacements[i] * jdata.M();
54
2/2
✓ Branch 0 taken 16761 times.
✓ Branch 1 taken 674 times.
 34870 if (parent > 0)
55
2/4
✓ Branch 3 taken 16761 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 16761 times.
✗ Branch 8 not taken.
 33522 data.oMi[i] = data.oMi[parent] * data.liMi[i];
56 else
57
1/2
✓ Branch 3 taken 674 times.
✗ Branch 4 not taken.
 1348 data.oMi[i] = data.liMi[i];
58
59 34870 Matrix6xLike & J_ = J.const_cast_derived();
60
4/8
✓ Branch 2 taken 17435 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 17435 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 17435 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 17435 times.
✗ Branch 12 not taken.
 34870 jmodel.jointExtendedModelCols(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 676 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 674 times.
✗ Branch 2 not taken.
 676 assert(model.check(data) && "data is not consistent with model.");
75
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 674 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.
 676 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 17435 times.
✓ Branch 1 taken 674 times.
 18113 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
88 {
89
1/2
✓ Branch 1 taken 17435 times.
✗ Branch 2 not taken.
 17437 Pass::run(
90 17437 model.joints[i], data.joints[i],
91 34874 ArgsType(model, data, q.derived(), data.J.const_cast_derived()));
92 }
93
94 676 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 10206 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 5103 times.
✗ Branch 2 not taken.
 10206 const JointIndex & i = jmodel.id();
116
4/8
✓ Branch 2 taken 5103 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5103 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5103 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5103 times.
✗ Branch 12 not taken.
 10206 jmodel.jointExtendedModelCols(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 189 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 189 times.
✗ Branch 2 not taken.
 189 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 5103 times.
✓ Branch 1 taken 189 times.
 5292 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
132 {
133
1/2
✓ Branch 4 taken 5103 times.
✗ Branch 5 not taken.
 5103 Pass::run(model.joints[i], data.joints[i], typename Pass::ArgsType(data));
134 }
135
136 189 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 1590 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 1590 times.
✗ Branch 2 not taken.
 1590 assert(model.check(data) && "data is not consistent with model.");
185
186
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1590 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.
 1590 PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.rows(), 6);
187
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1590 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.
 1590 PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), model.nvExtended);
188
189
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1590 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.
 1590 PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6);
190
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1590 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.
 1590 PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv);
191
192 1590 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 1590 const bool is_joint_mimic = (model.mimic_joint_supports[joint_id].back() == joint_id);
201 1590 const int joint_first_col = model.idx_vExtendeds[joint_id];
202 1590 const int joint_last_col = model.idx_vExtendeds[joint_id] + model.nvExtendeds[joint_id] - 1;
203
204 // If the current joint is mimic, start the first pass (on non mimic joints) at the first non
205 // mimic parent Else if the joint is not a mimic, include the current joint in the first pass
206 1590 const int colRef =
207
2/2
✓ Branch 0 taken 68 times.
✓ Branch 1 taken 1522 times.
 1590 is_joint_mimic ? data.non_mimic_parents_fromRow[(size_t)joint_first_col] : joint_last_col;
208
209 // If the current joint is mimic, start the second pass (on mimic joints) at the current joint
210 // Else if the joint is not a mimic, start the second pass (on mimic joints) at the first
211 // mimic parent
212 1590 const int colRefMimicPass =
213
2/2
✓ Branch 0 taken 68 times.
✓ Branch 1 taken 1522 times.
 1590 is_joint_mimic ? joint_last_col : data.mimic_parents_fromRow[(size_t)joint_first_col];
214
215
3/4
✓ Branch 0 taken 451 times.
✓ Branch 1 taken 438 times.
✓ Branch 2 taken 701 times.
✗ Branch 3 not taken.
 1590 switch (rf)
216 {
217 451 case WORLD: {
218
2/2
✓ Branch 0 taken 4534 times.
✓ Branch 1 taken 451 times.
 4985 for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0;
219 4534 jExtended = data.non_mimic_parents_fromRow[(size_t)jExtended])
220 {
221
2/4
✓ Branch 1 taken 4534 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4534 times.
✗ Branch 5 not taken.
 4534 MotionIn v_in(Jin.col(jExtended));
222
2/4
✓ Branch 2 taken 4534 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4534 times.
✗ Branch 6 not taken.
 4534 MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[size_t(jExtended)]));
223
224
1/2
✓ Branch 1 taken 4534 times.
✗ Branch 2 not taken.
 4534 v_out = v_in;
225 }
226 // Add mimicking joint effect into mimicked column
227
2/2
✓ Branch 0 taken 82 times.
✓ Branch 1 taken 451 times.
 533 for (Eigen::DenseIndex jExtended = colRefMimicPass; jExtended >= 0;
228 82 jExtended = data.mimic_parents_fromRow[(size_t)jExtended])
229 {
230
2/4
✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82 times.
✗ Branch 5 not taken.
 82 MotionIn v_in(Jin.col(jExtended));
231
2/4
✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 82 times.
✗ Branch 6 not taken.
 82 MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[size_t(jExtended)]));
232
233
1/2
✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
 82 v_out += v_in;
234 }
235 451 break;
236 }
237 438 case LOCAL_WORLD_ALIGNED: {
238
2/2
✓ Branch 0 taken 4390 times.
✓ Branch 1 taken 438 times.
 4828 for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0;
239 4390 jExtended = data.non_mimic_parents_fromRow[(size_t)jExtended])
240 {
241
2/4
✓ Branch 1 taken 4390 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4390 times.
✗ Branch 5 not taken.
 4390 MotionIn v_in(Jin.col(jExtended));
242
2/4
✓ Branch 2 taken 4390 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4390 times.
✗ Branch 6 not taken.
 4390 MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[size_t(jExtended)]));
243
244
1/2
✓ Branch 1 taken 4390 times.
✗ Branch 2 not taken.
 4390 v_out = v_in;
245
6/12
✓ Branch 1 taken 4390 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4390 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4390 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4390 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4390 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4390 times.
✗ Branch 17 not taken.
 4390 v_out.linear().noalias() -= placement.translation().cross(v_in.angular());
246 }
247 // Add mimicking joint effect into mimicked column
248
2/2
✓ Branch 0 taken 82 times.
✓ Branch 1 taken 438 times.
 520 for (Eigen::DenseIndex jExtended = colRefMimicPass; jExtended >= 0;
249 82 jExtended = data.mimic_parents_fromRow[(size_t)jExtended])
250 {
251
2/4
✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82 times.
✗ Branch 5 not taken.
 82 MotionIn v_in(Jin.col(jExtended));
252
2/4
✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 82 times.
✗ Branch 6 not taken.
 82 MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[size_t(jExtended)]));
253
254
1/2
✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
 82 v_out += v_in;
255
6/12
✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 82 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 82 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 82 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 82 times.
✗ Branch 17 not taken.
 82 v_out.linear().noalias() -= placement.translation().cross(v_in.angular());
256 }
257 438 break;
258 }
259 701 case LOCAL: {
260
2/2
✓ Branch 0 taken 7193 times.
✓ Branch 1 taken 701 times.
 7894 for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0;
261 7193 jExtended = data.non_mimic_parents_fromRow[(size_t)jExtended])
262 {
263
2/4
✓ Branch 1 taken 7193 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7193 times.
✗ Branch 5 not taken.
 7193 MotionIn v_in(Jin.col(jExtended));
264
2/4
✓ Branch 2 taken 7193 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7193 times.
✗ Branch 6 not taken.
 7193 MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[size_t(jExtended)]));
265
266
2/4
✓ Branch 1 taken 7193 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7193 times.
✗ Branch 5 not taken.
 7193 v_out = placement.actInv(v_in);
267 }
268 // Add mimicking joint effect into mimicked column
269
2/2
✓ Branch 0 taken 82 times.
✓ Branch 1 taken 701 times.
 783 for (Eigen::DenseIndex jExtended = colRefMimicPass; jExtended >= 0;
270 82 jExtended = data.mimic_parents_fromRow[(size_t)jExtended])
271 {
272
2/4
✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82 times.
✗ Branch 5 not taken.
 82 MotionIn v_in(Jin.col(jExtended));
273
2/4
✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 82 times.
✗ Branch 6 not taken.
 82 MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[size_t(jExtended)]));
274
275
2/4
✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82 times.
✗ Branch 5 not taken.
 82 v_out += placement.actInv(v_in);
276 }
277 701 break;
278 }
279 ✗ default:
280 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "must never happened");
281 break;
282 }
283 1590 }
284
285 template<
286 typename Scalar,
287 int Options,
288 template<typename, int> class JointCollectionTpl,
289 typename Matrix6xLikeIn,
290 typename Matrix6xLikeOut>
291 977 void translateJointJacobian(
292 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
293 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
294 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex joint_id,
295 const ReferenceFrame rf,
296 const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
297 const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
298 {
299 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
300 977 const typename Data::SE3 & oMjoint = data.oMi[joint_id];
301
302 977 translateJointJacobian(model, data, joint_id, rf, oMjoint, Jin, Jout);
303 977 }
304 } // namespace details
305 namespace impl
306 {
307 /* Return the jacobian of the output frame attached to joint <jointId> in the
308 world frame or in the local frame depending on the template argument. The
309 function computeJacobians should have been called first. */
310 template<
311 typename Scalar,
312 int Options,
313 template<typename, int> class JointCollectionTpl,
314 typename Matrix6xLike>
315 970 void getJointJacobian(
316 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
317 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
318 const JointIndex joint_id,
319 const ReferenceFrame reference_frame,
320 const Eigen::MatrixBase<Matrix6xLike> & J)
321 {
322
1/2
✓ Branch 1 taken 970 times.
✗ Branch 2 not taken.
 970 assert(model.check(data) && "data is not consistent with model.");
323
324 970 ::pinocchio::details::translateJointJacobian(
325 970 model, data, joint_id, reference_frame, data.J, J.const_cast_derived());
326 970 }
327
328 /// Compute the minimal number of value to fill the WORLD jacobian for a particular joint.
329 /// Fill liMi, oMi and J.
330 template<
331 typename Scalar,
332 int Options,
333 template<typename, int> class JointCollectionTpl,
334 typename ConfigVectorType,
335 typename Matrix6xLike>
336 struct JointJacobianWorldForwardStep
337 : public fusion::JointUnaryVisitorBase<JointJacobianWorldForwardStep<
338 Scalar,
339 Options,
340 JointCollectionTpl,
341 ConfigVectorType,
342 Matrix6xLike>>
343 {
344 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
345 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
346 typedef JointModelMimicTpl<Scalar, Options, JointCollectionTpl> JointModelMimic;
347
348 typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &, Matrix6xLike &>
349 ArgsType;
350
351 template<typename JointModel>
352 1932 static void algo(
353 const JointModelBase<JointModel> & jmodel,
354 JointDataBase<typename JointModel::JointDataDerived> & jdata,
355 const Model & model,
356 Data & data,
357 const Eigen::MatrixBase<ConfigVectorType> & q,
358 const Eigen::MatrixBase<Matrix6xLike> & J)
359 {
360 1932 algo_impl(jmodel, jdata, model, data, q, J);
361 }
362
363 template<typename JointModel>
364 1856 static void algo_impl(
365 const JointModelBase<JointModel> & jmodel,
366 JointDataBase<typename JointModel::JointDataDerived> & jdata,
367 const Model & model,
368 Data & data,
369 const Eigen::MatrixBase<ConfigVectorType> & q,
370 const Eigen::MatrixBase<Matrix6xLike> & J)
371 {
372 typedef typename Model::JointIndex JointIndex;
373
374
1/2
✓ Branch 1 taken 928 times.
✗ Branch 2 not taken.
 1856 const JointIndex & i = jmodel.id();
375 1856 const JointIndex & parent = model.parents[i];
376
377
1/2
✓ Branch 3 taken 928 times.
✗ Branch 4 not taken.
 1856 jmodel.calc(jdata.derived(), q.derived());
378
379
6/10
✓ Branch 1 taken 928 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 739 times.
✓ Branch 5 taken 189 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 739 times.
✓ Branch 9 taken 189 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 739 times.
✗ Branch 13 not taken.
 1856 data.liMi[i] = model.jointPlacements[i] * jdata.M();
380
2/2
✓ Branch 0 taken 739 times.
✓ Branch 1 taken 189 times.
 1856 if (parent > 0)
381
2/4
✓ Branch 3 taken 739 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 739 times.
✗ Branch 8 not taken.
 1478 data.oMi[i] = data.oMi[parent] * data.liMi[i];
382 else
383
1/2
✓ Branch 3 taken 189 times.
✗ Branch 4 not taken.
 378 data.oMi[i] = data.liMi[i];
384
385 1856 Matrix6xLike & J_ = J.const_cast_derived();
386
4/8
✓ Branch 2 taken 928 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 928 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 928 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 928 times.
✗ Branch 12 not taken.
 1856 jmodel.jointCols(J_) = data.oMi[i].act(jdata.S());
387 }
388
389 // Mimic specialization: We don't fill the jacobian, this will be done in
390 // JointJacobianWorldMimicStep
391 38 static void algo_impl(
392 const JointModelBase<JointModelMimic> & jmodel,
393 JointDataBase<typename JointModelMimic::JointDataDerived> & jdata,
394 const Model & model,
395 Data & data,
396 const Eigen::MatrixBase<ConfigVectorType> & q,
397 const Eigen::MatrixBase<Matrix6xLike> &)
398 {
399 typedef typename Model::JointIndex JointIndex;
400
401
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 38 const JointIndex & i = jmodel.id();
402 38 const JointIndex & parent = model.parents[i];
403
404
1/2
✓ Branch 3 taken 38 times.
✗ Branch 4 not taken.
 38 jmodel.calc(jdata.derived(), q.derived());
405
406
3/6
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 38 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 38 times.
✗ Branch 10 not taken.
 38 data.liMi[i] = model.jointPlacements[i] * jdata.M();
407
1/2
✓ Branch 0 taken 38 times.
✗ Branch 1 not taken.
 38 if (parent > 0)
408
2/4
✓ Branch 3 taken 38 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 38 times.
✗ Branch 8 not taken.
 38 data.oMi[i] = data.oMi[parent] * data.liMi[i];
409 else
410 ✗ data.oMi[i] = data.liMi[i];
411 38 }
412 };
413
414 /// Modify the jacobian to add mimic joint effect
415 template<
416 typename Scalar,
417 int Options,
418 template<typename, int> class JointCollectionTpl,
419 typename Matrix6xLike>
420 struct JointJacobianWorldMimicStep
421 : public fusion::JointUnaryVisitorBase<
422 JointJacobianWorldMimicStep<Scalar, Options, JointCollectionTpl, Matrix6xLike>>
423 {
424 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
425
426 typedef boost::fusion::vector<const Data &, Matrix6xLike &> ArgsType;
427
428 template<typename JointModel>
429 76 static void algo(
430 const JointModelBase<JointModel> & jmodel,
431 JointDataBase<typename JointModel::JointDataDerived> & jdata,
432 const Data & data,
433 const Eigen::MatrixBase<Matrix6xLike> & J)
434 {
435 typedef typename Model::JointIndex JointIndex;
436
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 76 const JointIndex & i = jmodel.id();
437 76 Matrix6xLike & J_ = J.const_cast_derived();
438
439
4/8
✓ Branch 2 taken 38 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 38 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 38 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 38 times.
✗ Branch 12 not taken.
 76 jmodel.jointCols(J_) += data.oMi[i].act(jdata.S());
440 }
441 };
442
443 /// Compute the minimal number of value to fill the LOCAL_WORLD_ALIGNED jacobian for a
444 /// particular joint.
445 /// Need oMi filled.
446 /// Fill J.
447 template<
448 typename Scalar,
449 int Options,
450 template<typename, int> class JointCollectionTpl,
451 typename Matrix6xLike>
452 struct JointJacobianLocalWorldAlignedForwardStep
453 : public fusion::JointUnaryVisitorBase<JointJacobianLocalWorldAlignedForwardStep<
454 Scalar,
455 Options,
456 JointCollectionTpl,
457 Matrix6xLike>>
458 {
459 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
460 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
461 typedef SE3Tpl<Scalar, Options> SE3;
462 typedef JointModelMimicTpl<Scalar, Options, JointCollectionTpl> JointModelMimic;
463
464 typedef boost::fusion::vector<const Data &, const SE3 &, Matrix6xLike &> ArgsType;
465
466 template<typename JointModel>
467 1932 static void algo(
468 const JointModelBase<JointModel> & jmodel,
469 JointDataBase<typename JointModel::JointDataDerived> & jdata,
470 const Data & data,
471 const SE3 & oMf,
472 const Eigen::MatrixBase<Matrix6xLike> & J)
473 {
474 1932 algo_impl(jmodel, jdata, data, oMf, J);
475 }
476
477 template<typename JointModel>
478 1856 static void algo_impl(
479 const JointModelBase<JointModel> & jmodel,
480 JointDataBase<typename JointModel::JointDataDerived> & jdata,
481 const Data & data,
482 const SE3 & oMf,
483 const Eigen::MatrixBase<Matrix6xLike> & J)
484 {
485 typedef typename Model::JointIndex JointIndex;
486
487
1/2
✓ Branch 1 taken 928 times.
✗ Branch 2 not taken.
 1856 const JointIndex & i = jmodel.id();
488
489 1856 Matrix6xLike & J_ = J.const_cast_derived();
490
1/2
✓ Branch 2 taken 928 times.
✗ Branch 3 not taken.
 1856 auto placement = data.oMi[i];
491
4/8
✓ Branch 1 taken 928 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 928 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 928 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 928 times.
✗ Branch 11 not taken.
 1856 placement.translation().noalias() -= oMf.translation();
492
4/8
✓ Branch 1 taken 928 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 928 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 928 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 928 times.
✗ Branch 11 not taken.
 1856 jmodel.jointCols(J_) = placement.act(jdata.S());
493 }
494
495 // Mimic specialization: We apply mimicking joint effect of mimicked column.
496 38 static void algo_impl(
497 const JointModelBase<JointModelMimic> & jmodel,
498 JointDataBase<typename JointModelMimic::JointDataDerived> & jdata,
499 const Data & data,
500 const SE3 & oMf,
501 const Eigen::MatrixBase<Matrix6xLike> & J)
502 {
503 typedef typename Model::JointIndex JointIndex;
504
505
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 38 const JointIndex & i = jmodel.id();
506
507 38 Matrix6xLike & J_ = J.const_cast_derived();
508
1/2
✓ Branch 2 taken 38 times.
✗ Branch 3 not taken.
 38 auto placement = data.oMi[i];
509
4/8
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 38 times.
✗ Branch 11 not taken.
 38 placement.translation().noalias() -= oMf.translation();
510
4/8
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 38 times.
✗ Branch 11 not taken.
 38 jmodel.jointCols(J_) += placement.act(jdata.S());
511 38 }
512 };
513
514 /// Compute the minimal number of value to fill the LOCAL jacobian for a particular joint.
515 /// Fill liMi, iMf and J.
516 template<
517 typename Scalar,
518 int Options,
519 template<typename, int> class JointCollectionTpl,
520 typename ConfigVectorType,
521 typename Matrix6xLike>
522 struct JointJacobianLocalBackwardStep
523 : public fusion::JointUnaryVisitorBase<JointJacobianLocalBackwardStep<
524 Scalar,
525 Options,
526 JointCollectionTpl,
527 ConfigVectorType,
528 Matrix6xLike>>
529 {
530 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
531 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
532 typedef JointModelMimicTpl<Scalar, Options, JointCollectionTpl> JointModelMimic;
533
534 typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &, Matrix6xLike &>
535 ArgsType;
536
537 template<typename JointModel>
538 8482 static void algo(
539 const JointModelBase<JointModel> & jmodel,
540 JointDataBase<typename JointModel::JointDataDerived> & jdata,
541 const Model & model,
542 Data & data,
543 const Eigen::MatrixBase<ConfigVectorType> & q,
544 const Eigen::MatrixBase<Matrix6xLike> & J)
545 {
546 8482 algo_impl(jmodel, jdata, model, data, q, J);
547 }
548
549 template<typename JointModel>
550 8330 static void algo_impl(
551 const JointModelBase<JointModel> & jmodel,
552 JointDataBase<typename JointModel::JointDataDerived> & jdata,
553 const Model & model,
554 Data & data,
555 const Eigen::MatrixBase<ConfigVectorType> & q,
556 const Eigen::MatrixBase<Matrix6xLike> & J)
557 {
558 typedef typename Model::JointIndex JointIndex;
559
1/2
✓ Branch 1 taken 4165 times.
✗ Branch 2 not taken.
 8330 const JointIndex & i = jmodel.id();
560 8330 const JointIndex & parent = model.parents[i];
561 8330 Matrix6xLike & J_ = J.const_cast_derived();
562
563
1/2
✓ Branch 3 taken 4165 times.
✗ Branch 4 not taken.
 8330 jmodel.calc(jdata.derived(), q.derived());
564
565
6/10
✓ Branch 1 taken 4165 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3768 times.
✓ Branch 5 taken 397 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3768 times.
✓ Branch 9 taken 397 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3768 times.
✗ Branch 13 not taken.
 8330 data.liMi[i] = model.jointPlacements[i] * jdata.M();
566
2/4
✓ Branch 3 taken 4165 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 4165 times.
✗ Branch 8 not taken.
 8330 data.iMf[parent] = data.liMi[i] * data.iMf[i];
567
4/8
✓ Branch 2 taken 4165 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4165 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4165 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4165 times.
✗ Branch 12 not taken.
 8330 jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S());
568 }
569
570 // Mimic specialization: We don't fill the jacobian, this will be done in
571 // JointJacobianLocalMimicStep
572 76 static void algo_impl(
573 const JointModelBase<JointModelMimic> & jmodel,
574 JointDataBase<typename JointModelMimic::JointDataDerived> & jdata,
575 const Model & model,
576 Data & data,
577 const Eigen::MatrixBase<ConfigVectorType> & q,
578 const Eigen::MatrixBase<Matrix6xLike> &)
579 {
580 typedef typename Model::JointIndex JointIndex;
581
1/2
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
 76 const JointIndex & i = jmodel.id();
582 76 const JointIndex & parent = model.parents[i];
583
584
1/2
✓ Branch 3 taken 76 times.
✗ Branch 4 not taken.
 76 jmodel.calc(jdata.derived(), q.derived());
585
586
3/6
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 76 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 76 times.
✗ Branch 10 not taken.
 76 data.liMi[i] = model.jointPlacements[i] * jdata.M();
587
2/4
✓ Branch 3 taken 76 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 76 times.
✗ Branch 8 not taken.
 76 data.iMf[parent] = data.liMi[i] * data.iMf[i];
588 76 }
589 };
590
591 /// Modify the jacobian to add mimic joint effect
592 /// Modify J
593 template<
594 typename Scalar,
595 int Options,
596 template<typename, int> class JointCollectionTpl,
597 typename Matrix6xLike>
598 struct JointJacobianLocalMimicStep
599 : public fusion::JointUnaryVisitorBase<
600 JointJacobianLocalMimicStep<Scalar, Options, JointCollectionTpl, Matrix6xLike>>
601 {
602 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
603
604 typedef boost::fusion::vector<const Data &, Matrix6xLike &> ArgsType;
605
606 template<typename JointModel>
607 152 static void algo(
608 const JointModelBase<JointModel> & jmodel,
609 JointDataBase<typename JointModel::JointDataDerived> & jdata,
610 const Data & data,
611 const Eigen::MatrixBase<Matrix6xLike> & J)
612 {
613 typedef typename Model::JointIndex JointIndex;
614
1/2
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
 152 const JointIndex & i = jmodel.id();
615 152 Matrix6xLike & J_ = J.const_cast_derived();
616
617
4/8
✓ Branch 2 taken 76 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 76 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 76 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 76 times.
✗ Branch 12 not taken.
 152 jmodel.jointCols(J_) += data.iMf[i].actInv(jdata.S());
618 }
619 };
620
621 template<
622 typename Scalar,
623 int Options,
624 template<typename, int> class JointCollectionTpl,
625 typename ConfigVectorType,
626 typename Matrix6xLike>
627 567 void computeJointJacobian(
628 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
629 DataTpl<Scalar, Options, JointCollectionTpl> & data,
630 const Eigen::MatrixBase<ConfigVectorType> & q,
631 const JointIndex jointId,
632 const Eigen::MatrixBase<Matrix6xLike> & J)
633 {
634
1/2
✓ Branch 1 taken 567 times.
✗ Branch 2 not taken.
 567 assert(model.check(data) && "data is not consistent with model.");
635
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 567 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.
 567 PINOCCHIO_CHECK_ARGUMENT_SIZE(
636 q.size(), model.nq, "The configuration vector is not of right size");
637
638 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
639 typedef typename Model::JointIndex JointIndex;
640
641 567 data.iMf[jointId].setIdentity();
642 567 Matrix6xLike & J_ = J.const_cast_derived();
643 typedef JointJacobianLocalBackwardStep<
644 Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike>
645 BackwardPass;
646 typedef JointJacobianLocalMimicStep<Scalar, Options, JointCollectionTpl, Matrix6xLike>
647 MimicPass;
648
649 // Fill the jacobian for normal joints
650
2/2
✓ Branch 0 taken 3249 times.
✓ Branch 1 taken 567 times.
 3816 for (JointIndex i = jointId; i > 0; i = model.parents[i])
651 {
652
1/2
✓ Branch 1 taken 3249 times.
✗ Branch 2 not taken.
 3249 BackwardPass::run(
653 3249 model.joints[i], data.joints[i],
654 6498 typename BackwardPass::ArgsType(model, data, q.derived(), J_));
655 }
656
657 // Patch the jacobian with mimic joint effect
658 567 const typename Model::IndexVector & mimic_joint_support = model.mimic_joint_supports[jointId];
659
2/2
✓ Branch 1 taken 38 times.
✓ Branch 2 taken 567 times.
 605 for (size_t i = 1; i < mimic_joint_support.size(); i++)
660 {
661
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 38 MimicPass::run(
662 38 model.joints[mimic_joint_support[i]], data.joints[mimic_joint_support[i]],
663 76 typename MimicPass::ArgsType(data, J_));
664 }
665 567 }
666
667 template<
668 typename Scalar,
669 int Options,
670 template<typename, int> class JointCollectionTpl,
671 typename ConfigVectorType,
672 typename TangentVectorType>
673 struct JointJacobiansTimeVariationForwardStep
674 : public fusion::JointUnaryVisitorBase<JointJacobiansTimeVariationForwardStep<
675 Scalar,
676 Options,
677 JointCollectionTpl,
678 ConfigVectorType,
679 TangentVectorType>>
680 {
681 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
682 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
683
684 typedef boost::fusion::
685 vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &>
686 ArgsType;
687
688 template<typename JointModel>
689 864 static void algo(
690 const JointModelBase<JointModel> & jmodel,
691 JointDataBase<typename JointModel::JointDataDerived> & jdata,
692 const Model & model,
693 Data & data,
694 const Eigen::MatrixBase<ConfigVectorType> & q,
695 const Eigen::MatrixBase<TangentVectorType> & v)
696 {
697 typedef typename Model::JointIndex JointIndex;
698 typedef typename Data::SE3 SE3;
699 typedef typename Data::Motion Motion;
700
701
1/2
✓ Branch 1 taken 432 times.
✗ Branch 2 not taken.
 864 const JointIndex & i = (JointIndex)jmodel.id();
702 864 const JointIndex & parent = model.parents[i];
703
704 864 SE3 & oMi = data.oMi[i];
705 864 Motion & vJ = data.v[i];
706
707
1/2
✓ Branch 4 taken 432 times.
✗ Branch 5 not taken.
 864 jmodel.calc(jdata.derived(), q.derived(), v.derived());
708
709
2/4
✓ Branch 1 taken 432 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 432 times.
✗ Branch 5 not taken.
 864 vJ = jdata.v();
710
711
6/10
✓ Branch 1 taken 432 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 412 times.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 412 times.
✓ Branch 9 taken 20 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 412 times.
✗ Branch 13 not taken.
 864 data.liMi[i] = model.jointPlacements[i] * jdata.M();
712
2/2
✓ Branch 0 taken 416 times.
✓ Branch 1 taken 16 times.
 864 if (parent > 0)
713 {
714
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];
715
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]);
716 }
717 else
718 {
719
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 32 oMi = data.liMi[i];
720 }
721
722
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.jointExtendedModelCols(data.J) = oMi.act(jdata.S());
723
724 // Spatial velocity of joint i expressed in the global frame o
725
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);
726
727 typedef
728 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
729 ColsBlock;
730
1/2
✓ Branch 1 taken 432 times.
✗ Branch 2 not taken.
 864 ColsBlock dJcols = jmodel.jointExtendedModelCols(data.dJ);
731
1/2
✓ Branch 1 taken 432 times.
✗ Branch 2 not taken.
 864 ColsBlock Jcols = jmodel.jointExtendedModelCols(data.J);
732
733
1/2
✓ Branch 2 taken 432 times.
✗ Branch 3 not taken.
 864 motionSet::motionAction(data.ov[i], Jcols, dJcols);
734 }
735 };
736
737 template<
738 typename Scalar,
739 int Options,
740 template<typename, int> class JointCollectionTpl,
741 typename ConfigVectorType,
742 typename TangentVectorType>
743 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x &
744 16 computeJointJacobiansTimeVariation(
745 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
746 DataTpl<Scalar, Options, JointCollectionTpl> & data,
747 const Eigen::MatrixBase<ConfigVectorType> & q,
748 const Eigen::MatrixBase<TangentVectorType> & v)
749 {
750
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 assert(model.check(data) && "data is not consistent with model.");
751
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(
752 q.size(), model.nq, "The configuration vector is not of right size");
753
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");
754
755 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
756 typedef typename Model::JointIndex JointIndex;
757
758 typedef JointJacobiansTimeVariationForwardStep<
759 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType>
760 Pass;
761
2/2
✓ Branch 0 taken 432 times.
✓ Branch 1 taken 16 times.
 448 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
762 {
763
1/2
✓ Branch 1 taken 432 times.
✗ Branch 2 not taken.
 432 Pass::run(
764 432 model.joints[i], data.joints[i],
765 864 typename Pass::ArgsType(model, data, q.derived(), v.derived()));
766 }
767
768 16 return data.dJ;
769 }
770
771 template<
772 typename Scalar,
773 int Options,
774 template<typename, int> class JointCollectionTpl,
775 typename Matrix6xLike>
776 7 void getJointJacobianTimeVariation(
777 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
778 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
779 const JointIndex jointId,
780 const ReferenceFrame rf,
781 const Eigen::MatrixBase<Matrix6xLike> & dJ_)
782 {
783 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
784 typedef typename Data::SE3 SE3;
785 typedef typename SE3::Vector3 Vector3;
786 typedef typename Data::Motion Motion;
787
788
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 7 PINOCCHIO_CHECK_INPUT_ARGUMENT(
789 jointId < JointIndex(model.njoints)
790 && "jointId is larger than the number of joints contained in the model");
791
792 7 Matrix6xLike & dJ = dJ_.const_cast_derived();
793 7 ::pinocchio::details::translateJointJacobian(model, data, jointId, rf, data.dJ, dJ);
794
795 // Add contribution for LOCAL and LOCAL_WORLD_ALIGNED
796
3/3
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 3 times.
 7 switch (rf)
797 {
798 2 case LOCAL: {
799 2 const SE3 & oMjoint = data.oMi[jointId];
800 2 const Motion & v_joint = data.v[jointId];
801 2 const int colRef = model.nvExtendeds[jointId] + model.idx_vExtendeds[jointId] - 1;
802
2/2
✓ Branch 0 taken 28 times.
✓ Branch 1 taken 2 times.
 30 for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0;
803 28 jExtended = data.parents_fromRow[(size_t)jExtended])
804 {
805 typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn;
806 typedef const MotionRef<ConstColXprIn> MotionIn;
807
808 typedef typename Matrix6xLike::ColXpr ColXprOut;
809 typedef MotionRef<ColXprOut> MotionOut;
810
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(jExtended));
811
2/4
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 28 times.
✗ Branch 6 not taken.
 28 MotionOut v_out(dJ.col(data.idx_vExtended_to_idx_v_fromRow[size_t(jExtended)]));
812
813
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));
814 }
815 2 break;
816 }
817 2 case LOCAL_WORLD_ALIGNED: {
818 2 const Motion & ov_joint = data.ov[jointId];
819 2 const SE3 & oMjoint = data.oMi[jointId];
820 2 const int colRef = model.nvExtendeds[jointId] + model.idx_vExtendeds[jointId] - 1;
821
2/2
✓ Branch 0 taken 28 times.
✓ Branch 1 taken 2 times.
 30 for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0;
822 28 jExtended = data.parents_fromRow[(size_t)jExtended])
823 {
824 typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn;
825 typedef const MotionRef<ConstColXprIn> MotionIn;
826
827 typedef typename Matrix6xLike::ColXpr ColXprOut;
828 typedef MotionRef<ColXprOut> MotionOut;
829
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(jExtended));
830
2/4
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 28 times.
✗ Branch 6 not taken.
 28 MotionOut v_out(dJ.col(data.idx_vExtended_to_idx_v_fromRow[size_t(jExtended)]));
831
832
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() -=
833
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()))
834
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 56 .cross(v_in.angular());
835 }
836 2 break;
837 }
838
839 3 case WORLD:
840 default:
841 3 break;
842 }
843 7 }
844 } // namespace impl
845
846 template<
847 typename Scalar,
848 int Options,
849 template<typename, int> class JointCollectionTpl,
850 typename ConfigVectorType>
851 680 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeJointJacobians(
852 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
853 DataTpl<Scalar, Options, JointCollectionTpl> & data,
854 const Eigen::MatrixBase<ConfigVectorType> & q)
855 {
856
1/2
✓ Branch 2 taken 674 times.
✗ Branch 3 not taken.
 680 return impl::computeJointJacobians(model, data, make_const_ref(q));
857 }
858
859 template<
860 typename Scalar,
861 int Options,
862 template<typename, int> class JointCollectionTpl,
863 typename Matrix6Like>
864 999 void getJointJacobian(
865 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
866 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
867 const JointIndex joint_id,
868 const ReferenceFrame reference_frame,
869 const Eigen::MatrixBase<Matrix6Like> & J)
870 {
871
1/2
✓ Branch 2 taken 970 times.
✗ Branch 3 not taken.
 999 impl::getJointJacobian(model, data, joint_id, reference_frame, make_ref(J));
872 999 }
873
874 template<
875 typename Scalar,
876 int Options,
877 template<typename, int> class JointCollectionTpl,
878 typename ConfigVectorType,
879 typename Matrix6Like>
880 567 void computeJointJacobian(
881 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
882 DataTpl<Scalar, Options, JointCollectionTpl> & data,
883 const Eigen::MatrixBase<ConfigVectorType> & q,
884 const JointIndex joint_id,
885 const Eigen::MatrixBase<Matrix6Like> & J)
886 {
887
2/4
✓ Branch 2 taken 567 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 567 times.
✗ Branch 6 not taken.
 567 impl::computeJointJacobian(model, data, make_const_ref(q), joint_id, make_ref(J));
888 567 }
889
890 template<
891 typename Scalar,
892 int Options,
893 template<typename, int> class JointCollectionTpl,
894 typename ConfigVectorType,
895 typename TangentVectorType>
896 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x &
897 16 computeJointJacobiansTimeVariation(
898 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
899 DataTpl<Scalar, Options, JointCollectionTpl> & data,
900 const Eigen::MatrixBase<ConfigVectorType> & q,
901 const Eigen::MatrixBase<TangentVectorType> & v)
902
903 {
904
2/4
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
 32 return impl::computeJointJacobiansTimeVariation(
905 32 model, data, make_const_ref(q), make_const_ref(v));
906 }
907
908 template<
909 typename Scalar,
910 int Options,
911 template<typename, int> class JointCollectionTpl,
912 typename Matrix6Like>
913 7 void getJointJacobianTimeVariation(
914 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
915 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
916 const JointIndex joint_id,
917 const ReferenceFrame reference_frame,
918 const Eigen::MatrixBase<Matrix6Like> & dJ)
919 {
920
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 impl::getJointJacobianTimeVariation(model, data, joint_id, reference_frame, make_ref(dJ));
921 7 }
922
923 } // namespace pinocchio
924
925 /// @endcond
926
927 #endif // ifndef __pinocchio_algorithm_jacobian_hxx__
928