Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/joint/joint-composite.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 49 | 49 | 100.0% |
Branches: | 69 | 130 | 53.1% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2016-2020 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_multibody_joint_composite_hxx__ | ||
6 | #define __pinocchio_multibody_joint_composite_hxx__ | ||
7 | |||
8 | #include "pinocchio/multibody/visitor.hpp" | ||
9 | |||
10 | namespace pinocchio | ||
11 | { | ||
12 | |||
13 | template< | ||
14 | typename Scalar, | ||
15 | int Options, | ||
16 | template<typename S, int O> | ||
17 | class JointCollectionTpl, | ||
18 | typename ConfigVectorType> | ||
19 | struct JointCompositeCalcZeroOrderStep | ||
20 | : fusion::JointUnaryVisitorBase< | ||
21 | JointCompositeCalcZeroOrderStep<Scalar, Options, JointCollectionTpl, ConfigVectorType>> | ||
22 | { | ||
23 | typedef JointModelCompositeTpl<Scalar, Options, JointCollectionTpl> JointModelComposite; | ||
24 | typedef JointDataCompositeTpl<Scalar, Options, JointCollectionTpl> JointDataComposite; | ||
25 | |||
26 | typedef boost::fusion:: | ||
27 | vector<const JointModelComposite &, JointDataComposite &, const ConfigVectorType &> | ||
28 | ArgsType; | ||
29 | |||
30 | template<typename JointModel> | ||
31 | 26 | static void algo( | |
32 | const pinocchio::JointModelBase<JointModel> & jmodel, | ||
33 | pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
34 | const JointModelComposite & model, | ||
35 | JointDataComposite & data, | ||
36 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
37 | { | ||
38 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
26 | const JointIndex & i = jmodel.id(); |
39 | 26 | const JointIndex succ = i + 1; // successor | |
40 | |||
41 |
1/2✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
|
26 | jmodel.calc(jdata.derived(), q.derived()); |
42 | |||
43 |
4/10✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 8 taken 13 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 12 taken 13 times.
✗ Branch 13 not taken.
|
26 | data.pjMi[i] = model.jointPlacements[i] * jdata.M(); |
44 | |||
45 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 11 times.
|
26 | if (succ == model.joints.size()) |
46 | { | ||
47 |
1/2✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
4 | data.iMlast[i] = data.pjMi[i]; |
48 |
5/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
|
4 | data.S.matrix().rightCols(model.m_nvs[i]) = jdata.S().matrix(); |
49 | } | ||
50 | else | ||
51 | { | ||
52 | 22 | const int idx_v = model.m_idx_v[i] - model.m_idx_v[0]; | |
53 | |||
54 |
2/4✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
|
22 | data.iMlast[i] = data.pjMi[i] * data.iMlast[succ]; |
55 |
5/10✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 11 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 11 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 11 times.
✗ Branch 16 not taken.
|
22 | data.S.matrix().middleCols(idx_v, model.m_nvs[i]) = data.iMlast[succ].actInv(jdata.S()); |
56 | } | ||
57 | } | ||
58 | }; | ||
59 | |||
60 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
61 | template<typename ConfigVectorType> | ||
62 | 2 | inline void JointModelCompositeTpl<Scalar, Options, JointCollectionTpl>::calc( | |
63 | JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const | ||
64 | { | ||
65 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
|
2 | assert(joints.size() > 0); |
66 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
|
2 | assert(data.joints.size() == joints.size()); |
67 | |||
68 | typedef JointCompositeCalcZeroOrderStep<Scalar, Options, JointCollectionTpl, ConfigVectorType> | ||
69 | Algo; | ||
70 | |||
71 |
1/2✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | data.joint_q = qs.segment(idx_q(), nq()); |
72 |
2/2✓ Branch 1 taken 13 times.
✓ Branch 2 taken 2 times.
|
15 | for (int i = (int)(joints.size() - 1); i >= 0; --i) |
73 | { | ||
74 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | Algo::run( |
75 | 13 | joints[(size_t)i], data.joints[(size_t)i], | |
76 | 26 | typename Algo::ArgsType(*this, data, qs.derived())); | |
77 | } | ||
78 | 2 | data.M = data.iMlast.front(); | |
79 | 2 | } | |
80 | |||
81 | template< | ||
82 | typename Scalar, | ||
83 | int Options, | ||
84 | template<typename S, int O> | ||
85 | class JointCollectionTpl, | ||
86 | typename ConfigVectorType, | ||
87 | typename TangentVectorType> | ||
88 | struct JointCompositeCalcFirstOrderStep | ||
89 | : public fusion::JointUnaryVisitorBase<JointCompositeCalcFirstOrderStep< | ||
90 | Scalar, | ||
91 | Options, | ||
92 | JointCollectionTpl, | ||
93 | ConfigVectorType, | ||
94 | TangentVectorType>> | ||
95 | { | ||
96 | typedef JointModelCompositeTpl<Scalar, Options, JointCollectionTpl> JointModelComposite; | ||
97 | typedef JointDataCompositeTpl<Scalar, Options, JointCollectionTpl> JointDataComposite; | ||
98 | |||
99 | typedef boost::fusion::vector< | ||
100 | const JointModelComposite &, | ||
101 | JointDataComposite &, | ||
102 | const ConfigVectorType &, | ||
103 | const TangentVectorType &> | ||
104 | ArgsType; | ||
105 | |||
106 | template<typename JointModel> | ||
107 | 64 | static void algo( | |
108 | const pinocchio::JointModelBase<JointModel> & jmodel, | ||
109 | pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
110 | const JointModelComposite & model, | ||
111 | JointDataComposite & data, | ||
112 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
113 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
114 | { | ||
115 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | const JointIndex & i = jmodel.id(); |
116 | 64 | const JointIndex succ = i + 1; // successor | |
117 | |||
118 |
1/2✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
|
64 | jmodel.calc(jdata.derived(), q.derived(), v.derived()); |
119 | |||
120 |
6/10✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 26 times.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 26 times.
✗ Branch 13 not taken.
|
64 | data.pjMi[i] = model.jointPlacements[i] * jdata.M(); |
121 | |||
122 |
2/2✓ Branch 1 taken 8 times.
✓ Branch 2 taken 24 times.
|
64 | if (succ == model.joints.size()) |
123 | { | ||
124 |
1/2✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
|
16 | data.iMlast[i] = data.pjMi[i]; |
125 |
5/10✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 8 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 8 times.
✗ Branch 15 not taken.
|
16 | data.S.matrix().rightCols(model.m_nvs[i]) = jdata.S().matrix(); |
126 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | data.v = jdata.v(); |
127 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | data.c = jdata.c(); |
128 | } | ||
129 | else | ||
130 | { | ||
131 | 48 | const int idx_v = model.m_idx_v[i] - model.m_idx_v[0]; | |
132 | |||
133 |
2/4✓ Branch 3 taken 24 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
|
48 | data.iMlast[i] = data.pjMi[i] * data.iMlast[succ]; |
134 |
5/10✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 24 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 24 times.
✗ Branch 16 not taken.
|
48 | data.S.matrix().middleCols(idx_v, model.m_nvs[i]) = data.iMlast[succ].actInv(jdata.S()); |
135 | |||
136 |
2/4✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
|
48 | typename JointModelComposite::Motion v_tmp = data.iMlast[succ].actInv(jdata.v()); |
137 | |||
138 |
1/2✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
|
48 | data.v += v_tmp; |
139 | |||
140 |
2/4✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
|
48 | data.c -= data.v.cross(v_tmp); |
141 |
3/6✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24 times.
✗ Branch 9 not taken.
|
48 | data.c += data.iMlast[succ].actInv(jdata.c()); |
142 | } | ||
143 | } | ||
144 | }; | ||
145 | |||
146 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
147 | template<typename ConfigVectorType, typename TangentVectorType> | ||
148 | 8 | inline void JointModelCompositeTpl<Scalar, Options, JointCollectionTpl>::calc( | |
149 | JointDataDerived & jdata, | ||
150 | const Eigen::MatrixBase<ConfigVectorType> & qs, | ||
151 | const Eigen::MatrixBase<TangentVectorType> & vs) const | ||
152 | { | ||
153 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
|
8 | assert(joints.size() > 0); |
154 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
|
8 | assert(jdata.joints.size() == joints.size()); |
155 | |||
156 | typedef JointCompositeCalcFirstOrderStep< | ||
157 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType> | ||
158 | Algo; | ||
159 | |||
160 |
1/2✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
8 | jdata.joint_q = qs.segment(idx_q(), nq()); |
161 |
1/2✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
8 | jdata.joint_v = vs.segment(idx_v(), nv()); |
162 |
2/2✓ Branch 1 taken 32 times.
✓ Branch 2 taken 8 times.
|
40 | for (int i = (int)(joints.size() - 1); i >= 0; --i) |
163 | { | ||
164 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
32 | Algo::run( |
165 | 32 | joints[(size_t)i], jdata.joints[(size_t)i], | |
166 | 64 | typename Algo::ArgsType(*this, jdata, qs.derived(), vs.derived())); | |
167 | } | ||
168 | |||
169 | 8 | jdata.M = jdata.iMlast.front(); | |
170 | 8 | } | |
171 | |||
172 | template< | ||
173 | typename Scalar, | ||
174 | int Options, | ||
175 | template<typename S, int O> | ||
176 | class JointCollectionTpl, | ||
177 | typename TangentVectorType> | ||
178 | struct JointCompositeCalcFirstOrderStep< | ||
179 | Scalar, | ||
180 | Options, | ||
181 | JointCollectionTpl, | ||
182 | Blank, | ||
183 | TangentVectorType> | ||
184 | : public fusion::JointUnaryVisitorBase<JointCompositeCalcFirstOrderStep< | ||
185 | Scalar, | ||
186 | Options, | ||
187 | JointCollectionTpl, | ||
188 | Blank, | ||
189 | TangentVectorType>> | ||
190 | { | ||
191 | typedef JointModelCompositeTpl<Scalar, Options, JointCollectionTpl> JointModelComposite; | ||
192 | typedef JointDataCompositeTpl<Scalar, Options, JointCollectionTpl> JointDataComposite; | ||
193 | |||
194 | typedef boost::fusion:: | ||
195 | vector<const JointModelComposite &, JointDataComposite &, Blank, const TangentVectorType &> | ||
196 | ArgsType; | ||
197 | |||
198 | template<typename JointModel> | ||
199 | static void algo( | ||
200 | const pinocchio::JointModelBase<JointModel> & jmodel, | ||
201 | pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
202 | const JointModelComposite & model, | ||
203 | JointDataComposite & data, | ||
204 | const Blank blank, | ||
205 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
206 | { | ||
207 | const JointIndex i = jmodel.id(); | ||
208 | const JointIndex succ = i + 1; // successor | ||
209 | |||
210 | jmodel.calc(jdata.derived(), blank, v.derived()); | ||
211 | |||
212 | if (succ == model.joints.size()) | ||
213 | { | ||
214 | data.v = jdata.v(); | ||
215 | data.c = jdata.c(); | ||
216 | } | ||
217 | else | ||
218 | { | ||
219 | typename JointModelComposite::Motion v_tmp = data.iMlast[succ].actInv(jdata.v()); | ||
220 | |||
221 | data.v += v_tmp; | ||
222 | |||
223 | data.c -= data.v.cross(v_tmp); | ||
224 | data.c += data.iMlast[succ].actInv(jdata.c()); | ||
225 | } | ||
226 | } | ||
227 | }; | ||
228 | |||
229 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
230 | template<typename TangentVectorType> | ||
231 | inline void JointModelCompositeTpl<Scalar, Options, JointCollectionTpl>::calc( | ||
232 | JointDataDerived & jdata, | ||
233 | const Blank blank, | ||
234 | const Eigen::MatrixBase<TangentVectorType> & vs) const | ||
235 | { | ||
236 | assert(joints.size() > 0); | ||
237 | assert(jdata.joints.size() == joints.size()); | ||
238 | |||
239 | typedef JointCompositeCalcFirstOrderStep< | ||
240 | Scalar, Options, JointCollectionTpl, Blank, TangentVectorType> | ||
241 | Algo; | ||
242 | |||
243 | jdata.joint_v = vs.segment(idx_v(), nv()); | ||
244 | for (int i = (int)(joints.size() - 1); i >= 0; --i) | ||
245 | { | ||
246 | Algo::run( | ||
247 | joints[(size_t)i], jdata.joints[(size_t)i], | ||
248 | typename Algo::ArgsType(*this, jdata, blank, vs.derived())); | ||
249 | } | ||
250 | |||
251 | jdata.M = jdata.iMlast.front(); | ||
252 | } | ||
253 | |||
254 | } // namespace pinocchio | ||
255 | |||
256 | #endif // ifndef __pinocchio_multibody_joint_composite_hxx__ | ||
257 |