GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/model.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 337 350 96.3%
Branches: 341 636 53.6%
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 749 times.
✗ Branch 2 not taken.
 749 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 749 times.
✗ Branch 2 not taken.
 749 const FrameIndex frame_id = model_in.getFrameId(frame_name_in_model_in);
47
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 749 times.
 749 assert(frame_id < model_in.frames.size());
48 749 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 745 times.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 4 times.
✓ Branch 6 taken 745 times.
 753 && 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 745 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 648 times.
✓ Branch 3 taken 20 times.
 668 for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid)
82 {
83
1/2
✓ Branch 2 taken 648 times.
✗ Branch 3 not taken.
 648 Frame frame = modelAB.frames[fid];
84
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 642 times.
 648 if (frame.parentJoint == 0)
85 {
86
2/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 6 PINOCCHIO_CHECK_INPUT_ARGUMENT(
87 !model.existFrame(frame.name, frame.type),
88 "The two models have conflicting frame names.");
89
90 6 frame.parentJoint = jid;
91
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 if (frame.parentFrame != 0)
92 {
93 5 frame.parentFrame = getFrameId(
94
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 modelAB, model, modelAB.frames[frame.parentFrame].name,
95 5 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 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 6 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 6 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
✓ Branch 12 taken 5 times.
✓ Branch 13 taken 1 times.
✓ Branch 14 taken 5 times.
 6 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 6 times.
✗ Branch 2 not taken.
 6 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 15099 times.
✓ Branch 3 taken 267 times.
 30732 for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid)
196 {
197
1/2
✓ Branch 2 taken 15099 times.
✗ Branch 3 not taken.
 30198 Frame frame = modelAB.frames[fid];
198
3/4
✓ Branch 1 taken 15099 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 642 times.
✓ Branch 4 taken 14457 times.
 30198 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 geomModel.collisionPairs.reserve(
423 geomModelA.collisionPairs.size() + geomModelB.collisionPairs.size()
424 + geomModelA.geometryObjects.size() * geomModelB.geometryObjects.size());
425
426 for (std::size_t icp = 0; icp < geomModelA.collisionPairs.size(); ++icp)
427 {
428 const CollisionPair & cp = geomModelA.collisionPairs[icp];
429 GeomIndex go1 = geomModel.getGeometryId(geomModelA.geometryObjects[cp.first].name);
430 GeomIndex go2 = geomModel.getGeometryId(geomModelA.geometryObjects[cp.second].name);
431 geomModel.addCollisionPair(CollisionPair(go1, go2));
432 }
433
434 for (std::size_t icp = 0; icp < geomModelB.collisionPairs.size(); ++icp)
435 {
436 const CollisionPair & cp = geomModelB.collisionPairs[icp];
437 GeomIndex go1 = geomModel.getGeometryId(geomModelB.geometryObjects[cp.first].name);
438 GeomIndex go2 = geomModel.getGeometryId(geomModelB.geometryObjects[cp.second].name);
439 geomModel.addCollisionPair(CollisionPair(go1, go2));
440 }
441
442 // add the collision pairs between geomModelA and geomModelB.
443 for (Index i = 0; i < geomModelA.geometryObjects.size(); ++i)
444 {
445 GeomIndex go1 = geomModel.getGeometryId(geomModelA.geometryObjects[i].name);
446 for (Index j = 0; j < geomModelB.geometryObjects.size(); ++j)
447 {
448 GeomIndex go2 = geomModel.getGeometryId(geomModelB.geometryObjects[j].name);
449 if (
450 geomModel.geometryObjects[go1].parentJoint != geomModel.geometryObjects[go2].parentJoint)
451 geomModel.addCollisionPair(CollisionPair(go1, go2));
452 }
453 }
454 #endif
455 10 }
456
457 template<
458 typename Scalar,
459 int Options,
460 template<typename, int>
461 class JointCollectionTpl,
462 typename ConfigVectorType>
463 15 void buildReducedModel(
464 const ModelTpl<Scalar, Options, JointCollectionTpl> & input_model,
465 std::vector<JointIndex> list_of_joints_to_lock,
466 const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
467 ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model)
468 {
469
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(
470 reference_configuration.size(), input_model.nq,
471 "The configuration vector is not of right size");
472
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 15 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 15 PINOCCHIO_CHECK_INPUT_ARGUMENT(
473 list_of_joints_to_lock.size() <= (size_t)input_model.njoints,
474 "The number of joints to lock is greater than the total of joints in the reduced_model");
475
476 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
477 typedef typename Model::JointModel JointModel;
478 typedef typename Model::JointData JointData;
479 typedef typename Model::Frame Frame;
480 typedef typename Model::SE3 SE3;
481
482 // Sort indexes
483
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());
484
485 15 typename Model::FrameVector::const_iterator frame_it = input_model.frames.begin();
486
487 // Check that they are not two identical elements
488
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)
489 {
490
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 16 PINOCCHIO_CHECK_INPUT_ARGUMENT(
491 list_of_joints_to_lock[id - 1] < list_of_joints_to_lock[id],
492 "The input list_of_joints_to_lock contains two identical indexes.");
493 }
494
495 // Reserve memory
496 15 const Eigen::DenseIndex njoints =
497 15 input_model.njoints - (Eigen::DenseIndex)list_of_joints_to_lock.size();
498
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 reduced_model.names.reserve((size_t)njoints);
499
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 reduced_model.joints.reserve((size_t)njoints);
500
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 reduced_model.jointPlacements.reserve((size_t)njoints);
501
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 reduced_model.parents.reserve((size_t)njoints);
502
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 reduced_model.inertias.reserve((size_t)njoints);
503
504
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 reduced_model.names[0] = input_model.names[0];
505
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 reduced_model.joints[0] = input_model.joints[0];
506
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 reduced_model.jointPlacements[0] = input_model.jointPlacements[0];
507 15 reduced_model.parents[0] = input_model.parents[0];
508
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 reduced_model.inertias[0] = input_model.inertias[0];
509
510
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 reduced_model.name = input_model.name;
511
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 reduced_model.gravity = input_model.gravity;
512
513 15 size_t current_index_to_lock = 0;
514
515
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)
516 {
517
518 297 const JointIndex joint_id_to_lock = (current_index_to_lock < list_of_joints_to_lock.size())
519
2/2
✓ Branch 0 taken 151 times.
✓ Branch 1 taken 146 times.
 297 ? list_of_joints_to_lock[current_index_to_lock]
520 : 0;
521
522 297 const JointIndex input_parent_joint_index = input_model.parents[joint_id];
523 297 const std::string & joint_name = input_model.names[joint_id];
524 297 const JointModel & joint_input_model = input_model.joints[joint_id];
525
526 // retrieve the closest joint parent in the new kinematic tree
527 297 const std::string & parent_joint_name = input_model.names[input_parent_joint_index];
528
1/2
✓ Branch 1 taken 297 times.
✗ Branch 2 not taken.
 297 const bool exist_parent_joint = reduced_model.existJointName(parent_joint_name);
529
530
2/2
✓ Branch 0 taken 274 times.
✓ Branch 1 taken 23 times.
 297 const JointIndex reduced_parent_joint_index =
531 exist_parent_joint
532
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)
533
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 : reduced_model.frames[reduced_model.getFrameId(parent_joint_name)].parentJoint;
534
535
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 =
536 exist_parent_joint
537 ? SE3::Identity()
538
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;
539
540
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 270 times.
 297 const FrameIndex reduced_previous_frame_index =
541
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);
542
543
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 270 times.
 297 if (joint_id == joint_id_to_lock)
