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