GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/model.hxx
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 491 501 98.0%
Branches: 465 860 54.1%
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].parentJoint == 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 static typename std::enable_if<
158 !std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value,
159 JointModel>::type
160 516 updateMimicIds(
161 const JointModel & jmodel, const Model & /*old_model*/, const Model & /*new_model*/)
162 {
163 516 return jmodel;
164 }
165
166 template<typename JointModel>
167 static typename std::enable_if<
168 std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value,
169 JointModel>::type
170 9 updateMimicIds(
171 const JointModelMimicTpl<Scalar, Options, JointCollectionTpl> & jmodel,
172 const Model & old_model,
173 const Model & new_model)
174 {
175
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 JointModel res(jmodel);
176
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 const JointIndex mimicked_old_id = res.jmodel().id();
177
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 const std::string mimicked_name = old_model.names[mimicked_old_id];
178
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 const JointIndex mimicked_new_id = new_model.getJointId(mimicked_name);
179
4/8
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
 27 res.setMimicIndexes(
180 9 mimicked_new_id, new_model.joints[mimicked_new_id].idx_q(),
181 9 new_model.joints[mimicked_new_id].idx_v(),
182 9 new_model.joints[mimicked_new_id].idx_vExtended());
183 18 return res;
184 9 }
185
186 template<typename JointModel>
187 534 static void algo(
188 const JointModelBase<JointModel> & jmodel_in,
189 const Model & modelAB,
190 const GeometryModel & geomModelAB,
191 JointIndex parent_id,
192 const typename Model::SE3 & pMi,
193 Model & model,
194 GeometryModel & geomModel)
195 {
196 // If old parent is universe, use what's provided in the input.
197 // otherwise, get the parent from modelAB.
198
1/2
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
 534 const JointIndex joint_id_in = jmodel_in.id();
199
2/2
✓ Branch 1 taken 246 times.
✓ Branch 2 taken 21 times.
 534 if (modelAB.parents[joint_id_in] > 0)
200
1/2
✓ Branch 3 taken 246 times.
✗ Branch 4 not taken.
 492 parent_id = getJointId(modelAB, model, modelAB.names[modelAB.parents[joint_id_in]]);
201
202
2/6
✓ Branch 2 taken 267 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 267 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 534 PINOCCHIO_CHECK_INPUT_ARGUMENT(
203 !model.existJointName(modelAB.names[joint_id_in]),
204 "The two models have conflicting joint names.");
205
206 // For mimic joints, update the reference joint id
207
1/2
✓ Branch 2 taken 267 times.
✗ Branch 3 not taken.
 534 JointModel jmodel_inter = updateMimicIds<JointModel>(jmodel_in.derived(), modelAB, model);
208
209
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(
210 parent_id,
211 jmodel_inter, // Use the intermediate joint (jmodel_inter) with updates id, idx, ...
212 534 pMi * modelAB.jointPlacements[joint_id_in], modelAB.names[joint_id_in],
213 534 jmodel_in.jointVelocitySelector(
214 modelAB
215
1/2
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
 534 .effortLimit), // Need to select the vector base on the origin idxs, so use jmodel_in.
216
1/2
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
 1068 jmodel_in.jointVelocitySelector(modelAB.velocityLimit),
217
1/2
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
 1068 jmodel_in.jointConfigSelector(modelAB.lowerPositionLimit),
218
1/2
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
 1068 jmodel_in.jointConfigSelector(modelAB.upperPositionLimit),
219
1/2
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
 1068 jmodel_in.jointVelocitySelector(modelAB.friction),
220
1/2
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
 534 jmodel_in.jointVelocitySelector(modelAB.damping));
221
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 267 times.
 534 assert(joint_id_out < model.joints.size());
222
223
2/4
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 267 times.
✗ Branch 6 not taken.
 534 model.appendBodyToJoint(joint_id_out, modelAB.inertias[joint_id_in]);
224
225 534 typename Model::JointModel & jmodel_out = model.joints[joint_id_out];
226
227
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) =
228
1/2
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
 534 jmodel_in.jointVelocitySelector(modelAB.rotorInertia);
229
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) =
230
1/2
✓ Branch 1 taken 267 times.
✗ Branch 2 not taken.
 534 jmodel_in.jointVelocitySelector(modelAB.rotorGearRatio);
231
232 // Add all frames whose parent is this joint.
233
2/2
✓ Branch 2 taken 15093 times.
✓ Branch 3 taken 267 times.
 30720 for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid)
234 {
235
1/2
✓ Branch 2 taken 15093 times.
✗ Branch 3 not taken.
 30186 Frame frame = modelAB.frames[fid];
236
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())
237 {
238
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(
239 !model.existFrame(frame.name, frame.type),
240 "The two models have conflicting frame names.");
241
242 1284 frame.parentJoint = joint_id_out;
243
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);
244
2/2
✓ Branch 0 taken 622 times.
✓ Branch 1 taken 20 times.
 1284 if (frame.parentFrame != 0)
245 {
246 1244 frame.parentFrame = getFrameId(
247
1/2
✓ Branch 1 taken 622 times.
✗ Branch 2 not taken.
 1244 modelAB, model, modelAB.frames[frame.parentFrame].name,
248 1244 modelAB.frames[frame.parentFrame].type);
249 }
250 // Some frames may have some inertia attached to them. In this case, we need to remove
251 // it from the parent joint. To prevent introducing NaNs, we check if the frame inertia
252 // is not NaN and is not zero.
253
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())
254 {
255
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 model.inertias[frame.parentJoint] -= frame.inertia;
256 }
257
1/2
✓ Branch 1 taken 642 times.
✗ Branch 2 not taken.
 1284 model.addFrame(frame);
258 }
259 }
260 // Add all geometries whose parent is this joint.
261
2/2
✓ Branch 2 taken 2537 times.
✓ Branch 3 taken 267 times.
 5608 for (GeomIndex gid = 0; gid < geomModelAB.geometryObjects.size(); ++gid)