544 {
545 // the joint should not be added to the Model but aggragated to its parent joint
546 // Add frames up to the joint to lock
547
1/2
✓ Branch 2 taken 364 times.
✗ Branch 3 not taken.
 364 while ((*frame_it).name != joint_name)
548 {
549 364 ++frame_it;
550 364 const Frame & input_frame = *frame_it;
551
2/2
✓ Branch 1 taken 27 times.
✓ Branch 2 taken 337 times.
 364 if (input_frame.name == joint_name)
552 27 break;
553 337 const std::string & support_joint_name = input_model.names[input_frame.parent];
554
555
1/2
✓ Branch 2 taken 337 times.
✗ Branch 3 not taken.
 337 std::vector<JointIndex>::const_iterator support_joint_it = std::find(
556 337 list_of_joints_to_lock.begin(), list_of_joints_to_lock.end(), input_frame.parent);
557
558
2/2
✓ Branch 2 taken 16 times.
✓ Branch 3 taken 321 times.
 337 if (support_joint_it != list_of_joints_to_lock.end())
559 {
560
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 16 if (
561 ✗ input_frame.type == JOINT && reduced_model.existFrame(input_frame.name)
562
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)
563 ✗ continue; // this means that the Joint is now fixed and has been replaced by a Frame.
564 // No need to add a new one.
565
566 // The joint has been removed and replaced by a Frame
567
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 const FrameIndex joint_frame_id = reduced_model.getFrameId(support_joint_name);
568 16 const Frame & joint_frame = reduced_model.frames[joint_frame_id];
569
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 Frame reduced_frame = input_frame;
570
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;
571 16 reduced_frame.parent = joint_frame.parent;
572 32 reduced_frame.previousFrame =
573
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);
574
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 reduced_model.addFrame(reduced_frame, false);
575 16 }
576 else
577 {
578
1/2
✓ Branch 1 taken 321 times.
✗ Branch 2 not taken.
 321 Frame reduced_frame = input_frame;
579
1/2
✓ Branch 2 taken 321 times.
✗ Branch 3 not taken.
 321 reduced_frame.parent = reduced_model.getJointId(input_model.names[input_frame.parent]);
580 642 reduced_frame.previousFrame =
581
2/4
✓ Branch 2 taken 321 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 321 times.
✗ Branch 6 not taken.
 321 reduced_model.getFrameId(input_model.frames[input_frame.previousFrame].name);
582
1/2
✓ Branch 1 taken 321 times.
✗ Branch 2 not taken.
 321 reduced_model.addFrame(reduced_frame, false);
583 321 }
584 }
585
586 // Compute the new placement of the joint with respect to its parent joint in the new
587 // kinematic tree.
588
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 JointData joint_data = joint_input_model.createData();
589
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 joint_input_model.calc(joint_data, reference_configuration);
590
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 =
591 27 parent_frame_placement * input_model.jointPlacements[joint_id] * joint_data.M();
592
593
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 Frame frame = Frame(
594 joint_name, reduced_parent_joint_index, reduced_previous_frame_index, liMi, FIXED_JOINT,
595 27 input_model.inertias[joint_id]);
596
597
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 FrameIndex frame_id = reduced_model.addFrame(frame);
598 27 reduced_model.frames[frame_id].parentFrame =
599 frame_id; // a bit weird, but this is a solution for missing parent frame
600
601 27 current_index_to_lock++;
602 27 }
603 else
604 {
605 // the joint should be added to the Model as it is
606
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(
607 reduced_parent_joint_index, joint_input_model,
608 270 parent_frame_placement * input_model.jointPlacements[joint_id], joint_name,
609
1/2
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
 540 joint_input_model.jointVelocitySelector(input_model.effortLimit),
610
1/2
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
 540 joint_input_model.jointVelocitySelector(input_model.velocityLimit),
611
1/2
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
 540 joint_input_model.jointConfigSelector(input_model.lowerPositionLimit),
612
1/2
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
 540 joint_input_model.jointConfigSelector(input_model.upperPositionLimit),
613
1/2
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
 540 joint_input_model.jointVelocitySelector(input_model.friction),
614
1/2
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
 270 joint_input_model.jointVelocitySelector(input_model.damping));
615 // Append inertia
616
2/4
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 270 times.
✗ Branch 5 not taken.
 540 reduced_model.appendBodyToJoint(
617 270 reduced_joint_id, input_model.inertias[joint_id], SE3::Identity());
618
619 // Copy other kinematics and dynamics properties
620 270 const typename Model::JointModel & jmodel_out = reduced_model.joints[reduced_joint_id];
621
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) =
622
1/2
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
 270 joint_input_model.jointVelocitySelector(input_model.rotorInertia);
