GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/model.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 359 370 97.0%
Branches: 364 672 54.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2019-2022 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_model_hxx__
6 #define __pinocchio_algorithm_model_hxx__
7
8 #include <algorithm>
9 #include "pinocchio/algorithm/joint-configuration.hpp"
10 #include "pinocchio/spatial/fwd.hpp"
11
12 namespace pinocchio
13 {
14 namespace details
15 {
16
17 // Retrieve the joint id in model_out, given the info of model_in.
18 // If the user change all the joint names, the universe name won't correspond to the first joint
19 // in the tree when searching by name. We thus need to retrieve it with other means, e.g.
20 // checking the index of the joints.
21 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
22 246 JointIndex getJointId(
23 const ModelTpl<Scalar, Options, JointCollectionTpl> & model_in,
24 const ModelTpl<Scalar, Options, JointCollectionTpl> & model_out,
25 const std::string & joint_name_in_model_in)
26 {
27 246 const JointIndex joint_id = model_in.getJointId(joint_name_in_model_in);
28
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 246 times.
 246 assert(joint_id < model_in.joints.size());
29
2/6
✗ Branch 0 not taken.
✓ Branch 1 taken 246 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 246 times.
 246 if (joint_id == 0 && model_in.parents[0] == 0) // This is the universe, maybe renamed.
30 ✗ return model_out.getJointId(model_out.names[0]);
31 else
32 246 return model_out.getJointId(joint_name_in_model_in);
33 }
34
35 // Retrieve the frame id in model_out, given the info of model_in.
36 // If the user change all the frame names, the universe name won't correspond to the first frame
37 // in the tree when searching by name. We thus need to retrieve it with other means, e.g.
38 // checking the fields of the frames.
39 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
40
1/2
✓ Branch 1 taken 748 times.
✗ Branch 2 not taken.
 748 FrameIndex getFrameId(
41 const ModelTpl<Scalar, Options, JointCollectionTpl> & model_in,
42 const ModelTpl<Scalar, Options, JointCollectionTpl> & model_out,
43 const std::string & frame_name_in_model_in,
44 const FrameType & type)
45 {
46
1/2
✓ Branch 1 taken 748 times.
✗ Branch 2 not taken.
 748 const FrameIndex frame_id = model_in.getFrameId(frame_name_in_model_in);
47
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 748 times.
 748 assert(frame_id < model_in.frames.size());
48 748 if (
49
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 frame_id == 0 && model_in.frames[0].parentFrame == 0
50
5/6
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 744 times.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 4 times.
✓ Branch 6 taken 744 times.
 752 && model_in.frames[0].parent == 0) // This is the universe, maybe renamed.
51 4 return model_out.getFrameId(model_out.frames[0].name, type);
52 else
53 744 return model_out.getFrameId(frame_name_in_model_in, type);
54 }
55
56 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
57 20 void appendUniverseToModel(
58 const ModelTpl<Scalar, Options, JointCollectionTpl> & modelAB,
59 const GeometryModel & geomModelAB,
60 FrameIndex parentFrame,
61 const SE3Tpl<Scalar, Options> & pfMAB,
62 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
63 GeometryModel & geomModel)
64 {
65 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
66 typedef typename Model::Frame Frame;
67
68
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 20 PINOCCHIO_THROW(
69 parentFrame < model.frames.size(), std::invalid_argument,
70 "parentFrame is greater than the size of the frames vector.");
71
72 20 const Frame & pframe = model.frames[parentFrame];
73 20 JointIndex jid = pframe.parentJoint;
74
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
 20 assert(jid < model.joints.size());
75
76 // If inertia is not NaN, add it.
77
1/2
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
 20 if (modelAB.inertias[0] == modelAB.inertias[0])
78
1/2
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
 20 model.appendBodyToJoint(jid, modelAB.inertias[0], pframe.placement * pfMAB);
79
80 // Add all frames whose parent is this joint.
81
2/2
✓ Branch 2 taken 647 times.
✓ Branch 3 taken 20 times.
 667 for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid)
82 {
83
1/2
✓ Branch 2 taken 647 times.
✗ Branch 3 not taken.
 647 Frame frame = modelAB.frames[fid];
84
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 642 times.
 647 if (frame.parentJoint == 0)
85 {
86
2/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 5 PINOCCHIO_CHECK_INPUT_ARGUMENT(
87 !model.existFrame(frame.name, frame.type),
88 "The two models have conflicting frame names.");
89
90 5 frame.parentJoint = jid;
91
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
 5 if (frame.parentFrame != 0)
92 {
93 4 frame.parentFrame = getFrameId(
94
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 modelAB, model, modelAB.frames[frame.parentFrame].name,
95 4 modelAB.frames[frame.parentFrame].type);
96 }
97 else
98 {
99 1 frame.parentFrame = parentFrame;
100 }
101
102 // Modify frame placement
103
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 frame.placement = pframe.placement * pfMAB * frame.placement;
104 // Some frames may have some inertia attached to them. In this case, we need to remove it
105 // from the parent joint. To prevent introducing NaNs, we check if the frame inertia is
106 // not NaN and is not zero.
107
8/12
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
✓ Branch 12 taken 4 times.
✓ Branch 13 taken 1 times.
✓ Branch 14 taken 4 times.
 5 if (frame.inertia == frame.inertia && frame.inertia != Inertia::Zero())
108 {
109
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 model.inertias[frame.parentJoint] -= frame.inertia;
110 }
111
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 model.addFrame(frame);
112 }
113 }
114
115 // Add all geometries whose parent is this joint.
116
2/2
✓ Branch 2 taken 114 times.
✓ Branch 3 taken 20 times.
 134 for (GeomIndex gid = 0; gid < geomModelAB.geometryObjects.size(); ++gid)
117 {
118
1/2
✓ Branch 2 taken 114 times.
✗ Branch 3 not taken.
 114 GeometryObject go = geomModelAB.geometryObjects[gid];
119
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 113 times.
 114 if (go.parentJoint == 0)
120 {
121 1 go.parentJoint = jid;
122
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (go.parentFrame != 0)
123 {
124 1 go.parentFrame = getFrameId(
125
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 modelAB, model, modelAB.frames[go.parentFrame].name,
126 1 modelAB.frames[go.parentFrame].type);
127 }
128 else
129 {
130 ✗ go.parentFrame = parentFrame;
131 }
132
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 go.placement = pframe.placement * pfMAB * go.placement;
133
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 geomModel.addGeometryObject(go);
134 }
135 }
136 20 }
137
138 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
139 struct AppendJointOfModelAlgoTpl
140 : public fusion::JointUnaryVisitorBase<
141 AppendJointOfModelAlgoTpl<Scalar, Options, JointCollectionTpl>>
142 {
143
144 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
145 typedef typename Model::Frame Frame;
146
147 typedef boost::fusion::vector<
148 const Model &,
149 const GeometryModel &,
150 JointIndex,
151 const typename Model::SE3 &,
152 Model &,
153 GeometryModel &>
154 ArgsType;
155
156 template<typename JointModel>
157 534 static void algo(
158 const JointModelBase<JointModel> & jmodel_in,
159 const Model & modelAB,
160 const GeometryModel & geomModelAB,
161 JointIndex parent_id,
162 const typename Model::SE3 & pMi,
163 Model & model,
164 GeometryModel & geomModel)
165 {
166 // If old parent is universe, use what's provided in the input.
167 // otherwise, get the parent from modelAB.
168 534 const JointIndex joint_id_in = jmodel_in.id();
169
2/2
✓ Branch 1 taken 246 times.
✓ Branch 2 taken 21 times.
 534 if (modelAB.parents[joint_id_in] > 0)
170 492 parent_id = getJointId(modelAB, model, modelAB.names[modelAB.parents[joint_id_in]]);
171
172
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 267 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 534 PINOCCHIO_CHECK_INPUT_ARGUMENT(
173 !model.existJointName(modelAB.names[joint_id_in]),
174 "The two models have conflicting joint names.");
175
176
9/18
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 267 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 267 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 267 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 267 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 267 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 267 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 267 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 267 times.
✗ Branch 26 not taken.
 3738 JointIndex joint_id_out = model.addJoint(
177 534 parent_id, jmodel_in, pMi * modelAB.jointPlacements[joint_id_in],
178
1/2
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
 1602 modelAB.names[joint_id_in], jmodel_in.jointVelocitySelector(modelAB.effortLimit),
179
1/2
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
 1068 jmodel_in.jointVelocitySelector(modelAB.velocityLimit),
180
1/2
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
 1068 jmodel_in.jointConfigSelector(modelAB.lowerPositionLimit),
181
1/2
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
 1068 jmodel_in.jointConfigSelector(modelAB.upperPositionLimit),
182
1/2
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
 1068 jmodel_in.jointVelocitySelector(modelAB.friction),
183 534 jmodel_in.jointVelocitySelector(modelAB.damping));
184
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 267 times.
 534 assert(joint_id_out < model.joints.size());
185
186
1/2
✓ Branch 3 taken 267 times.
✗ Branch 4 not taken.
 534 model.appendBodyToJoint(joint_id_out, modelAB.inertias[joint_id_in]);
187
188 534 const typename Model::JointModel & jmodel_out = model.joints[joint_id_out];
189
2/4
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 267 times.
✗ Branch 5 not taken.
 534 jmodel_out.jointVelocitySelector(model.rotorInertia) =
190 534 jmodel_in.jointVelocitySelector(modelAB.rotorInertia);
191
2/4
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 267 times.
✗ Branch 5 not taken.
 534 jmodel_out.jointVelocitySelector(model.rotorGearRatio) =
192 534 jmodel_in.jointVelocitySelector(modelAB.rotorGearRatio);
193
194 // Add all frames whose parent is this joint.
195
2/2
✓ Branch 2 taken 15093 times.
✓ Branch 3 taken 267 times.
 30720 for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid)
196 {
197
1/2
✓ Branch 2 taken 15093 times.
✗ Branch 3 not taken.
 30186 Frame frame = modelAB.frames[fid];
198
3/4
✓ Branch 1 taken 15093 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 642 times.
✓ Branch 4 taken 14451 times.
 30186 if (frame.parentJoint == jmodel_in.id())
199 {
200
2/6
✓ Branch 1 taken 642 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 642 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 1284 PINOCCHIO_CHECK_INPUT_ARGUMENT(
201 !model.existFrame(frame.name, frame.type),
202 "The two models have conflicting frame names.");
203
204 1284 frame.parentJoint = joint_id_out;
205
3/4
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 622 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 20 times.
 1284 assert(frame.parentFrame > 0 || frame.type == JOINT);
206
2/2
✓ Branch 0 taken 622 times.
✓ Branch 1 taken 20 times.
 1284 if (frame.parentFrame != 0)
207 {
208 1244 frame.parentFrame = getFrameId(
209
1/2
✓ Branch 1 taken 622 times.
✗ Branch 2 not taken.
 1244 modelAB, model, modelAB.frames[frame.parentFrame].name,
210 1244 modelAB.frames[frame.parentFrame].type);
211 }
212 // Some frames may have some inertia attached to them. In this case, we need to remove
213 // it from the parent joint. To prevent introducing NaNs, we check if the frame inertia
214 // is not NaN and is not zero.
215
8/12
✓ Branch 1 taken 642 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 642 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 642 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 642 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
✓ Branch 12 taken 641 times.
✓ Branch 13 taken 1 times.
✓ Branch 14 taken 641 times.
 1284 if (frame.inertia == frame.inertia && frame.inertia != Inertia::Zero())
216 {
217
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 model.inertias[frame.parentJoint] -= frame.inertia;
218 }
219
1/2
✓ Branch 1 taken 642 times.
✗ Branch 2 not taken.
 1284 model.addFrame(frame);
220 }
221 }
222 // Add all geometries whose parent is this joint.
223
2/2
✓ Branch 2 taken 2537 times.
✓ Branch 3 taken 267 times.
 5608 for (GeomIndex gid = 0; gid < geomModelAB.geometryObjects.size(); ++gid)
224 {
225
1/2
✓ Branch 2 taken 2537 times.
✗ Branch 3 not taken.
 5074 GeometryObject go = geomModelAB.geometryObjects[gid];
226
2/2
✓ Branch 0 taken 113 times.
✓ Branch 1 taken 2424 times.
 5074 if (go.parentJoint == joint_id_in)
227 {
228 226 go.parentJoint = joint_id_out;
229
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 113 times.
 226 assert(go.parentFrame > 0);
230
5/6
✓ Branch 0 taken 113 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 111 times.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 111 times.
✓ Branch 6 taken 2 times.
 226 if (go.parentFrame != 0 && go.parentFrame < modelAB.frames.size())
231 {
232 222 go.parentFrame = getFrameId(
233
1/2
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
 222 modelAB, model, modelAB.frames[go.parentFrame].name,
234 222 modelAB.frames[go.parentFrame].type);
235 }
236
1/2
✓ Branch 1 taken 113 times.
✗ Branch 2 not taken.
 226 geomModel.addGeometryObject(go);
237 }
238 }
239 }
240 };
241
242 } // namespace details
243
244 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
245 5 void appendModel(
246 const ModelTpl<Scalar, Options, JointCollectionTpl> & modelA,
247 const ModelTpl<Scalar, Options, JointCollectionTpl> & modelB,
248 const FrameIndex frameInModelA,
249 const SE3Tpl<Scalar, Options> & aMb,
250 ModelTpl<Scalar, Options, JointCollectionTpl> & model)
251 {
252
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 GeometryModel geomModelA, geomModelB, geomModel;
253
254
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 appendModel(modelA, modelB, geomModelA, geomModelB, frameInModelA, aMb, model, geomModel);
255 5 }
256
257 // Compute whether Joint child is a descendent of parent in a given model
258 // Joints are represented by their id in the model
259 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
260 158 static bool hasAncestor(
261 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
262 JointIndex child,
263 JointIndex parent)
264 {
265 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::IndexVector IndexVector_t;
266 // Any joints has universe as an acenstor
267
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 158 times.
 158 assert(model.supports[child][0] == 0);
268 158 for (typename IndexVector_t::const_iterator it = model.supports[child].begin();
269
1/2
✓ Branch 4 taken 284 times.
✗ Branch 5 not taken.
 284 it != model.supports[child].end(); ++it)
270 {
271
2/2
✓ Branch 1 taken 158 times.
✓ Branch 2 taken 126 times.
 284 if (*it == parent)
272 158 return true;
273 }
274 ✗ return false;
275 }
276
277 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
278 10 void appendModel(
279 const ModelTpl<Scalar, Options, JointCollectionTpl> & modelA,
280 const ModelTpl<Scalar, Options, JointCollectionTpl> & modelB,
281 const GeometryModel & geomModelA,
282 const GeometryModel & geomModelB,
283 const FrameIndex frameInModelA,
284 const SE3Tpl<Scalar, Options> & aMb,
285 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
286 GeometryModel & geomModel)
287 {
288 typedef details::AppendJointOfModelAlgoTpl<Scalar, Options, JointCollectionTpl>
289 AppendJointOfModelAlgo;
290 typedef typename AppendJointOfModelAlgo::ArgsType ArgsType;
291
292
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 10 PINOCCHIO_CHECK_INPUT_ARGUMENT(
293 (bool)(frameInModelA < (FrameIndex)modelA.nframes),
294 "frameInModelA is an invalid Frame index, greater than the "
295 "number of frames contained in modelA.");
296
297 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
298 typedef typename Model::SE3 SE3;
299 typedef typename Model::Frame Frame;
300
301 10 const Frame & frame = modelA.frames[frameInModelA];
302
4/8
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 8 times.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 10 static const SE3 id = SE3::Identity();
303
304 10 int njoints = modelA.njoints + modelB.njoints - 1;
305
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 model.names.reserve((size_t)njoints);
306
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 model.joints.reserve((size_t)njoints);
307
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 model.jointPlacements.reserve((size_t)njoints);
308
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 model.parents.reserve((size_t)njoints);
309
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 model.inertias.reserve((size_t)njoints);
310 10 int nframes = modelA.nframes + modelB.nframes - 1;
311
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 model.frames.reserve((size_t)nframes);
312
313
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 geomModel.geometryObjects.reserve(geomModelA.ngeoms + geomModelB.ngeoms);
314
315
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 details::appendUniverseToModel(modelA, geomModelA, 0, id, model, geomModel);
316 // Compute joints of A that should be added before and after joints of B
317 10 std::vector<JointIndex> AJointsBeforeB;
318 10 std::vector<JointIndex> AJointsAfterB;
319 // All joints until the parent of frameInModelA come first
320
2/2
✓ Branch 0 taken 58 times.
✓ Branch 1 taken 10 times.
 68 for (JointIndex jid = 1; jid <= frame.parent; ++jid)
321 {
322
1/2
✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
 58 AJointsBeforeB.push_back(jid);
323 }
324 // descendants of the parent of frameInModelA come also before model B
325 // TODO(jcarpent): enhancement by taking into account the compactness of the joint ordering.
326
2/2
✓ Branch 1 taken 158 times.
✓ Branch 2 taken 10 times.
 168 for (JointIndex jid = frame.parent + 1; jid < modelA.joints.size(); ++jid)
327 {
328
1/2
✓ Branch 1 taken 158 times.
✗ Branch 2 not taken.
 158 if (hasAncestor(modelA, jid, frame.parent))
329 {
330
1/2
✓ Branch 1 taken 158 times.
✗ Branch 2 not taken.
 158 AJointsBeforeB.push_back(jid);
331 }
332 else
333 {
334 ✗ AJointsAfterB.push_back(jid);
335 }
336 }
337 // Copy modelA joints that should come before model B
338 10 for (std::vector<JointIndex>::const_iterator jid = AJointsBeforeB.begin();
339
2/2
✓ Branch 2 taken 216 times.
✓ Branch 3 taken 10 times.
 226 jid != AJointsBeforeB.end(); ++jid)
340 {
341
1/2
✓ Branch 1 taken 216 times.
✗ Branch 2 not taken.
 216 ArgsType args(modelA, geomModelA, 0, id, model, geomModel);
342
2/4
✓ Branch 1 taken 216 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 216 times.
✗ Branch 7 not taken.
 216 AppendJointOfModelAlgo::run(modelA.joints[*jid], args);
343 }
344
345 // Copy modelB joints
346
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 details::appendUniverseToModel(
347
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 modelB, geomModelB, details::getFrameId(modelA, model, frame.name, frame.type), aMb, model,
348 geomModel);
349
2/2
✓ Branch 1 taken 51 times.
✓ Branch 2 taken 10 times.
 61 for (JointIndex jid = 1; jid < modelB.joints.size(); ++jid)
350 {
351
4/6
✓ Branch 1 taken 11 times.
✓ Branch 2 taken 40 times.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 40 times.
✗ Branch 8 not taken.
 51 SE3 pMi = (modelB.parents[jid] == 0 ? frame.placement * aMb : id);
352
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 ArgsType args(modelB, geomModelB, frame.parentJoint, pMi, model, geomModel);
353
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 51 times.
✗ Branch 6 not taken.
 51 AppendJointOfModelAlgo::run(modelB.joints[jid], args);
354 }
355
356 // Copy remaining joints of modelA
357 // Copy modelA joints that should come before model B
358 10 for (std::vector<JointIndex>::const_iterator jid = AJointsAfterB.begin();
359
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
 10 jid != AJointsAfterB.end(); ++jid)
360 {
361 ✗ ArgsType args(modelA, geomModelA, 0, id, model, geomModel);
362 ✗ AppendJointOfModelAlgo::run(modelA.joints[*jid], args);
363 }
364
365 // Retrieve and set the reference configurations
366 typedef typename Model::ConfigVectorMap ConfigVectorMap;
367
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 typename Model::ConfigVectorType neutral_config_vector(model.nq);
368 // Get neutral configuration
369
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 neutral(model, neutral_config_vector);
370
371 // Get all reference keys from ModelA
372 10 for (typename ConfigVectorMap::const_iterator config_it =
373 10 modelA.referenceConfigurations.begin();
374
2/2
✓ Branch 3 taken 16 times.
✓ Branch 4 taken 10 times.
 26 config_it != modelA.referenceConfigurations.end(); ++config_it)
375 {
376 16 const std::string & config_name = config_it->first;
377 16 const typename Model::ConfigVectorType & config_vectorA = config_it->second;
378
379
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 typename Model::ConfigVectorType config_vector(neutral_config_vector);
380
2/2
✓ Branch 1 taken 418 times.
✓ Branch 2 taken 16 times.
 434 for (JointIndex joint_idA = 1; joint_idA < modelA.joints.size(); ++joint_idA)
381 {
382
1/2
✓ Branch 2 taken 418 times.
✗ Branch 3 not taken.
 418 const JointIndex joint_id = model.getJointId(modelA.names[joint_idA]);
383 418 const typename Model::JointModel & joint_model = model.joints[joint_id];
384 418 const typename Model::JointModel & joint_modelA = modelA.joints[joint_idA];
385
386
2/4
✓ Branch 1 taken 418 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 418 times.
✗ Branch 5 not taken.
 418 joint_model.jointConfigSelector(config_vector) =
387
1/2
✓ Branch 1 taken 418 times.
✗ Branch 2 not taken.
 836 joint_modelA.jointConfigSelector(config_vectorA);
388 }
389
390
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 16 model.referenceConfigurations.insert(std::make_pair(config_name, config_vector));
391 }
392
393 // Get all reference keys from ModelB
394 10 for (typename ConfigVectorMap::const_iterator config_it =
395 10 modelB.referenceConfigurations.begin();
396
2/2
✓ Branch 3 taken 16 times.
✓ Branch 4 taken 10 times.
 26 config_it != modelB.referenceConfigurations.end(); ++config_it)
397 {
398 16 const std::string & config_name = config_it->first;
399 16 const typename Model::ConfigVectorType & config_vectorB = config_it->second;
400
401
3/4
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✓ Branch 6 taken 7 times.
 16 if (model.referenceConfigurations.find(config_name) == model.referenceConfigurations.end())
402 {
403 // not found
404
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 model.referenceConfigurations.insert(std::make_pair(config_name, neutral_config_vector));
405 }
406
407 16 typename Model::ConfigVectorType & config_vector =
408
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 model.referenceConfigurations.find(config_name)->second;
409
2/2
✓ Branch 1 taken 96 times.
✓ Branch 2 taken 16 times.
 112 for (JointIndex joint_idB = 1; joint_idB < modelB.joints.size(); ++joint_idB)
410 {
411
1/2
✓ Branch 2 taken 96 times.
✗ Branch 3 not taken.
 96 const JointIndex joint_id = model.getJointId(modelB.names[joint_idB]);
412 96 const typename Model::JointModel & joint_model = model.joints[joint_id];
413 96 const typename Model::JointModel & joint_modelB = modelB.joints[joint_idB];
414
415
2/4
✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 96 times.
✗ Branch 5 not taken.
 96 joint_model.jointConfigSelector(config_vector) =
416
1/2
✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
 192 joint_modelB.jointConfigSelector(config_vectorB);
417 }
418 }
419
420 #ifdef PINOCCHIO_WITH_HPP_FCL
421 // Add collision pairs of geomModelA and geomModelB
422 20 geomModel.collisionPairs.reserve(
423 10 geomModelA.collisionPairs.size() + geomModelB.collisionPairs.size()
424
1/2
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
 10 + geomModelA.geometryObjects.size() * geomModelB.geometryObjects.size());
425
426
2/2
✓ Branch 1 taken 1041 times.
✓ Branch 2 taken 10 times.
 1051 for (std::size_t icp = 0; icp < geomModelA.collisionPairs.size(); ++icp)
427 {
428 1041 const CollisionPair & cp = geomModelA.collisionPairs[icp];
429
1/2
✓ Branch 2 taken 1041 times.
✗ Branch 3 not taken.
 1041 GeomIndex go1 = geomModel.getGeometryId(geomModelA.geometryObjects[cp.first].name);
430
1/2
✓ Branch 2 taken 1041 times.
✗ Branch 3 not taken.
 1041 GeomIndex go2 = geomModel.getGeometryId(geomModelA.geometryObjects[cp.second].name);
431
2/4
✓ Branch 1 taken 1041 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1041 times.
✗ Branch 5 not taken.
 1041 geomModel.addCollisionPair(CollisionPair(go1, go2));
432 }
433
434
2/2
✓ Branch 1 taken 56 times.
✓ Branch 2 taken 10 times.
 66 for (std::size_t icp = 0; icp < geomModelB.collisionPairs.size(); ++icp)
435 {
436 56 const CollisionPair & cp = geomModelB.collisionPairs[icp];
437
1/2
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
 56 GeomIndex go1 = geomModel.getGeometryId(geomModelB.geometryObjects[cp.first].name);
438
1/2
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
 56 GeomIndex go2 = geomModel.getGeometryId(geomModelB.geometryObjects[cp.second].name);
439
2/4
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
 56 geomModel.addCollisionPair(CollisionPair(go1, go2));
440 }
441
442 // add the collision pairs between geomModelA and geomModelB.
443
2/2
✓ Branch 1 taken 88 times.
✓ Branch 2 taken 10 times.
 98 for (Index i = 0; i < geomModelA.geometryObjects.size(); ++i)
444 {
445
1/2
✓ Branch 2 taken 88 times.
✗ Branch 3 not taken.
 88 GeomIndex go1 = geomModel.getGeometryId(geomModelA.geometryObjects[i].name);
446
2/2
✓ Branch 1 taken 500 times.
✓ Branch 2 taken 88 times.
 588 for (Index j = 0; j < geomModelB.geometryObjects.size(); ++j)
447 {
448
1/2
✓ Branch 2 taken 500 times.
✗ Branch 3 not taken.
 500 GeomIndex go2 = geomModel.getGeometryId(geomModelB.geometryObjects[j].name);
449 500 if (
450
1/2
✓ Branch 2 taken 500 times.
✗ Branch 3 not taken.
 500 geomModel.geometryObjects[go1].parentJoint != geomModel.geometryObjects[go2].parentJoint)
451
2/4
✓ Branch 1 taken 500 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 500 times.
✗ Branch 5 not taken.
 500 geomModel.addCollisionPair(CollisionPair(go1, go2));
452 }
453 }
454 #endif
455 10 }
456
457 template<
458 typename Scalar,
459 int Options,
460 template<typename, int> class JointCollectionTpl,
461 typename ConfigVectorType>
462 15 void buildReducedModel(
463 const ModelTpl<Scalar, Options, JointCollectionTpl> & input_model,
464 std::vector<JointIndex> list_of_joints_to_lock,
465 const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
466 ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model)
467 {
468
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 15 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.
 15 PINOCCHIO_CHECK_ARGUMENT_SIZE(
469 reference_configuration.size(), input_model.nq,
470 "The configuration vector is not of right size");
471
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 15 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 15 PINOCCHIO_CHECK_INPUT_ARGUMENT(
472 list_of_joints_to_lock.size() <= (size_t)input_model.njoints,
473 "The number of joints to lock is greater than the total of joints in the reduced_model");
474
475 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
476 typedef typename Model::JointModel JointModel;
477 typedef typename Model::JointData JointData;
478 typedef typename Model::Frame Frame;
479 typedef typename Model::SE3 SE3;
480
481 // Sort indexes
482
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 std::sort(list_of_joints_to_lock.begin(), list_of_joints_to_lock.end());
483
484 15 typename Model::FrameVector::const_iterator frame_it = input_model.frames.begin();
485
486 // Check that they are not two identical elements
487
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 15 times.
 31 for (size_t id = 1; id < list_of_joints_to_lock.size(); ++id)
488 {
489
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 16 PINOCCHIO_CHECK_INPUT_ARGUMENT(
490 list_of_joints_to_lock[id - 1] < list_of_joints_to_lock[id],
491 "The input list_of_joints_to_lock contains two identical indexes.");
492 }
493
494 // Reserve memory
495 15 const Eigen::DenseIndex njoints =
496 15 input_model.njoints - (Eigen::DenseIndex)list_of_joints_to_lock.size();
497
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 reduced_model.names.reserve((size_t)njoints);
498
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 reduced_model.joints.reserve((size_t)njoints);
499
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 reduced_model.jointPlacements.reserve((size_t)njoints);
500
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 reduced_model.parents.reserve((size_t)njoints);
501
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 reduced_model.inertias.reserve((size_t)njoints);
502
503
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 reduced_model.names[0] = input_model.names[0];
504
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 reduced_model.joints[0] = input_model.joints[0];
505
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 reduced_model.jointPlacements[0] = input_model.jointPlacements[0];
506 15 reduced_model.parents[0] = input_model.parents[0];
507
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 reduced_model.inertias[0] = input_model.inertias[0];
508
509
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 reduced_model.name = input_model.name;
510
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 reduced_model.gravity = input_model.gravity;
511
512 15 size_t current_index_to_lock = 0;
513
514
2/2
✓ Branch 0 taken 297 times.
✓ Branch 1 taken 15 times.
 312 for (JointIndex joint_id = 1; joint_id < (JointIndex)input_model.njoints; ++joint_id)
515 {
516
517 297 const JointIndex joint_id_to_lock = (current_index_to_lock < list_of_joints_to_lock.size())
518
2/2
✓ Branch 0 taken 151 times.
✓ Branch 1 taken 146 times.
 297 ? list_of_joints_to_lock[current_index_to_lock]
519 : 0;
520
521 297 const JointIndex input_parent_joint_index = input_model.parents[joint_id];
522 297 const std::string & joint_name = input_model.names[joint_id];
523 297 const JointModel & joint_input_model = input_model.joints[joint_id];
524
525 // retrieve the closest joint parent in the new kinematic tree
526 297 const std::string & parent_joint_name = input_model.names[input_parent_joint_index];
527
1/2
✓ Branch 1 taken 297 times.
✗ Branch 2 not taken.
 297 const bool exist_parent_joint = reduced_model.existJointName(parent_joint_name);
528
529
2/2
✓ Branch 0 taken 274 times.
✓ Branch 1 taken 23 times.
 297 const JointIndex reduced_parent_joint_index =
530 exist_parent_joint
531
3/4
✓ Branch 0 taken 274 times.
✓ Branch 1 taken 23 times.
✓ Branch 3 taken 274 times.
✗ Branch 4 not taken.
 320 ? reduced_model.getJointId(parent_joint_name)
532
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 : reduced_model.frames[reduced_model.getFrameId(parent_joint_name)].parentJoint;
533
534
3/4
✓ Branch 0 taken 274 times.
✓ Branch 1 taken 23 times.
✓ Branch 3 taken 274 times.
✗ Branch 4 not taken.
 297 const SE3 parent_frame_placement =
535 exist_parent_joint
536 ? SE3::Identity()
537
2/4
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
 23 : reduced_model.frames[reduced_model.getFrameId(parent_joint_name)].placement;
538
539
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 270 times.
 297 const FrameIndex reduced_previous_frame_index =
540
3/4
✓ Branch 0 taken 274 times.
✓ Branch 1 taken 23 times.
✓ Branch 3 taken 23 times.
✗ Branch 4 not taken.
 297 exist_parent_joint ? 0 : reduced_model.getFrameId(parent_joint_name);
541
542
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 270 times.
 297 if (joint_id == joint_id_to_lock)
543 {
544 // the joint should not be added to the Model but aggragated to its parent joint
545 // Add frames up to the joint to lock
546
1/2
✓ Branch 2 taken 358 times.
✗ Branch 3 not taken.
 358 while ((*frame_it).name != joint_name)
547 {
548 358 ++frame_it;
549 358 const Frame & input_frame = *frame_it;
550
2/2
✓ Branch 1 taken 27 times.
✓ Branch 2 taken 331 times.
 358 if (input_frame.name == joint_name)
551 27 break;
552 331 const std::string & support_joint_name = input_model.names[input_frame.parent];
553
554
1/2
✓ Branch 2 taken 331 times.
✗ Branch 3 not taken.
 331 std::vector<JointIndex>::const_iterator support_joint_it = std::find(
555 331 list_of_joints_to_lock.begin(), list_of_joints_to_lock.end(), input_frame.parent);
556
557
2/2
✓ Branch 2 taken 16 times.
✓ Branch 3 taken 315 times.
 331 if (support_joint_it != list_of_joints_to_lock.end())
558 {
559
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 16 if (
560 ✗ input_frame.type == JOINT && reduced_model.existFrame(input_frame.name)
561
2/6
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 16 times.
 16 && support_joint_name == input_frame.name)
562 ✗ continue; // this means that the Joint is now fixed and has been replaced by a Frame.
563 // No need to add a new one.
564
565 // The joint has been removed and replaced by a Frame
566
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 const FrameIndex joint_frame_id = reduced_model.getFrameId(support_joint_name);
567 16 const Frame & joint_frame = reduced_model.frames[joint_frame_id];
568
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 Frame reduced_frame = input_frame;
569
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 16 reduced_frame.placement = joint_frame.placement * input_frame.placement;
570 16 reduced_frame.parent = joint_frame.parent;
571 32 reduced_frame.previousFrame =
572
2/4
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
 16 reduced_model.getFrameId(input_model.frames[input_frame.previousFrame].name);
573
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 reduced_model.addFrame(reduced_frame, false);
574 16 }
575 else
576 {
577
1/2
✓ Branch 1 taken 315 times.
✗ Branch 2 not taken.
 315 Frame reduced_frame = input_frame;
578
1/2
✓ Branch 2 taken 315 times.
✗ Branch 3 not taken.
 315 reduced_frame.parent = reduced_model.getJointId(input_model.names[input_frame.parent]);
579 630 reduced_frame.previousFrame =
580
2/4
✓ Branch 2 taken 315 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 315 times.
✗ Branch 6 not taken.
 315 reduced_model.getFrameId(input_model.frames[input_frame.previousFrame].name);
581
1/2
✓ Branch 1 taken 315 times.
✗ Branch 2 not taken.
 315 reduced_model.addFrame(reduced_frame, false);
582 315 }
583 }
584
585 // Compute the new placement of the joint with respect to its parent joint in the new
586 // kinematic tree.
587
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 JointData joint_data = joint_input_model.createData();
588
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 joint_input_model.calc(joint_data, reference_configuration);
589
3/6
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
 54 const SE3 liMi =
590 27 parent_frame_placement * input_model.jointPlacements[joint_id] * joint_data.M();
591
592
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 Frame frame = Frame(
593 joint_name, reduced_parent_joint_index, reduced_previous_frame_index, liMi, FIXED_JOINT,
594 27 input_model.inertias[joint_id]);
595
596
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 FrameIndex frame_id = reduced_model.addFrame(frame);
597 27 reduced_model.frames[frame_id].parentFrame =
598 frame_id; // a bit weird, but this is a solution for missing parent frame
599
600 27 current_index_to_lock++;
601 27 }
602 else
603 {
604 // the joint should be added to the Model as it is
605
8/16
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 270 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 270 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 270 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 270 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 270 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 270 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 270 times.
✗ Branch 23 not taken.
 1890 JointIndex reduced_joint_id = reduced_model.addJoint(
606 reduced_parent_joint_index, joint_input_model,
607 270 parent_frame_placement * input_model.jointPlacements[joint_id], joint_name,
608
1/2
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
 540 joint_input_model.jointVelocitySelector(input_model.effortLimit),
609
1/2
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
 540 joint_input_model.jointVelocitySelector(input_model.velocityLimit),
610
1/2
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
 540 joint_input_model.jointConfigSelector(input_model.lowerPositionLimit),
611
1/2
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
 540 joint_input_model.jointConfigSelector(input_model.upperPositionLimit),
612
1/2
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
 540 joint_input_model.jointVelocitySelector(input_model.friction),
613
1/2
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
 270 joint_input_model.jointVelocitySelector(input_model.damping));
614 // Append inertia
615
2/4
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 270 times.
✗ Branch 5 not taken.
 540 reduced_model.appendBodyToJoint(
616 270 reduced_joint_id, input_model.inertias[joint_id], SE3::Identity());
617
618 // Copy other kinematics and dynamics properties
619 270 const typename Model::JointModel & jmodel_out = reduced_model.joints[reduced_joint_id];
620
2/4
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 270 times.
✗ Branch 5 not taken.
 270 jmodel_out.jointVelocitySelector(reduced_model.rotorInertia) =
621
1/2
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
 270 joint_input_model.jointVelocitySelector(input_model.rotorInertia);
622
2/4
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 270 times.
✗ Branch 5 not taken.
 270 jmodel_out.jointVelocitySelector(reduced_model.rotorGearRatio) =
623
1/2
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
 540 joint_input_model.jointVelocitySelector(input_model.rotorGearRatio);
624 }
625 }
626
627 // Retrieve and extend the reference configurations
628 typedef typename Model::ConfigVectorMap ConfigVectorMap;
629 15 for (typename ConfigVectorMap::const_iterator config_it =
630 15 input_model.referenceConfigurations.begin();
631
2/2
✓ Branch 3 taken 5 times.
✓ Branch 4 taken 15 times.
 20 config_it != input_model.referenceConfigurations.end(); ++config_it)
632 {
633 5 const std::string & config_name = config_it->first;
634 5 const typename Model::ConfigVectorType & input_config_vector = config_it->second;
635
636
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 typename Model::ConfigVectorType reduced_config_vector(reduced_model.nq);
637
2/2
✓ Branch 1 taken 141 times.
✓ Branch 2 taken 5 times.
 146 for (JointIndex reduced_joint_id = 1; reduced_joint_id < reduced_model.joints.size();
638 ++reduced_joint_id)
639 {
640 const JointIndex input_joint_id =
641
1/2
✓ Branch 2 taken 141 times.
✗ Branch 3 not taken.
 141 input_model.getJointId(reduced_model.names[reduced_joint_id]);
642 141 const JointModel & input_joint_model = input_model.joints[input_joint_id];
643 141 const JointModel & reduced_joint_model = reduced_model.joints[reduced_joint_id];
644
645
2/4
✓ Branch 1 taken 141 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 141 times.
✗ Branch 5 not taken.
 141 reduced_joint_model.jointConfigSelector(reduced_config_vector) =
646
1/2
✓ Branch 1 taken 141 times.
✗ Branch 2 not taken.
 282 input_joint_model.jointConfigSelector(input_config_vector);
647 }
648
649
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 reduced_model.referenceConfigurations.insert(
650 std::make_pair(config_name, reduced_config_vector));
651 }
652
653 // Add all the missing frames
654
2/2
✓ Branch 3 taken 404 times.
✓ Branch 4 taken 15 times.
 419 for (; frame_it != input_model.frames.end(); ++frame_it)
655 {
656 404 const Frame & input_frame = *frame_it;
657 404 const std::string & support_joint_name = input_model.names[input_frame.parentJoint];
658
659
1/2
✓ Branch 2 taken 404 times.
✗ Branch 3 not taken.
 404 std::vector<JointIndex>::const_iterator support_joint_it = std::find(
660 404 list_of_joints_to_lock.begin(), list_of_joints_to_lock.end(), input_frame.parentJoint);
661
662
2/2
✓ Branch 2 taken 46 times.
✓ Branch 3 taken 358 times.
 404 if (support_joint_it != list_of_joints_to_lock.end())
663 {
664
2/2
✓ Branch 0 taken 11 times.
✓ Branch 1 taken 35 times.
 46 if (
665
2/4
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
 11 input_frame.type == JOINT && reduced_model.existFrame(input_frame.name)
666
5/6
✓ Branch 0 taken 11 times.
✓ Branch 1 taken 35 times.
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 11 times.
✓ Branch 6 taken 35 times.
 57 && support_joint_name == input_frame.name)
667 11 continue; // this means that the Joint is now fixed and has been replaced by a Frame. No
668 // need to add a new one.
669
670 // The joint has been removed and replaced by a Frame
671
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 const FrameIndex joint_frame_id = reduced_model.getFrameId(support_joint_name);
672 35 const Frame & joint_frame = reduced_model.frames[joint_frame_id];
673
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 Frame reduced_frame = input_frame;
674
2/4
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35 times.
✗ Branch 5 not taken.
 35 reduced_frame.placement = joint_frame.placement * input_frame.placement;
675 35 reduced_frame.parentJoint = joint_frame.parentJoint;
676 35 reduced_frame.parentFrame =
677
2/4
✓ Branch 2 taken 35 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 35 times.
✗ Branch 6 not taken.
 35 reduced_model.getFrameId(input_model.frames[input_frame.parentFrame].name);
678
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 reduced_model.addFrame(reduced_frame, false);
679 35 }
680 else
681 {
682
1/2
✓ Branch 1 taken 358 times.
✗ Branch 2 not taken.
 358 Frame reduced_frame = input_frame;
683 358 reduced_frame.parentJoint =
684
1/2
✓ Branch 2 taken 358 times.
✗ Branch 3 not taken.
 358 reduced_model.getJointId(input_model.names[input_frame.parentJoint]);
685 358 reduced_frame.parentFrame =
686
2/4
✓ Branch 2 taken 358 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 358 times.
✗ Branch 6 not taken.
 358 reduced_model.getFrameId(input_model.frames[input_frame.parentFrame].name);
687
1/2
✓ Branch 1 taken 358 times.
✗ Branch 2 not taken.
 358 reduced_model.addFrame(reduced_frame, false);
688 358 }
689 }
690 15 }
691
692 template<
693 typename Scalar,
694 int Options,
695 template<typename, int> class JointCollectionTpl,
696 typename ConfigVectorType>
697 3 void buildReducedModel(
698 const ModelTpl<Scalar, Options, JointCollectionTpl> & input_model,
699 const GeometryModel & input_geom_model,
700 const std::vector<JointIndex> & list_of_joints_to_lock,
701 const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
702 ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model,
703 GeometryModel & reduced_geom_model)
704 {
705
706
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 const std::vector<GeometryModel> temp_input_geoms(1, input_geom_model);
707 3 std::vector<GeometryModel> temp_reduced_geom_models;
708
709
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 buildReducedModel(
710 input_model, temp_input_geoms, list_of_joints_to_lock, reference_configuration, reduced_model,
711 temp_reduced_geom_models);
712
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 reduced_geom_model = temp_reduced_geom_models.front();
713 3 }
714
715 template<
716 typename Scalar,
717 int Options,
718 template<typename, int> class JointCollectionTpl,
719 typename GeometryModelAllocator,
720 typename ConfigVectorType>
721 11 void buildReducedModel(
722 const ModelTpl<Scalar, Options, JointCollectionTpl> & input_model,
723 const std::vector<GeometryModel, GeometryModelAllocator> & list_of_geom_models,
724 const std::vector<JointIndex> & list_of_joints_to_lock,
725 const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
726 ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model,
727 std::vector<GeometryModel, GeometryModelAllocator> & list_of_reduced_geom_models)
728 {
729
730 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
731
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 11 buildReducedModel(input_model, list_of_joints_to_lock, reference_configuration, reduced_model);
732
733 // for all GeometryModels
734
2/2
✓ Branch 2 taken 10 times.
✓ Branch 3 taken 6 times.
 28 for (size_t gmi = 0; gmi < list_of_geom_models.size(); ++gmi)
735 {
736 17 const GeometryModel & input_geom_model = list_of_geom_models[gmi];
737
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 17 GeometryModel reduced_geom_model;
738
739 // Add all the geometries
740 typedef GeometryModel::GeometryObject GeometryObject;
741 typedef GeometryModel::GeometryObjectVector GeometryObjectVector;
742 280 for (GeometryObjectVector::const_iterator it = input_geom_model.geometryObjects.begin();
743
2/2
✓ Branch 3 taken 172 times.
✓ Branch 4 taken 10 times.
 280 it != input_geom_model.geometryObjects.end(); ++it)
744 {
745 263 const GeometryModel::GeometryObject & geom = *it;
746
747 263 const JointIndex joint_id_in_input_model = geom.parentJoint;
748
1/6
✗ Branch 0 not taken.
✓ Branch 1 taken 172 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 263 _PINOCCHIO_CHECK_INPUT_ARGUMENT_2(
749 (joint_id_in_input_model < (JointIndex)input_model.njoints),
750 "Invalid joint parent index for the geometry with name " + geom.name);
751 263 const std::string & parent_joint_name = input_model.names[joint_id_in_input_model];
752
753 263 JointIndex reduced_joint_id = (JointIndex)-1;
754 typedef typename Model::SE3 SE3;
755
1/2
✓ Branch 1 taken 172 times.
✗ Branch 2 not taken.
 263 SE3 relative_placement = SE3::Identity();
756
3/4
✓ Branch 1 taken 172 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 155 times.
✓ Branch 4 taken 17 times.
 263 if (reduced_model.existJointName(parent_joint_name))
757 {
758
1/2
✓ Branch 1 taken 155 times.
✗ Branch 2 not taken.
 229 reduced_joint_id = reduced_model.getJointId(parent_joint_name);
759 }
760 else // The joint is now a frame
761 {
762
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 34 const FrameIndex reduced_frame_id = reduced_model.getFrameId(parent_joint_name);
763 34 reduced_joint_id = reduced_model.frames[reduced_frame_id].parentJoint;
764
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 34 relative_placement = reduced_model.frames[reduced_frame_id].placement;
765 }
766
767
1/2
✓ Branch 1 taken 172 times.
✗ Branch 2 not taken.
 263 GeometryObject reduced_geom(geom);
768 263 reduced_geom.parentJoint = reduced_joint_id;
769 263 reduced_geom.parentFrame =
770
1/2
✓ Branch 2 taken 172 times.
✗ Branch 3 not taken.
 263 reduced_model.getBodyId(input_model.frames[geom.parentFrame].name);
771
2/4
✓ Branch 1 taken 172 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 172 times.
✗ Branch 5 not taken.
 263 reduced_geom.placement = relative_placement * geom.placement;
772
1/2
✓ Branch 1 taken 172 times.
✗ Branch 2 not taken.
 263 reduced_geom_model.addGeometryObject(reduced_geom);
773 }
774
775 #ifdef PINOCCHIO_WITH_HPP_FCL
776 // Add all the collision pairs - the index of the geometry objects should have not changed
777
778 typedef GeometryModel::CollisionPairVector CollisionPairVector;
779 17 for (CollisionPairVector::const_iterator it = input_geom_model.collisionPairs.begin();
780
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 10 times.
 17 it != input_geom_model.collisionPairs.end(); ++it)
781 {
782 ✗ const CollisionPair & cp = *it;
783 ✗ reduced_geom_model.addCollisionPair(cp);
784 }
785 #endif
786
787
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 17 list_of_reduced_geom_models.push_back(reduced_geom_model);
788 }
789 11 }
790
791 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
792 4 JointIndex findCommonAncestor(
793 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
794 JointIndex joint1_id,
795 JointIndex joint2_id,
796 size_t & index_ancestor_in_support1,
797 size_t & index_ancestor_in_support2)
798 {
799
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 4 PINOCCHIO_CHECK_INPUT_ARGUMENT(
800 joint1_id < (JointIndex)model.njoints, "joint1_id is not valid.");
801
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 4 PINOCCHIO_CHECK_INPUT_ARGUMENT(
802 joint2_id < (JointIndex)model.njoints, "joint2_id is not valid.");
803
804 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
805 typedef typename Model::IndexVector IndexVector;
806
807
4/4
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 1 times.
 4 if (joint1_id == 0 || joint2_id == 0)
808 {
809 3 index_ancestor_in_support1 = index_ancestor_in_support2 = 0;
810 3 return 0;
811 }
812
813 1 const IndexVector & support1 = model.supports[joint1_id];
814 1 const IndexVector & support2 = model.supports[joint2_id];
815
816 1 index_ancestor_in_support1 = support1.size() - 1;
817 1 index_ancestor_in_support2 = support2.size() - 1;
818
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 1 times.
 9 while (joint1_id != joint2_id)
819 {
820
1/2
✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
 8 if (joint1_id > joint2_id)
821 8 joint1_id = support1[--index_ancestor_in_support1];
822 else
823 ✗ joint2_id = support2[--index_ancestor_in_support2];
824 }
825
826 1 return joint1_id;
827 }
828
829 } // namespace pinocchio
830
831 #endif // ifndef __pinocchio_algorithm_model_hxx__
832