262 {
263
1/2
✓ Branch 2 taken 2537 times.
✗ Branch 3 not taken.
 5074 GeometryObject go = geomModelAB.geometryObjects[gid];
264
2/2
✓ Branch 0 taken 113 times.
✓ Branch 1 taken 2424 times.
 5074 if (go.parentJoint == joint_id_in)
265 {
266 226 go.parentJoint = joint_id_out;
267
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 113 times.
 226 assert(go.parentFrame > 0);
268
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())
269 {
270 222 go.parentFrame = getFrameId(
271
1/2
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
 222 modelAB, model, modelAB.frames[go.parentFrame].name,
272 222 modelAB.frames[go.parentFrame].type);
273 }
274
1/2
✓ Branch 1 taken 113 times.
✗ Branch 2 not taken.
 226 geomModel.addGeometryObject(go);
275 }
276 }
277 }
278 };
279
280 /// Insert \p value inside a sorted \p container so container stay sorted
281 template<typename Container>
282 524 void insertSort(typename Container::const_reference value, Container & container)
283 {
284
1/2
✓ Branch 5 taken 524 times.
✗ Branch 6 not taken.
 524 container.insert(std::lower_bound(container.begin(), container.end(), value), value);
285 524 }
286
287 } // namespace details
288
289 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
290 5 void appendModel(
291 const ModelTpl<Scalar, Options, JointCollectionTpl> & modelA,
292 const ModelTpl<Scalar, Options, JointCollectionTpl> & modelB,
293 const FrameIndex frameInModelA,
294 const SE3Tpl<Scalar, Options> & aMb,
295 ModelTpl<Scalar, Options, JointCollectionTpl> & model)
296 {
297
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;
298
299
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 appendModel(modelA, modelB, geomModelA, geomModelB, frameInModelA, aMb, model, geomModel);
300 5 }
301
302 // Compute whether Joint child is a descendent of parent in a given model
303 // Joints are represented by their id in the model
304 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
305 158 static bool hasAncestor(
306 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
307 JointIndex child,
308 JointIndex parent)
309 {
310 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::IndexVector IndexVector_t;
311 // Any joints has universe as an acenstor
312
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 158 times.
 158 assert(model.supports[child][0] == 0);
313 158 for (typename IndexVector_t::const_iterator it = model.supports[child].begin();
314
1/2
✓ Branch 4 taken 284 times.
✗ Branch 5 not taken.
 284 it != model.supports[child].end(); ++it)
315 {
316
2/2
✓ Branch 1 taken 158 times.
✓ Branch 2 taken 126 times.
 284 if (*it == parent)
317 158 return true;
318 }
319 ✗ return false;
320 }
321
322 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
323 10 void appendModel(
324 const ModelTpl<Scalar, Options, JointCollectionTpl> & modelA,
325 const ModelTpl<Scalar, Options, JointCollectionTpl> & modelB,
326 const GeometryModel & geomModelA,
327 const GeometryModel & geomModelB,
328 const FrameIndex frameInModelA,
329 const SE3Tpl<Scalar, Options> & aMb,
330 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
331 GeometryModel & geomModel)
332 {
333 typedef details::AppendJointOfModelAlgoTpl<Scalar, Options, JointCollectionTpl>
334 AppendJointOfModelAlgo;
335 typedef typename AppendJointOfModelAlgo::ArgsType ArgsType;
336
337
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 10 PINOCCHIO_CHECK_INPUT_ARGUMENT(
338 (bool)(frameInModelA < (FrameIndex)modelA.nframes),
339 "frameInModelA is an invalid Frame index, greater than the "
340 "number of frames contained in modelA.");
341
342 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
343 typedef typename Model::SE3 SE3;
344 typedef typename Model::Frame Frame;
345
346 10 const Frame & frame = modelA.frames[frameInModelA];
347
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();
348
349 10 int njoints = modelA.njoints + modelB.njoints - 1;
350
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 model.names.reserve((size_t)njoints);
351
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 model.joints.reserve((size_t)njoints);
352
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 model.jointPlacements.reserve((size_t)njoints);
353
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 model.parents.reserve((size_t)njoints);
354
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 model.inertias.reserve((size_t)njoints);
355 10 int nframes = modelA.nframes + modelB.nframes - 1;
356
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 model.frames.reserve((size_t)nframes);
357
358
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 geomModel.geometryObjects.reserve(geomModelA.ngeoms + geomModelB.ngeoms);
359
360
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 details::appendUniverseToModel(modelA, geomModelA, 0, id, model, geomModel);
361 // Compute joints of A that should be added before and after joints of B
362 10 std::vector<JointIndex> AJointsBeforeB;
363 10 std::vector<JointIndex> AJointsAfterB;
364 // All joints until the parent of frameInModelA come first
365
2/2
✓ Branch 0 taken 58 times.
✓ Branch 1 taken 10 times.
 68 for (JointIndex jid = 1; jid <= frame.parent; ++jid)
366 {
367
1/2
✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
 58 AJointsBeforeB.push_back(jid);
368 }
369 // descendants of the parent of frameInModelA come also before model B
370 // TODO(jcarpent): enhancement by taking into account the compactness of the joint ordering.
371
2/2
✓ Branch 1 taken 158 times.
✓ Branch 2 taken 10 times.
 168 for (JointIndex jid = frame.parent + 1; jid < modelA.joints.size(); ++jid)
372 {
373
1/2
✓ Branch 1 taken 158 times.
✗ Branch 2 not taken.
 158 if (hasAncestor(modelA, jid, frame.parent))
374 {
375
1/2
✓ Branch 1 taken 158 times.
✗ Branch 2 not taken.
 158 AJointsBeforeB.push_back(jid);
376 }
377 else
378 {
379 ✗ AJointsAfterB.push_back(jid);
380 }
381 }
382 // Copy modelA joints that should come before model B
383 10 for (std::vector<JointIndex>::const_iterator jid = AJointsBeforeB.begin();
384
2/2
✓ Branch 2 taken 216 times.
✓ Branch 3 taken 10 times.
 226 jid != AJointsBeforeB.end(); ++jid)
385 {
386
1/2
✓ Branch 1 taken 216 times.
✗ Branch 2 not taken.
 216 ArgsType args(modelA, geomModelA, 0, id, model, geomModel);
387
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);
388 }
389
390 // Copy modelB joints
391
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 details::appendUniverseToModel(
392
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 modelB, geomModelB, details::getFrameId(modelA, model, frame.name, frame.type), aMb, model,
393 geomModel);
394
2/2
✓ Branch 1 taken 51 times.
✓ Branch 2 taken 10 times.
 61 for (JointIndex jid = 1; jid < modelB.joints.size(); ++jid)
395 {
396
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);
397
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 ArgsType args(modelB, geomModelB, frame.parentJoint, pMi, model, geomModel);
398
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);
399 }
400
401 // Copy remaining joints of modelA
402 // Copy modelA joints that should come before model B
403 10 for (std::vector<JointIndex>::const_iterator jid = AJointsAfterB.begin();
404
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
 10 jid != AJointsAfterB.end(); ++jid)