623
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) =
624
1/2
✓ Branch 1 taken 270 times.
✗ Branch 2 not taken.
 540 joint_input_model.jointVelocitySelector(input_model.rotorGearRatio);
625 }
626 }
627
628 // Retrieve and extend the reference configurations
629 typedef typename Model::ConfigVectorMap ConfigVectorMap;
630 15 for (typename ConfigVectorMap::const_iterator config_it =
631 15 input_model.referenceConfigurations.begin();
632
2/2
✓ Branch 3 taken 5 times.
✓ Branch 4 taken 15 times.
 20 config_it != input_model.referenceConfigurations.end(); ++config_it)
633 {
634 5 const std::string & config_name = config_it->first;
635 5 const typename Model::ConfigVectorType & input_config_vector = config_it->second;
636
637
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 typename Model::ConfigVectorType reduced_config_vector(reduced_model.nq);
638
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();
639 ++reduced_joint_id)
640 {
641 const JointIndex input_joint_id =
642
1/2
✓ Branch 2 taken 141 times.
✗ Branch 3 not taken.
 141 input_model.getJointId(reduced_model.names[reduced_joint_id]);
643 141 const JointModel & input_joint_model = input_model.joints[input_joint_id];
644 141 const JointModel & reduced_joint_model = reduced_model.joints[reduced_joint_id];
645
646
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) =
647
1/2
✓ Branch 1 taken 141 times.
✗ Branch 2 not taken.
 282 input_joint_model.jointConfigSelector(input_config_vector);
