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