405 {
406 ✗ ArgsType args(modelA, geomModelA, 0, id, model, geomModel);
407 ✗ AppendJointOfModelAlgo::run(modelA.joints[*jid], args);
408 }
409
410 // Retrieve and set the reference configurations
411 typedef typename Model::ConfigVectorMap ConfigVectorMap;
412
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 typename Model::ConfigVectorType neutral_config_vector(model.nq);
413 // Get neutral configuration
414
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 neutral(model, neutral_config_vector);
415
416 // Get all reference keys from ModelA
417 10 for (typename ConfigVectorMap::const_iterator config_it =
418 10 modelA.referenceConfigurations.begin();
419
2/2
✓ Branch 3 taken 16 times.
✓ Branch 4 taken 10 times.
 26 config_it != modelA.referenceConfigurations.end(); ++config_it)
420 {
421 16 const std::string & config_name = config_it->first;
422 16 const typename Model::ConfigVectorType & config_vectorA = config_it->second;
423
424
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 typename Model::ConfigVectorType config_vector(neutral_config_vector);
425
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)
426 {
427
1/2
✓ Branch 2 taken 418 times.
✗ Branch 3 not taken.
 418 const JointIndex joint_id = model.getJointId(modelA.names[joint_idA]);
428 418 const typename Model::JointModel & joint_model = model.joints[joint_id];
429 418 const typename Model::JointModel & joint_modelA = modelA.joints[joint_idA];
430
431
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) =
432
1/2
✓ Branch 1 taken 418 times.
✗ Branch 2 not taken.
 836 joint_modelA.jointConfigSelector(config_vectorA);
433 }
434
435
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));
436 }
437
438 // Get all reference keys from ModelB
439 10 for (typename ConfigVectorMap::const_iterator config_it =
440 10 modelB.referenceConfigurations.begin();
441
2/2
✓ Branch 3 taken 16 times.
✓ Branch 4 taken 10 times.
 26 config_it != modelB.referenceConfigurations.end(); ++config_it)
442 {
443 16 const std::string & config_name = config_it->first;
444 16 const typename Model::ConfigVectorType & config_vectorB = config_it->second;
445
446
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())
447 {
448 // not found
449
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));
450 }
451
452 16 typename Model::ConfigVectorType & config_vector =
453
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 model.referenceConfigurations.find(config_name)->second;
454
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)
455 {
456
1/2
✓ Branch 2 taken 96 times.
✗ Branch 3 not taken.
 96 const JointIndex joint_id = model.getJointId(modelB.names[joint_idB]);
457 96 const typename Model::JointModel & joint_model = model.joints[joint_id];
458 96 const typename Model::JointModel & joint_modelB = modelB.joints[joint_idB];
459
460
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) =
461
1/2
✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
 192 joint_modelB.jointConfigSelector(config_vectorB);
462 }
463 }
464
465 #ifdef PINOCCHIO_WITH_HPP_FCL
466 // Add collision pairs of geomModelA and geomModelB
467 20 geomModel.collisionPairs.reserve(
468 10 geomModelA.collisionPairs.size() + geomModelB.collisionPairs.size()
469
1/2
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
 10 + geomModelA.geometryObjects.size() * geomModelB.geometryObjects.size());
470
471
2/2
✓ Branch 1 taken 1041 times.
✓ Branch 2 taken 10 times.
 1051 for (std::size_t icp = 0; icp < geomModelA.collisionPairs.size(); ++icp)
472 {
473 1041 const CollisionPair & cp = geomModelA.collisionPairs[icp];
474
1/2
✓ Branch 2 taken 1041 times.
✗ Branch 3 not taken.
 1041 GeomIndex go1 = geomModel.getGeometryId(geomModelA.geometryObjects[cp.first].name);
475
1/2
✓ Branch 2 taken 1041 times.
✗ Branch 3 not taken.
 1041 GeomIndex go2 = geomModel.getGeometryId(geomModelA.geometryObjects[cp.second].name);
476
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));
477 }
478
479
2/2
✓ Branch 1 taken 56 times.
✓ Branch 2 taken 10 times.
 66 for (std::size_t icp = 0; icp < geomModelB.collisionPairs.size(); ++icp)
480 {
481 56 const CollisionPair & cp = geomModelB.collisionPairs[icp];
482
1/2
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
 56 GeomIndex go1 = geomModel.getGeometryId(geomModelB.geometryObjects[cp.first].name);
483
1/2
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
 56 GeomIndex go2 = geomModel.getGeometryId(geomModelB.geometryObjects[cp.second].name);
484
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));
485 }
486
487 // add the collision pairs between geomModelA and geomModelB.
488
2/2
✓ Branch 1 taken 88 times.
✓ Branch 2 taken 10 times.
 98 for (Index i = 0; i < geomModelA.geometryObjects.size(); ++i)