648 }
649
650
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(
651 std::make_pair(config_name, reduced_config_vector));
652 }
653
654 // Add all the missing frames
655
2/2
✓ Branch 3 taken 404 times.
✓ Branch 4 taken 15 times.
 419 for (; frame_it != input_model.frames.end(); ++frame_it)
656 {
657 404 const Frame & input_frame = *frame_it;
658 404 const std::string & support_joint_name = input_model.names[input_frame.parentJoint];
659
660
1/2
✓ Branch 2 taken 404 times.
✗ Branch 3 not taken.
 404 std::vector<JointIndex>::const_iterator support_joint_it = std::find(
661 404 list_of_joints_to_lock.begin(), list_of_joints_to_lock.end(), input_frame.parentJoint);
662
663
2/2
✓ Branch 2 taken 46 times.
✓ Branch 3 taken 358 times.
 404 if (support_joint_it != list_of_joints_to_lock.end())
664 {
665
2/2
✓ Branch 0 taken 11 times.
✓ Branch 1 taken 35 times.
 46 if (
666
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)
667
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)
668 11 continue; // this means that the Joint is now fixed and has been replaced by a Frame. No
669 // need to add a new one.
670
671 // The joint has been removed and replaced by a Frame
672
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 const FrameIndex joint_frame_id = reduced_model.getFrameId(support_joint_name);
673 35 const Frame & joint_frame = reduced_model.frames[joint_frame_id];
674
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 Frame reduced_frame = input_frame;
675
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;
676 35 reduced_frame.parentJoint = joint_frame.parentJoint;
677 35 reduced_frame.parentFrame =
678
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);
679
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 reduced_model.addFrame(reduced_frame, false);
680 35 }
681 else
682 {
683
1/2
✓ Branch 1 taken 358 times.
✗ Branch 2 not taken.
 358 Frame reduced_frame = input_frame;
684 358 reduced_frame.parentJoint =
685
1/2
✓ Branch 2 taken 358 times.
✗ Branch 3 not taken.
 358 reduced_model.getJointId(input_model.names[input_frame.parentJoint]);
686 358 reduced_frame.parentFrame =
687
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);
688
1/2
✓ Branch 1 taken 358 times.
✗ Branch 2 not taken.
 358 reduced_model.addFrame(reduced_frame, false);
