GCC Code Coverage Report

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