489 {
490
1/2
✓ Branch 2 taken 88 times.
✗ Branch 3 not taken.
 88 GeomIndex go1 = geomModel.getGeometryId(geomModelA.geometryObjects[i].name);
491
2/2
✓ Branch 1 taken 500 times.
✓ Branch 2 taken 88 times.
 588 for (Index j = 0; j < geomModelB.geometryObjects.size(); ++j)
492 {
493
1/2
✓ Branch 2 taken 500 times.
✗ Branch 3 not taken.
 500 GeomIndex go2 = geomModel.getGeometryId(geomModelB.geometryObjects[j].name);
494 500 if (
495
1/2
✓ Branch 2 taken 500 times.
✗ Branch 3 not taken.
 500 geomModel.geometryObjects[go1].parentJoint != geomModel.geometryObjects[go2].parentJoint)
496
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));
497 }
498 }
499 #endif
500 10 }
501
502 template<
503 typename Scalar,
504 int Options,
505 template<typename, int> class JointCollectionTpl,
506 typename ConfigVectorType>
507 16 void buildReducedModel(
508 const ModelTpl<Scalar, Options, JointCollectionTpl> & input_model,
509 std::vector<JointIndex> list_of_joints_to_lock,
510 const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
511 ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model)
512 {
513
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 16 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.
 16 PINOCCHIO_CHECK_ARGUMENT_SIZE(
514 reference_configuration.size(), input_model.nq,
515 "The configuration vector is not of right size");
516
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 16 PINOCCHIO_CHECK_INPUT_ARGUMENT(
517 list_of_joints_to_lock.size() <= (size_t)input_model.njoints,
518 "The number of joints to lock is greater than the total of joints in the reduced_model");
519
520 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
521 typedef typename Model::JointModel JointModel;
522 typedef typename Model::JointData JointData;
523 typedef typename Model::Frame Frame;
524 typedef typename Model::SE3 SE3;
525
526 // Sort indexes
527
1/2
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
 16 std::sort(list_of_joints_to_lock.begin(), list_of_joints_to_lock.end());
528
529 16 typename Model::FrameVector::const_iterator frame_it = input_model.frames.begin();
530
531 // Check that they are not two identical elements
532
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 16 times.
 32 for (size_t id = 1; id < list_of_joints_to_lock.size(); ++id)
533 {
534
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 16 PINOCCHIO_CHECK_INPUT_ARGUMENT(
535 list_of_joints_to_lock[id - 1] < list_of_joints_to_lock[id],
536 "The input list_of_joints_to_lock contains two identical indexes.");
537 }
538
539 // Reserve memory
540 16 const Eigen::DenseIndex njoints =
541 16 input_model.njoints - (Eigen::DenseIndex)list_of_joints_to_lock.size();
542
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 reduced_model.names.reserve((size_t)njoints);
543
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 reduced_model.joints.reserve((size_t)njoints);
544
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 reduced_model.jointPlacements.reserve((size_t)njoints);
545
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 reduced_model.parents.reserve((size_t)njoints);
546
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 reduced_model.inertias.reserve((size_t)njoints);
547
548
1/2
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
 16 reduced_model.names[0] = input_model.names[0];
549
1/2
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
 16 reduced_model.joints[0] = input_model.joints[0];
550
1/2
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
 16 reduced_model.jointPlacements[0] = input_model.jointPlacements[0];
551 16 reduced_model.parents[0] = input_model.parents[0];
552
1/2
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
 16 reduced_model.inertias[0] = input_model.inertias[0];
553
554
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 reduced_model.name = input_model.name;
555
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 reduced_model.gravity = input_model.gravity;
556
557 16 size_t current_index_to_lock = 0;
558
559
2/2
✓ Branch 0 taken 302 times.
✓ Branch 1 taken 16 times.
 318 for (JointIndex joint_id = 1; joint_id < (JointIndex)input_model.njoints; ++joint_id)
560 {
561
562 302 const JointIndex joint_id_to_lock = (current_index_to_lock < list_of_joints_to_lock.size())
563
2/2
✓ Branch 0 taken 154 times.
✓ Branch 1 taken 148 times.
 302 ? list_of_joints_to_lock[current_index_to_lock]
564 : 0;
565
566 302 const JointIndex input_parent_joint_index = input_model.parents[joint_id];
567 302 const std::string & joint_name = input_model.names[joint_id];
568 302 const JointModel & joint_input_model = input_model.joints[joint_id];
569
570 // retrieve the closest joint parent in the new kinematic tree
571 302 const std::string & parent_joint_name = input_model.names[input_parent_joint_index];
572
1/2
✓ Branch 1 taken 302 times.
✗ Branch 2 not taken.
 302 const bool exist_parent_joint = reduced_model.existJointName(parent_joint_name);
573
574
2/2
✓ Branch 0 taken 278 times.
✓ Branch 1 taken 24 times.
 302 const JointIndex reduced_parent_joint_index =
575 exist_parent_joint
576
3/4
✓ Branch 0 taken 278 times.
✓ Branch 1 taken 24 times.
✓ Branch 3 taken 278 times.
✗ Branch 4 not taken.
 326 ? reduced_model.getJointId(parent_joint_name)
577
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 : reduced_model.frames[reduced_model.getFrameId(parent_joint_name)].parentJoint;
578
579
3/4
✓ Branch 0 taken 278 times.
✓ Branch 1 taken 24 times.
✓ Branch 3 taken 278 times.
✗ Branch 4 not taken.
 302 const SE3 parent_frame_placement =
580 exist_parent_joint
581 ? SE3::Identity()
582
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
 24 : reduced_model.frames[reduced_model.getFrameId(parent_joint_name)].placement;
583
584
2/2
✓ Branch 0 taken 28 times.
✓ Branch 1 taken 274 times.
 302 const FrameIndex reduced_previous_frame_index =
585
3/4
✓ Branch 0 taken 278 times.
✓ Branch 1 taken 24 times.
✓ Branch 3 taken 24 times.
✗ Branch 4 not taken.
 302 exist_parent_joint ? 0 : reduced_model.getFrameId(parent_joint_name);
586
587
2/2
✓ Branch 0 taken 28 times.
✓ Branch 1 taken 274 times.
 302 if (joint_id == joint_id_to_lock)
588 {
589 // the joint should not be added to the Model but aggragated to its parent joint
590 // Add frames up to the joint to lock
591
1/2
✓ Branch 2 taken 361 times.
✗ Branch 3 not taken.
 361 while ((*frame_it).name != joint_name)
592 {
593 361 ++frame_it;
594 361 const Frame & input_frame = *frame_it;
595
2/2
✓ Branch 1 taken 28 times.
✓ Branch 2 taken 333 times.
 361 if (input_frame.name == joint_name)
596 28 break;
597 333 const std::string & support_joint_name = input_model.names[input_frame.parentJoint];
598
599
1/2
✓ Branch 2 taken 333 times.
✗ Branch 3 not taken.
 333 std::vector<JointIndex>::const_iterator support_joint_it = std::find(
600 333 list_of_joints_to_lock.begin(), list_of_joints_to_lock.end(), input_frame.parentJoint);
601
602
2/2
✓ Branch 2 taken 16 times.
✓ Branch 3 taken 317 times.
 333 if (support_joint_it != list_of_joints_to_lock.end())
603 {
604
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 16 if (
605 ✗ input_frame.type == JOINT && reduced_model.existFrame(input_frame.name)
606
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)
607 ✗ continue; // this means that the Joint is now fixed and has been replaced by a Frame.
608 // No need to add a new one.
609
610 // The joint has been removed and replaced by a Frame
611
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 const FrameIndex joint_frame_id = reduced_model.getFrameId(support_joint_name);
612 16 const Frame & joint_frame = reduced_model.frames[joint_frame_id];
613
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 Frame reduced_frame = input_frame;
614
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;
615 16 reduced_frame.parentJoint = joint_frame.parentJoint;
616 16 reduced_frame.parentFrame =
617
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.parentFrame].name);
618
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 reduced_model.addFrame(reduced_frame, false);
619 16 }
620 else
621 {
622
1/2
✓ Branch 1 taken 317 times.
✗ Branch 2 not taken.
 317 Frame reduced_frame = input_frame;
623 317 reduced_frame.parentJoint =
624
1/2
✓ Branch 2 taken 317 times.
✗ Branch 3 not taken.
 317 reduced_model.getJointId(input_model.names[input_frame.parentJoint]);
625 317 reduced_frame.parentFrame =
626
2/4
✓ Branch 2 taken 317 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 317 times.
✗ Branch 6 not taken.
 317 reduced_model.getFrameId(input_model.frames[input_frame.parentFrame].name);
627
1/2
✓ Branch 1 taken 317 times.
✗ Branch 2 not taken.
 317 reduced_model.addFrame(reduced_frame, false);
628 317 }
629 }
630
631 // Compute the new placement of the joint with respect to its parent joint in the new
632 // kinematic tree.
633
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 28 JointData joint_data = joint_input_model.createData();
634
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 28 joint_input_model.calc(joint_data, reference_configuration);
635
3/6
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28 times.
✗ Branch 8 not taken.
 56 const SE3 liMi =
636 28 parent_frame_placement * input_model.jointPlacements[joint_id] * joint_data.M();
637
638
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 28 Frame frame = Frame(
639 joint_name, reduced_parent_joint_index, reduced_previous_frame_index, liMi, FIXED_JOINT,
640 28 input_model.inertias[joint_id]);
641
642
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 28 FrameIndex frame_id = reduced_model.addFrame(frame);
643 28 reduced_model.frames[frame_id].parentFrame =
644 frame_id; // a bit weird, but this is a solution for missing parent frame
645
646 28 current_index_to_lock++;
647 28 }
648 else
649 {
650 JointIndex reduced_joint_id;
651
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 273 times.
 274 if (boost::get<JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>(&joint_input_model))
652 {
653 1 auto mimic_joint =
654
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 boost::get<JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>(joint_input_model);
655
656 JointIndex mimicked_id =
657
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 reduced_model.getJointId(input_model.names[mimic_joint.jmodel().id()]);
658
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 mimic_joint.setMimicIndexes(
659 1 mimicked_id, reduced_model.idx_qs[mimicked_id], reduced_model.idx_vs[mimicked_id],
660 1 reduced_model.idx_vExtendeds[mimicked_id]);
661
662
9/18
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
 7 reduced_joint_id = reduced_model.addJoint(
663 reduced_parent_joint_index, mimic_joint,
664 1 parent_frame_placement * input_model.jointPlacements[joint_id], joint_name,
665
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 mimic_joint.jointVelocitySelector(input_model.effortLimit),
666
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 mimic_joint.jointVelocitySelector(input_model.velocityLimit),
667
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 mimic_joint.jointConfigSelector(input_model.lowerPositionLimit),
668
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 mimic_joint.jointConfigSelector(input_model.upperPositionLimit),
669
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 mimic_joint.jointVelocitySelector(input_model.friction),
670
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 mimic_joint.jointVelocitySelector(input_model.damping));
671 1 }
672 else
673 {
674 // the joint should be added to the Model as it is
675
8/16
✓ Branch 1 taken 273 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 273 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 273 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 273 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 273 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 273 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 273 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 273 times.
✗ Branch 23 not taken.
 1911 reduced_joint_id = reduced_model.addJoint(
676 reduced_parent_joint_index, joint_input_model,
677 273 parent_frame_placement * input_model.jointPlacements[joint_id], joint_name,
678
1/2
✓ Branch 1 taken 273 times.
✗ Branch 2 not taken.
 546 joint_input_model.jointVelocitySelector(input_model.effortLimit),
679
1/2
✓ Branch 1 taken 273 times.
✗ Branch 2 not taken.
 546 joint_input_model.jointVelocitySelector(input_model.velocityLimit),
680
1/2
✓ Branch 1 taken 273 times.
✗ Branch 2 not taken.
 546 joint_input_model.jointConfigSelector(input_model.lowerPositionLimit),
681
1/2
✓ Branch 1 taken 273 times.
✗ Branch 2 not taken.
 546 joint_input_model.jointConfigSelector(input_model.upperPositionLimit),
682
1/2
✓ Branch 1 taken 273 times.
✗ Branch 2 not taken.
 546 joint_input_model.jointVelocitySelector(input_model.friction),
683
1/2
✓ Branch 1 taken 273 times.
✗ Branch 2 not taken.
 546 joint_input_model.jointVelocitySelector(input_model.damping));
684 }
685 // Append inertia
686
2/4
✓ Branch 1 taken 274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 274 times.
✗ Branch 5 not taken.
 548 reduced_model.appendBodyToJoint(
687 274 reduced_joint_id, input_model.inertias[joint_id], SE3::Identity());
688
689 // Copy other kinematics and dynamics properties
690 274 const typename Model::JointModel & jmodel_out = reduced_model.joints[reduced_joint_id];
691
2/4
✓ Branch 1 taken 274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 274 times.
✗ Branch 5 not taken.
 274 jmodel_out.jointVelocitySelector(reduced_model.rotorInertia) =
692
1/2
✓ Branch 1 taken 274 times.
✗ Branch 2 not taken.
 274 joint_input_model.jointVelocitySelector(input_model.rotorInertia);
693
2/4
✓ Branch 1 taken 274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 274 times.
✗ Branch 5 not taken.
 274 jmodel_out.jointVelocitySelector(reduced_model.rotorGearRatio) =
694
1/2
✓ Branch 1 taken 274 times.
✗ Branch 2 not taken.
 548 joint_input_model.jointVelocitySelector(input_model.rotorGearRatio);
695 }
696 }
697
698 // Retrieve and extend the reference configurations
699 typedef typename Model::ConfigVectorMap ConfigVectorMap;
700 16 for (typename ConfigVectorMap::const_iterator config_it =
701 16 input_model.referenceConfigurations.begin();
702
2/2
✓ Branch 3 taken 5 times.
✓ Branch 4 taken 16 times.
 21 config_it != input_model.referenceConfigurations.end(); ++config_it)