689 358 }
690 }
691 15 }
692
693 template<
694 typename Scalar,
695 int Options,
696 template<typename, int>
697 class JointCollectionTpl,
698 typename ConfigVectorType>
699 3 void buildReducedModel(
700 const ModelTpl<Scalar, Options, JointCollectionTpl> & input_model,
701 const GeometryModel & input_geom_model,
702 const std::vector<JointIndex> & list_of_joints_to_lock,
703 const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
704 ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model,
705 GeometryModel & reduced_geom_model)
706 {
707
708
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 const std::vector<GeometryModel> temp_input_geoms(1, input_geom_model);
709 3 std::vector<GeometryModel> temp_reduced_geom_models;
710
711
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 buildReducedModel(
712 input_model, temp_input_geoms, list_of_joints_to_lock, reference_configuration, reduced_model,
713 temp_reduced_geom_models);
714
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 reduced_geom_model = temp_reduced_geom_models.front();
715 3 }
716
717 template<
718 typename Scalar,
719 int Options,
720 template<typename, int>
721 class JointCollectionTpl,
722 typename GeometryModelAllocator,
723 typename ConfigVectorType>
724 6 void buildReducedModel(
725 const ModelTpl<Scalar, Options, JointCollectionTpl> & input_model,
726 const std::vector<GeometryModel, GeometryModelAllocator> & list_of_geom_models,
727 const std::vector<JointIndex> & list_of_joints_to_lock,
728 const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
729 ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model,
730 std::vector<GeometryModel, GeometryModelAllocator> & list_of_reduced_geom_models)
731 {
732
733 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
734
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 buildReducedModel(input_model, list_of_joints_to_lock, reference_configuration, reduced_model);
735
736 // for all GeometryModels
737
2/2
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 3 times.
 16 for (size_t gmi = 0; gmi < list_of_geom_models.size(); ++gmi)
738 {
739 10 const GeometryModel & input_geom_model = list_of_geom_models[gmi];
740
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 GeometryModel reduced_geom_model;
741
742 // Add all the geometries
743 typedef GeometryModel::GeometryObject GeometryObject;
744 typedef GeometryModel::GeometryObjectVector GeometryObjectVector;
745 84 for (GeometryObjectVector::const_iterator it = input_geom_model.geometryObjects.begin();
746
2/2
✓ Branch 3 taken 37 times.
✓ Branch 4 taken 5 times.
 84 it != input_geom_model.geometryObjects.end(); ++it)
747 {
748 74 const GeometryModel::GeometryObject & geom = *it;
749
750 74 const JointIndex joint_id_in_input_model = geom.parentJoint;
751
1/6
✗ Branch 0 not taken.
✓ Branch 1 taken 37 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 74 _PINOCCHIO_CHECK_INPUT_ARGUMENT_2(
752 (joint_id_in_input_model < (JointIndex)input_model.njoints),
753 "Invalid joint parent index for the geometry with name " + geom.name);
754 74 const std::string & parent_joint_name = input_model.names[joint_id_in_input_model];
755
756 74 JointIndex reduced_joint_id = (JointIndex)-1;
757 typedef typename Model::SE3 SE3;
758
1/2
✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
 74 SE3 relative_placement = SE3::Identity();
759
3/4
✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 20 times.
✓ Branch 4 taken 17 times.
 74 if (reduced_model.existJointName(parent_joint_name))
760 {
761
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 reduced_joint_id = reduced_model.getJointId(parent_joint_name);
762 }
763 else // The joint is now a frame
764 {
765
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 34 const FrameIndex reduced_frame_id = reduced_model.getFrameId(parent_joint_name);
766 34 reduced_joint_id = reduced_model.frames[reduced_frame_id].parentJoint;
767
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 34 relative_placement = reduced_model.frames[reduced_frame_id].placement;
768 }
769
770
1/2
✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
 74 GeometryObject reduced_geom(geom);
771 74 reduced_geom.parentJoint = reduced_joint_id;
772 74 reduced_geom.parentFrame =
773
1/2
✓ Branch 2 taken 37 times.
✗ Branch 3 not taken.
 74 reduced_model.getBodyId(input_model.frames[geom.parentFrame].name);
774
2/4
✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
 74 reduced_geom.placement = relative_placement * geom.placement;
775
1/2
✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
 74 reduced_geom_model.addGeometryObject(reduced_geom);
776 }
777
778 #ifdef PINOCCHIO_WITH_HPP_FCL
779 // Add all the collision pairs - the index of the geometry objects should have not changed
780
781 typedef GeometryModel::CollisionPairVector CollisionPairVector;
782 ✗ for (CollisionPairVector::const_iterator it = input_geom_model.collisionPairs.begin();
783 ✗ it != input_geom_model.collisionPairs.end(); ++it)
784 {
785 ✗ const CollisionPair & cp = *it;
786 ✗ reduced_geom_model.addCollisionPair(cp);
787 }
788 #endif
789
790
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 list_of_reduced_geom_models.push_back(reduced_geom_model);
791 }
792 6 }
793
794 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
795 4 JointIndex findCommonAncestor(
796 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
797 JointIndex joint1_id,
798 JointIndex joint2_id,
799 size_t & index_ancestor_in_support1,
800 size_t & index_ancestor_in_support2)
801 {
802
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 4 PINOCCHIO_CHECK_INPUT_ARGUMENT(
803 joint1_id < (JointIndex)model.njoints, "joint1_id is not valid.");
804
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 4 PINOCCHIO_CHECK_INPUT_ARGUMENT(
805 joint2_id < (JointIndex)model.njoints, "joint2_id is not valid.");
806
807 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
808 typedef typename Model::IndexVector IndexVector;
809
810
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)
811 {
812 3 index_ancestor_in_support1 = index_ancestor_in_support2 = 0;
813 3 return 0;
814 }
815
816 1 const IndexVector & support1 = model.supports[joint1_id];
817 1 const IndexVector & support2 = model.supports[joint2_id];
818
819 1 index_ancestor_in_support1 = support1.size() - 1;
820 1 index_ancestor_in_support2 = support2.size() - 1;
821
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 1 times.
 9 while (joint1_id != joint2_id)
822 {
823
1/2
✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
 8 if (joint1_id > joint2_id)
824 8 joint1_id = support1[--index_ancestor_in_support1];
825 else
826 ✗ joint2_id = support2[--index_ancestor_in_support2];
827 }
828
829 1 return joint1_id;
830 }
831
832 } // namespace pinocchio
833
834 #endif // ifndef __pinocchio_algorithm_model_hxx__
835