GCC Code Coverage Report

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