703 {
704 5 const std::string & config_name = config_it->first;
705 5 const typename Model::ConfigVectorType & input_config_vector = config_it->second;
706
707
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 typename Model::ConfigVectorType reduced_config_vector(reduced_model.nq);
708
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();
709 ++reduced_joint_id)
710 {
711 const JointIndex input_joint_id =
712
1/2
✓ Branch 2 taken 141 times.
✗ Branch 3 not taken.
 141 input_model.getJointId(reduced_model.names[reduced_joint_id]);
713 141 const JointModel & input_joint_model = input_model.joints[input_joint_id];
714 141 const JointModel & reduced_joint_model = reduced_model.joints[reduced_joint_id];
715
716
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) =
717
1/2
✓ Branch 1 taken 141 times.
✗ Branch 2 not taken.
 282 input_joint_model.jointConfigSelector(input_config_vector);
718 }
719
720
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(
721 std::make_pair(config_name, reduced_config_vector));
722 }
723
724 // Add all the missing frames
725
2/2
✓ Branch 3 taken 407 times.
✓ Branch 4 taken 16 times.
 423 for (; frame_it != input_model.frames.end(); ++frame_it)
726 {
727 407 const Frame & input_frame = *frame_it;
728 407 const std::string & support_joint_name = input_model.names[input_frame.parentJoint];
729
730
1/2
✓ Branch 2 taken 407 times.
✗ Branch 3 not taken.
 407 std::vector<JointIndex>::const_iterator support_joint_it = std::find(
731 407 list_of_joints_to_lock.begin(), list_of_joints_to_lock.end(), input_frame.parentJoint);
732
733
2/2
✓ Branch 2 taken 47 times.
✓ Branch 3 taken 360 times.
 407 if (support_joint_it != list_of_joints_to_lock.end())
734 {
735
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 35 times.
 47 if (
736
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 12 input_frame.type == JOINT && reduced_model.existFrame(input_frame.name)
737
5/6
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 35 times.
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 12 times.
✓ Branch 6 taken 35 times.
 59 && support_joint_name == input_frame.name)
738 12 continue; // this means that the Joint is now fixed and has been replaced by a Frame. No
739 // need to add a new one.
740
741 // The joint has been removed and replaced by a Frame
742
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 const FrameIndex joint_frame_id = reduced_model.getFrameId(support_joint_name);
743 35 const Frame & joint_frame = reduced_model.frames[joint_frame_id];
744
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 Frame reduced_frame = input_frame;
745
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;
746 35 reduced_frame.parentJoint = joint_frame.parentJoint;
747 35 reduced_frame.parentFrame =
748
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);
749
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 reduced_model.addFrame(reduced_frame, false);
750 35 }
751 else
752 {
753
1/2
✓ Branch 1 taken 360 times.
✗ Branch 2 not taken.
 360 Frame reduced_frame = input_frame;
754 360 reduced_frame.parentJoint =
755
1/2
✓ Branch 2 taken 360 times.
✗ Branch 3 not taken.
 360 reduced_model.getJointId(input_model.names[input_frame.parentJoint]);
756 360 reduced_frame.parentFrame =
757
2/4
✓ Branch 2 taken 360 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 360 times.
✗ Branch 6 not taken.
 360 reduced_model.getFrameId(input_model.frames[input_frame.parentFrame].name);
758
1/2
✓ Branch 1 taken 360 times.
✗ Branch 2 not taken.
 360 reduced_model.addFrame(reduced_frame, false);
759 360 }
760 }
761 16 }
762
763 template<
764 typename Scalar,
765 int Options,
766 template<typename, int> class JointCollectionTpl,
767 typename ConfigVectorType>
768 3 void buildReducedModel(
769 const ModelTpl<Scalar, Options, JointCollectionTpl> & input_model,
770 const GeometryModel & input_geom_model,
771 const std::vector<JointIndex> & list_of_joints_to_lock,
772 const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
773 ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model,
774 GeometryModel & reduced_geom_model)
775 {
776
777
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 const std::vector<GeometryModel> temp_input_geoms(1, input_geom_model);
778 3 std::vector<GeometryModel> temp_reduced_geom_models;
779
780
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 buildReducedModel(
781 input_model, temp_input_geoms, list_of_joints_to_lock, reference_configuration, reduced_model,
782 temp_reduced_geom_models);
783
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 reduced_geom_model = temp_reduced_geom_models.front();
784 3 }
785
786 template<
787 typename Scalar,
788 int Options,
789 template<typename, int> class JointCollectionTpl,
790 typename GeometryModelAllocator,
791 typename ConfigVectorType>
792 11 void buildReducedModel(
793 const ModelTpl<Scalar, Options, JointCollectionTpl> & input_model,
794 const std::vector<GeometryModel, GeometryModelAllocator> & list_of_geom_models,
795 const std::vector<JointIndex> & list_of_joints_to_lock,
796 const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
797 ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model,
798 std::vector<GeometryModel, GeometryModelAllocator> & list_of_reduced_geom_models)
799 {
800
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 11 buildReducedModel(input_model, list_of_joints_to_lock, reference_configuration, reduced_model);
801
802 // for all GeometryModels
803
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)
804 {
805 17 const GeometryModel & input_geom_model = list_of_geom_models[gmi];
806
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 17 GeometryModel reduced_geom_model;
807
808 // Add all the geometries
809 typedef GeometryModel::GeometryObject GeometryObject;
810 typedef GeometryModel::GeometryObjectVector GeometryObjectVector;
811 280 for (GeometryObjectVector::const_iterator it = input_geom_model.geometryObjects.begin();
812
2/2
✓ Branch 3 taken 172 times.
✓ Branch 4 taken 10 times.
 280 it != input_geom_model.geometryObjects.end(); ++it)
813 {
814 263 const GeometryModel::GeometryObject & geom = *it;
815
816 263 const JointIndex joint_id_in_input_model = geom.parentJoint;
817
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(
818 (joint_id_in_input_model < (JointIndex)input_model.njoints),
819 "Invalid joint parent index for the geometry with name " + geom.name);
820 263 const std::string & parent_joint_name = input_model.names[joint_id_in_input_model];
821
822 263 JointIndex reduced_joint_id = (JointIndex)-1;
823 typedef typename GeometryObject::SE3 SE3;
824
1/2
✓ Branch 1 taken 172 times.
✗ Branch 2 not taken.
 263 SE3 relative_placement = SE3::Identity();
825
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))
826 {
827
1/2
✓ Branch 1 taken 155 times.
✗ Branch 2 not taken.
 229 reduced_joint_id = reduced_model.getJointId(parent_joint_name);
828 }
829 else // The joint is now a frame
830 {
831
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 34 const FrameIndex reduced_frame_id = reduced_model.getFrameId(parent_joint_name);
832 34 reduced_joint_id = reduced_model.frames[reduced_frame_id].parentJoint;
833
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 34 relative_placement = reduced_model.frames[reduced_frame_id].placement;
834 }
835
836
1/2
✓ Branch 1 taken 172 times.
✗ Branch 2 not taken.
 263 GeometryObject reduced_geom(geom);
