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