837 263 reduced_geom.parentJoint = reduced_joint_id;
838 263 reduced_geom.parentFrame =
839
1/2
✓ Branch 2 taken 172 times.
✗ Branch 3 not taken.
 263 reduced_model.getBodyId(input_model.frames[geom.parentFrame].name);
840
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;
841
1/2
✓ Branch 1 taken 172 times.
✗ Branch 2 not taken.
 263 reduced_geom_model.addGeometryObject(reduced_geom);
842 }
843
844 #ifdef PINOCCHIO_WITH_HPP_FCL
845 // Add all the collision pairs - the index of the geometry objects should have not changed
846
847 typedef GeometryModel::CollisionPairVector CollisionPairVector;
848 17 for (CollisionPairVector::const_iterator it = input_geom_model.collisionPairs.begin();
849
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 10 times.
 17 it != input_geom_model.collisionPairs.end(); ++it)
850 {
851 ✗ const CollisionPair & cp = *it;
852 ✗ reduced_geom_model.addCollisionPair(cp);
853 }
854 #endif
855
856
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 17 list_of_reduced_geom_models.push_back(reduced_geom_model);
857 }
858 11 }
859
860 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
861 80 void transformJointIntoMimic(
862 const ModelTpl<Scalar, Options, JointCollectionTpl> & input_model,
863 const JointIndex & index_mimicked,
864 const JointIndex & index_mimicking,
865 const Scalar & scaling,
866 const Scalar & offset,
867 ModelTpl<Scalar, Options, JointCollectionTpl> & output_model)
868 {
869
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 80 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 80 PINOCCHIO_CHECK_INPUT_ARGUMENT(
870 index_mimicked <= (size_t)input_model.njoints,
871 "index_mimicked is greater than the total of joints");
872
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 80 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 80 PINOCCHIO_CHECK_INPUT_ARGUMENT(
873 index_mimicking <= (size_t)input_model.njoints,
874 "index_mimicking is greater than the total of joints");
875
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 80 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 80 PINOCCHIO_CHECK_INPUT_ARGUMENT(
876 index_mimicked < index_mimicking, "index_mimicking is greater than index_mimicked");
877
878 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
879 typedef typename Model::JointModel JointModel;
880
881 80 output_model = input_model;
882
883
2/4
✓ Branch 2 taken 80 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 80 times.
✗ Branch 7 not taken.
 80 output_model.joints[index_mimicking] = JointModelMimic(
884 80 input_model.joints[index_mimicking], output_model.joints[index_mimicked], scaling, offset);
885
886 80 int old_nq = input_model.joints[index_mimicking].nq();
887 80 int old_nv = input_model.joints[index_mimicking].nv();
888 80 output_model.nq = input_model.nq - old_nq;
889 80 output_model.nv = input_model.nv - old_nv;
890 80 int nq = output_model.nq;
891 80 int nv = output_model.nv;
892
893 // Resize limits
894 80 output_model.effortLimit.resize(nv);
895 80 output_model.velocityLimit.resize(nv);
896 80 output_model.lowerPositionLimit.resize(nq);
897 80 output_model.upperPositionLimit.resize(nq);
898 80 output_model.armature.resize(nv);
899 80 output_model.rotorInertia.resize(nv);
900 80 output_model.rotorGearRatio.resize(nv);
901 80 output_model.friction.resize(nv);
902 80 output_model.damping.resize(nv);
903
904 // Move indexes and limits
905
2/2
✓ Branch 0 taken 817 times.
✓ Branch 1 taken 80 times.
 897 for (JointIndex joint_id = 1; joint_id < (JointIndex)index_mimicking; ++joint_id)
906 {
907 817 const JointModel & jmodel_input = input_model.joints[joint_id];
908 817 const JointModel & jmodel_output = output_model.joints[joint_id];
909
2/4
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 817 times.
✗ Branch 5 not taken.
 817 jmodel_output.jointVelocitySelector(output_model.effortLimit) =
910 817 jmodel_input.jointVelocitySelector(input_model.effortLimit);
911
2/4
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 817 times.
✗ Branch 5 not taken.
 817 jmodel_output.jointVelocitySelector(output_model.velocityLimit) =
912 817 jmodel_input.jointVelocitySelector(input_model.velocityLimit);
913
914
2/4
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 817 times.
✗ Branch 5 not taken.
 817 jmodel_output.jointConfigSelector(output_model.lowerPositionLimit) =
915 817 jmodel_input.jointConfigSelector(input_model.lowerPositionLimit);
916
2/4
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 817 times.
✗ Branch 5 not taken.
 817 jmodel_output.jointConfigSelector(output_model.upperPositionLimit) =
917 817 jmodel_input.jointConfigSelector(input_model.upperPositionLimit);
918
919
2/4
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 817 times.
✗ Branch 5 not taken.
 817 jmodel_output.jointVelocitySelector(output_model.armature) =
920 817 jmodel_input.jointVelocitySelector(input_model.armature);
921
2/4
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 817 times.
✗ Branch 5 not taken.
 817 jmodel_output.jointVelocitySelector(output_model.rotorInertia) =
922 817 jmodel_input.jointVelocitySelector(input_model.rotorInertia);
923
2/4
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 817 times.
✗ Branch 5 not taken.
 817 jmodel_output.jointVelocitySelector(output_model.rotorGearRatio) =
924 817 jmodel_input.jointVelocitySelector(input_model.rotorGearRatio);
925
2/4
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 817 times.
✗ Branch 5 not taken.
 817 jmodel_output.jointVelocitySelector(output_model.friction) =
926 817 jmodel_input.jointVelocitySelector(input_model.friction);
927
2/4
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 817 times.
✗ Branch 5 not taken.
 817 jmodel_output.jointVelocitySelector(output_model.damping) =
928 1634 jmodel_input.jointVelocitySelector(input_model.damping);
929 }
930
931 // Move indexes and limits
932 80 int idx_q = output_model.idx_qs[index_mimicking];
933 80 int idx_v = output_model.idx_vs[index_mimicking];
934
2/2
✓ Branch 0 taken 1194 times.
✓ Branch 1 taken 80 times.
 1274 for (JointIndex joint_id = index_mimicking; joint_id < (JointIndex)input_model.njoints;
935 ++joint_id)
936 {
937 1194 const JointModel & jmodel_input = input_model.joints[joint_id];
938 1194 JointModel & jmodel_output = output_model.joints[joint_id];
939 1194 jmodel_output.setIndexes(jmodel_input.id(), idx_q, idx_v, jmodel_input.idx_vExtended());
940 1194 if (
941 1194 boost::get<JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>(&jmodel_output)
942
6/6
✓ Branch 0 taken 90 times.
✓ Branch 1 taken 1104 times.
✓ Branch 2 taken 10 times.
✓ Branch 3 taken 80 times.
✓ Branch 4 taken 10 times.
✓ Branch 5 taken 1184 times.
 1194 && joint_id != index_mimicking)
943 {
944 auto & jmimic =
945 10 boost::get<JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>(jmodel_output);
946 10 const JointIndex mimicked_id = jmimic.jmodel().id();
947 20 jmimic.setMimicIndexes(
948 10 mimicked_id, output_model.idx_qs[mimicked_id], output_model.idx_vs[mimicked_id],
949 jmodel_input.idx_vExtended());
950 }
951 1194 output_model.idx_qs[joint_id] = jmodel_output.idx_q();
952 1194 output_model.nqs[joint_id] = jmodel_output.nq();
953 1194 output_model.idx_vs[joint_id] = jmodel_output.idx_v();
954 1194 output_model.nvs[joint_id] = jmodel_output.nv();
955
956 1194 idx_q += jmodel_output.nq();
957 1194 idx_v += jmodel_output.nv();
958
2/2
✓ Branch 0 taken 1114 times.
✓ Branch 1 taken 80 times.
 1194 if (joint_id != index_mimicking)
959 {
960
2/4
✓ Branch 1 taken 1114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1114 times.
✗ Branch 5 not taken.
 1114 jmodel_output.jointVelocitySelector(output_model.effortLimit) =
961 1114 jmodel_input.jointVelocitySelector(input_model.effortLimit);
962
2/4
✓ Branch 1 taken 1114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1114 times.
✗ Branch 5 not taken.
 1114 jmodel_output.jointVelocitySelector(output_model.velocityLimit) =
963 1114 jmodel_input.jointVelocitySelector(input_model.velocityLimit);
964
965
2/4
✓ Branch 1 taken 1114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1114 times.
✗ Branch 5 not taken.
 1114 jmodel_output.jointConfigSelector(output_model.lowerPositionLimit) =
966 1114 jmodel_input.jointConfigSelector(input_model.lowerPositionLimit);
967
2/4
✓ Branch 1 taken 1114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1114 times.
✗ Branch 5 not taken.
 1114 jmodel_output.jointConfigSelector(output_model.upperPositionLimit) =
968 1114 jmodel_input.jointConfigSelector(input_model.upperPositionLimit);
969
970
2/4
✓ Branch 1 taken 1114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1114 times.
✗ Branch 5 not taken.
 1114 jmodel_output.jointVelocitySelector(output_model.armature) =
971 1114 jmodel_input.jointVelocitySelector(input_model.armature);
972
2/4
✓ Branch 1 taken 1114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1114 times.
✗ Branch 5 not taken.
 1114 jmodel_output.jointVelocitySelector(output_model.rotorInertia) =
973 1114 jmodel_input.jointVelocitySelector(input_model.rotorInertia);
974
2/4
✓ Branch 1 taken 1114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1114 times.
✗ Branch 5 not taken.
 1114 jmodel_output.jointVelocitySelector(output_model.rotorGearRatio) =
975 1114 jmodel_input.jointVelocitySelector(input_model.rotorGearRatio);
976
2/4
✓ Branch 1 taken 1114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1114 times.
✗ Branch 5 not taken.
 1114 jmodel_output.jointVelocitySelector(output_model.friction) =
977 1114 jmodel_input.jointVelocitySelector(input_model.friction);
978
2/4
✓ Branch 1 taken 1114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1114 times.
✗ Branch 5 not taken.
 1114 jmodel_output.jointVelocitySelector(output_model.damping) =
979 2228 jmodel_input.jointVelocitySelector(input_model.damping);
980 }
981 }
982
983 // Modify Model::mimic_joint_supports
984 80 const auto & subtree = input_model.subtrees[index_mimicking];
985
2/2
✓ Branch 1 taken 364 times.
✓ Branch 2 taken 80 times.
 444 for (size_t i = 0; i < subtree.size(); ++i)
986 {
987 364 auto & i_support = output_model.mimic_joint_supports[subtree[i]];
988 364 details::insertSort(index_mimicking, i_support);
989 }
990
991 80 details::insertSort(index_mimicking, output_model.mimicking_joints);
992 80 details::insertSort(index_mimicked, output_model.mimicked_joints);
993 80 }
994
995 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
996 57 void buildMimicModel(
997 const ModelTpl<Scalar, Options, JointCollectionTpl> & input_model,
998 const std::vector<JointIndex> & index_mimicked,
999 const std::vector<JointIndex> & index_mimicking,
1000 const std::vector<Scalar> & scaling,
1001 const std::vector<Scalar> & offset,
1002 ModelTpl<Scalar, Options, JointCollectionTpl> & output_model)
1003 {
1004 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
1005
1006
1/2
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
 57 Model model_temp;
1007
1/2
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
 57 model_temp = input_model;
1008
2/2
✓ Branch 1 taken 77 times.
✓ Branch 2 taken 57 times.
 134 for (size_t i = 0; i < index_mimicked.size(); i++)
1009 {
1010
1/2
✓ Branch 3 taken 77 times.
✗ Branch 4 not taken.
 77 transformJointIntoMimic(
1011 77 model_temp, index_mimicked[i], index_mimicking[i], scaling[i], offset[i], output_model);
1012
1/2
✓ Branch 1 taken 77 times.
✗ Branch 2 not taken.
 77 model_temp = output_model;
1013 }
1014 57 }
1015
1016 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
1017 29 JointIndex findCommonAncestor(
1018 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1019 JointIndex joint1_id,
1020 JointIndex joint2_id,
1021 size_t & index_ancestor_in_support1,
1022 size_t & index_ancestor_in_support2)
1023 {
1024
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 29 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 29 PINOCCHIO_CHECK_INPUT_ARGUMENT(
1025 joint1_id < (JointIndex)model.njoints, "joint1_id is not valid.");
1026
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 29 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 29 PINOCCHIO_CHECK_INPUT_ARGUMENT(
1027 joint2_id < (JointIndex)model.njoints, "joint2_id is not valid.");
1028
1029 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
1030 typedef typename Model::IndexVector IndexVector;
1031
1032
4/4
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 17 times.
 29 if (joint1_id == 0 || joint2_id == 0)
1033 {
1034 12 index_ancestor_in_support1 = index_ancestor_in_support2 = 0;
1035 12 return 0;
1036 }
1037
1038 17 const IndexVector & support1 = model.supports[joint1_id];
1039 17 const IndexVector & support2 = model.supports[joint2_id];
1040
1041 17 index_ancestor_in_support1 = support1.size() - 1;
1042 17 index_ancestor_in_support2 = support2.size() - 1;
1043
2/2
✓ Branch 0 taken 104 times.
✓ Branch 1 taken 17 times.
 121 while (joint1_id != joint2_id)
1044 {
1045
2/2
✓ Branch 0 taken 56 times.
✓ Branch 1 taken 48 times.
 104 if (joint1_id > joint2_id)
1046 56 joint1_id = support1[--index_ancestor_in_support1];
1047 else
1048 48 joint2_id = support2[--index_ancestor_in_support2];
1049 }
1050
1051 17 return joint1_id;
1052 }
1053
1054 } // namespace pinocchio
1055
1056 #endif // ifndef __pinocchio_algorithm_model_hxx__
1057