Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/model.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 24 | 30 | 80.0% |
Branches: | 60 | 148 | 40.5% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2020 CNRS INRIA | ||
3 | // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. | ||
4 | // | ||
5 | |||
6 | #ifndef __pinocchio_multibody_model_hpp__ | ||
7 | #define __pinocchio_multibody_model_hpp__ | ||
8 | |||
9 | #include "pinocchio/spatial/fwd.hpp" | ||
10 | #include "pinocchio/spatial/se3.hpp" | ||
11 | #include "pinocchio/spatial/force.hpp" | ||
12 | #include "pinocchio/spatial/motion.hpp" | ||
13 | #include "pinocchio/spatial/inertia.hpp" | ||
14 | |||
15 | #include "pinocchio/multibody/fwd.hpp" | ||
16 | #include "pinocchio/multibody/frame.hpp" | ||
17 | #include "pinocchio/multibody/joint/joint-generic.hpp" | ||
18 | |||
19 | #include "pinocchio/container/aligned-vector.hpp" | ||
20 | |||
21 | #include "pinocchio/serialization/serializable.hpp" | ||
22 | |||
23 | #include <map> | ||
24 | #include <iterator> | ||
25 | |||
26 | namespace pinocchio | ||
27 | { | ||
28 | template< | ||
29 | typename NewScalar, | ||
30 | typename Scalar, | ||
31 | int Options, | ||
32 | template<typename, int> | ||
33 | class JointCollectionTpl> | ||
34 | struct CastType<NewScalar, ModelTpl<Scalar, Options, JointCollectionTpl>> | ||
35 | { | ||
36 | typedef ModelTpl<NewScalar, Options, JointCollectionTpl> type; | ||
37 | }; | ||
38 | |||
39 | template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl> | ||
40 | struct traits<ModelTpl<_Scalar, _Options, JointCollectionTpl>> | ||
41 | { | ||
42 | typedef _Scalar Scalar; | ||
43 | }; | ||
44 | |||
45 | template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl> | ||
46 | struct ModelTpl | ||
47 | : serialization::Serializable<ModelTpl<_Scalar, _Options, JointCollectionTpl>> | ||
48 | , NumericalBase<ModelTpl<_Scalar, _Options, JointCollectionTpl>> | ||
49 | { | ||
50 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
51 | |||
52 | typedef _Scalar Scalar; | ||
53 | enum | ||
54 | { | ||
55 | Options = _Options | ||
56 | }; | ||
57 | |||
58 | typedef JointCollectionTpl<Scalar, Options> JointCollection; | ||
59 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
60 | |||
61 | typedef SE3Tpl<Scalar, Options> SE3; | ||
62 | typedef MotionTpl<Scalar, Options> Motion; | ||
63 | typedef ForceTpl<Scalar, Options> Force; | ||
64 | typedef InertiaTpl<Scalar, Options> Inertia; | ||
65 | typedef FrameTpl<Scalar, Options> Frame; | ||
66 | |||
67 | typedef pinocchio::Index Index; | ||
68 | typedef pinocchio::JointIndex JointIndex; | ||
69 | typedef pinocchio::GeomIndex GeomIndex; | ||
70 | typedef pinocchio::FrameIndex FrameIndex; | ||
71 | typedef std::vector<Index> IndexVector; | ||
72 | |||
73 | typedef JointModelTpl<Scalar, Options, JointCollectionTpl> JointModel; | ||
74 | typedef JointDataTpl<Scalar, Options, JointCollectionTpl> JointData; | ||
75 | |||
76 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector; | ||
77 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector; | ||
78 | |||
79 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(Frame) FrameVector; | ||
80 | |||
81 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs; | ||
82 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
83 | |||
84 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) InertiaVector; | ||
85 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(SE3) SE3Vector; | ||
86 | |||
87 | /// \brief Dense vectorized version of a joint configuration vector. | ||
88 | typedef VectorXs ConfigVectorType; | ||
89 | |||
90 | /// \brief Map between a string (key) and a configuration vector | ||
91 | typedef std::map<std::string, ConfigVectorType> ConfigVectorMap; | ||
92 | |||
93 | /// \brief Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, | ||
94 | /// etc). | ||
95 | /// It also handles the notion of co-tangent vector (e.g. torque, etc). | ||
96 | typedef VectorXs TangentVectorType; | ||
97 | |||
98 | /// \brief Dimension of the configuration vector representation. | ||
99 | int nq; | ||
100 | |||
101 | /// \brief Dimension of the velocity vector space. | ||
102 | int nv; | ||
103 | |||
104 | /// \brief Number of joints. | ||
105 | int njoints; | ||
106 | |||
107 | /// \brief Number of bodies. | ||
108 | int nbodies; | ||
109 | |||
110 | /// \brief Number of operational frames. | ||
111 | int nframes; | ||
112 | |||
113 | /// \brief Vector of spatial inertias supported by each joint. | ||
114 | InertiaVector inertias; | ||
115 | |||
116 | /// \brief Vector of joint placements: placement of a joint *i* wrt its parent joint frame. | ||
117 | SE3Vector jointPlacements; | ||
118 | |||
119 | /// \brief Vector of joint models. | ||
120 | JointModelVector joints; | ||
121 | |||
122 | /// \brief Vector of starting index of the *i*th joint in the configuration space. | ||
123 | std::vector<int> idx_qs; | ||
124 | |||
125 | /// \brief Vector of dimension of the joint configuration subspace. | ||
126 | std::vector<int> nqs; | ||
127 | |||
128 | /// \brief Starting index of the *i*th joint in the tangent configuration space. | ||
129 | std::vector<int> idx_vs; | ||
130 | |||
131 | /// \brief Dimension of the *i*th joint tangent subspace. | ||
132 | std::vector<int> nvs; | ||
133 | |||
134 | /// \brief Vector of parent joint indexes. The parent of joint *i*, denoted *li*, corresponds to | ||
135 | /// li==parents[i]. | ||
136 | std::vector<JointIndex> parents; | ||
137 | |||
138 | /// \brief Vector of children index. Chidren of the *i*th joint, denoted *mu(i)* corresponds to | ||
139 | /// the set (i==parents[k] for k in mu(i)). | ||
140 | std::vector<IndexVector> children; | ||
141 | |||
142 | /// \brief Name of the joints. | ||
143 | std::vector<std::string> names; | ||
144 | |||
145 | /// \brief Map of reference configurations, indexed by user given names. | ||
146 | ConfigVectorMap referenceConfigurations; | ||
147 | |||
148 | /// \brief Vector of armature values expressed at the joint level | ||
149 | /// This vector may contain the contribution of rotor inertia effects for instance. | ||
150 | VectorXs armature; | ||
151 | |||
152 | /// \brief Vector of rotor inertia parameters | ||
153 | TangentVectorType rotorInertia; | ||
154 | |||
155 | /// \brief Vector of rotor gear ratio parameters | ||
156 | TangentVectorType rotorGearRatio; | ||
157 | |||
158 | /// \brief Vector of joint friction parameters | ||
159 | TangentVectorType friction; | ||
160 | |||
161 | /// \brief Vector of joint damping parameters | ||
162 | TangentVectorType damping; | ||
163 | |||
164 | /// \brief Vector of maximal joint torques | ||
165 | TangentVectorType effortLimit; | ||
166 | |||
167 | /// \brief Vector of maximal joint velocities | ||
168 | TangentVectorType velocityLimit; | ||
169 | |||
170 | /// \brief Lower joint configuration limit | ||
171 | ConfigVectorType lowerPositionLimit; | ||
172 | |||
173 | /// \brief Upper joint configuration limit | ||
174 | ConfigVectorType upperPositionLimit; | ||
175 | |||
176 | /// \brief Vector of operational frames registered on the model. | ||
177 | FrameVector frames; | ||
178 | |||
179 | /// \brief Vector of joint supports. | ||
180 | /// supports[j] corresponds to the vector of indices of the joints located on the path between | ||
181 | /// joint *j* and "universe". | ||
182 | /// The first element of supports[j] is "universe", the last one is the index of joint *j* | ||
183 | /// itself. | ||
184 | std::vector<IndexVector> supports; | ||
185 | |||
186 | /// \brief Vector of joint subtrees. | ||
187 | /// subtree[j] corresponds to the subtree supported by the joint *j*. | ||
188 | /// The first element of subtree[j] is the index of the joint *j* itself. | ||
189 | std::vector<IndexVector> subtrees; | ||
190 | |||
191 | /// \brief Spatial gravity of the model. | ||
192 | Motion gravity; | ||
193 | |||
194 | /// \brief Default 3D gravity vector (=(0,0,-9.81)). | ||
195 | static const Vector3 gravity981; | ||
196 | |||
197 | /// \brief Model name. | ||
198 | std::string name; | ||
199 | |||
200 | /// \brief Default constructor. Builds an empty model with no joints. | ||
201 | 594 | ModelTpl() | |
202 | 594 | : nq(0) | |
203 | 594 | , nv(0) | |
204 | 594 | , njoints(1) | |
205 | 594 | , nbodies(1) | |
206 | 594 | , nframes(0) | |
207 |
2/4✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 594 times.
✗ Branch 6 not taken.
|
594 | , inertias(1, Inertia::Zero()) |
208 |
2/4✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 594 times.
✗ Branch 6 not taken.
|
594 | , jointPlacements(1, SE3::Identity()) |
209 |
1/2✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
|
594 | , joints(1) |
210 |
1/2✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
|
594 | , idx_qs(1, 0) |
211 |
1/2✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
|
594 | , nqs(1, 0) |
212 |
1/2✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
|
594 | , idx_vs(1, 0) |
213 |
1/2✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
|
594 | , nvs(1, 0) |
214 |
1/2✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
|
594 | , parents(1, 0) |
215 |
1/2✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
|
594 | , children(1) |
216 |
1/2✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
|
594 | , names(1) |
217 |
2/4✓ Branch 3 taken 594 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 594 times.
✗ Branch 7 not taken.
|
594 | , supports(1, IndexVector(1, 0)) |
218 |
1/2✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
|
594 | , subtrees(1) |
219 |
12/24✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 594 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 594 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 594 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 594 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 594 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 594 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 594 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 594 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 594 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 594 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 594 times.
✗ Branch 36 not taken.
|
1188 | , gravity(gravity981, Vector3::Zero()) |
220 | { | ||
221 |
1/2✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
|
594 | names[0] = "universe"; // Should be "universe joint (trivial)" |
222 | // FIXME Should the universe joint be a FIXED_JOINT even if it is | ||
223 | // in the list of joints ? See comment in definition of | ||
224 | // Model::addJointFrame and Model::addBodyFrame | ||
225 |
5/10✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 594 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 594 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 594 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 594 times.
✗ Branch 15 not taken.
|
594 | addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT)); |
226 | 594 | } | |
227 | |||
228 | /// | ||
229 | /// \brief Copy constructor by casting | ||
230 | /// | ||
231 | /// \param[in] other model to copy to *this | ||
232 | /// | ||
233 | template<typename S2, int O2> | ||
234 | ✗ | explicit ModelTpl(const ModelTpl<S2, O2> & other) | |
235 | { | ||
236 | ✗ | *this = other.template cast<Scalar>(); | |
237 | } | ||
238 | |||
239 | /// \returns A new copy of *this with the Scalar type casted to NewScalar. | ||
240 | template<typename NewScalar> | ||
241 | typename CastType<NewScalar, ModelTpl>::type cast() const; | ||
242 | |||
243 | /// | ||
244 | /// \brief Equality comparison operator. | ||
245 | /// | ||
246 | /// \returns true if *this is equal to other. | ||
247 | /// | ||
248 | bool operator==(const ModelTpl & other) const; | ||
249 | |||
250 | /// | ||
251 | /// \returns true if *this is NOT equal to other. | ||
252 | /// | ||
253 | ✗ | bool operator!=(const ModelTpl & other) const | |
254 | { | ||
255 | ✗ | return !(*this == other); | |
256 | } | ||
257 | |||
258 | /// | ||
259 | /// \brief Add a joint to the kinematic tree with infinite bounds. | ||
260 | /// | ||
261 | /// \remarks This method does not add a Frame of same name to the vector of frames. | ||
262 | /// Use Model::addJointFrame. | ||
263 | /// \remarks The inertia supported by the joint is set to Zero. | ||
264 | /// \remark Joints need to be added to the tree in a depth-first order. | ||
265 | /// | ||
266 | /// \tparam JointModelDerived The type of the joint model. | ||
267 | /// | ||
268 | /// \param[in] parent Index of the parent joint. | ||
269 | /// \param[in] joint_model The joint model. | ||
270 | /// \param[in] joint_placement Placement of the joint inside its parent joint. | ||
271 | /// \param[in] joint_name Name of the joint. If empty, the name is random. | ||
272 | /// | ||
273 | /// \return The index of the new joint. | ||
274 | /// | ||
275 | /// \sa Model::appendBodyToJoint | ||
276 | /// | ||
277 | JointIndex addJoint( | ||
278 | const JointIndex parent, | ||
279 | const JointModel & joint_model, | ||
280 | const SE3 & joint_placement, | ||
281 | const std::string & joint_name); | ||
282 | |||
283 | /// | ||
284 | /// \copydoc ModelTpl::addJoint(const JointIndex,const JointModel &,const SE3 &,const | ||
285 | /// std::string &) | ||
286 | /// | ||
287 | /// \param[in] max_effort Maximal joint torque. | ||
288 | /// \param[in] max_velocity Maximal joint velocity. | ||
289 | /// \param[in] min_config Lower joint configuration. | ||
290 | /// \param[in] max_config Upper joint configuration. | ||
291 | /// | ||
292 | JointIndex addJoint( | ||
293 | const JointIndex parent, | ||
294 | const JointModel & joint_model, | ||
295 | const SE3 & joint_placement, | ||
296 | const std::string & joint_name, | ||
297 | const VectorXs & max_effort, | ||
298 | const VectorXs & max_velocity, | ||
299 | const VectorXs & min_config, | ||
300 | const VectorXs & max_config); | ||
301 | |||
302 | /// | ||
303 | /// \copydoc ModelTpl::addJoint(const JointIndex,const JointModel &,const SE3 &,const | ||
304 | /// std::string &,const VectorXs &,const VectorXs &,const VectorXs &,const VectorXs &) | ||
305 | /// | ||
306 | /// \param[in] friction Joint friction parameters. | ||
307 | /// \param[in] damping Joint damping parameters. | ||
308 | /// | ||
309 | JointIndex addJoint( | ||
310 | const JointIndex parent, | ||
311 | const JointModel & joint_model, | ||
312 | const SE3 & joint_placement, | ||
313 | const std::string & joint_name, | ||
314 | const VectorXs & max_effort, | ||
315 | const VectorXs & max_velocity, | ||
316 | const VectorXs & min_config, | ||
317 | const VectorXs & max_config, | ||
318 | const VectorXs & friction, | ||
319 | const VectorXs & damping); | ||
320 | |||
321 | /// | ||
322 | /// \brief Add a joint to the frame tree. | ||
323 | /// | ||
324 | /// \param[in] jointIndex Index of the joint. | ||
325 | /// \param[in] frameIndex Index of the parent frame. If negative, | ||
326 | /// the parent frame is the frame of the parent joint. | ||
327 | /// | ||
328 | /// \return The index of the new frame | ||
329 | /// | ||
330 | FrameIndex addJointFrame(const JointIndex & joint_index, int previous_frame_index = -1); | ||
331 | |||
332 | /// | ||
333 | /// \brief Append a body to a given joint of the kinematic tree. | ||
334 | /// | ||
335 | /// \param[in] joint_index Index of the supporting joint. | ||
336 | /// \param[in] Y Spatial inertia of the body. | ||
337 | /// \param[in] body_placement The relative placement of the body regarding to the parent joint. | ||
338 | /// Set default to the Identity placement. | ||
339 | /// | ||
340 | /// \sa Model::addJoint | ||
341 | /// | ||
342 | void appendBodyToJoint( | ||
343 | const JointIndex joint_index, | ||
344 | const Inertia & Y, | ||
345 | const SE3 & body_placement = SE3::Identity()); | ||
346 | |||
347 | /// | ||
348 | /// \brief Add a body to the frame tree. | ||
349 | /// | ||
350 | /// \param[in] body_name Name of the body. | ||
351 | /// \param[in] parentJoint Index of the parent joint. | ||
352 | /// \param[in] body_placement The relative placement of the body regarding to the parent joint. | ||
353 | /// Set default to the Identity placement. \param[in] parentFrame Index of the parent frame. If | ||
354 | /// negative, | ||
355 | /// the parent frame is the frame of the parent joint. | ||
356 | /// | ||
357 | /// \return The index of the new frame | ||
358 | /// | ||
359 | FrameIndex addBodyFrame( | ||
360 | const std::string & body_name, | ||
361 | const JointIndex & parentJoint, | ||
362 | const SE3 & body_placement = SE3::Identity(), | ||
363 | int parentFrame = -1); | ||
364 | |||
365 | /// | ||
366 | /// \brief Return the index of a body given by its name. | ||
367 | /// | ||
368 | /// \warning If no body is found, return the number of elements at time T. | ||
369 | /// This can lead to errors if the model is expanded after this method is called | ||
370 | /// (for example to get the id of a parent body) | ||
371 | /// | ||
372 | /// \param[in] name Name of the body. | ||
373 | /// | ||
374 | /// \return Index of the body. | ||
375 | /// | ||
376 | FrameIndex getBodyId(const std::string & name) const; | ||
377 | |||
378 | /// | ||
379 | /// \brief Check if a body given by its name exists. | ||
380 | /// | ||
381 | /// \param[in] name Name of the body. | ||
382 | /// | ||
383 | /// \return True if the body exists in the kinematic tree. | ||
384 | /// | ||
385 | bool existBodyName(const std::string & name) const; | ||
386 | |||
387 | /// | ||
388 | /// \brief Return the index of a joint given by its name. | ||
389 | /// | ||
390 | /// \warning If no joint is found, return the number of elements at time T. | ||
391 | /// This can lead to errors if the model is expanded after this method is called | ||
392 | /// (for example to get the id of a parent joint) | ||
393 | /// \param[in] name Name of the joint. | ||
394 | /// | ||
395 | /// \return Index of the joint. | ||
396 | /// | ||
397 | JointIndex getJointId(const std::string & name) const; | ||
398 | |||
399 | /// | ||
400 | /// \brief Check if a joint given by its name exists. | ||
401 | /// | ||
402 | /// \param[in] name Name of the joint. | ||
403 | /// | ||
404 | /// \return True if the joint exists in the kinematic tree. | ||
405 | /// | ||
406 | bool existJointName(const std::string & name) const; | ||
407 | |||
408 | /// | ||
409 | /// \brief Returns the index of a frame given by its name. | ||
410 | /// \sa Model::existFrame to check if the frame exists or not. | ||
411 | /// | ||
412 | /// \warning If no frame is found, returns the size of the vector of Model::frames. | ||
413 | /// This can lead to errors if the model is expanded after this method is called | ||
414 | /// (for example to get the id of a parent frame). | ||
415 | /// | ||
416 | /// \param[in] name Name of the frame. | ||
417 | /// \param[in] type Type of the frame. | ||
418 | /// | ||
419 | /// \return Index of the frame. | ||
420 | /// | ||
421 | FrameIndex getFrameId( | ||
422 | const std::string & name, | ||
423 |
23/37✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 749 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 14 times.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 24 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 274 times.
✓ Branch 16 taken 24 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 23 times.
✓ Branch 19 taken 1 times.
✓ Branch 20 taken 274 times.
✓ Branch 21 taken 23 times.
✓ Branch 22 taken 1 times.
✓ Branch 23 taken 23 times.
✗ Branch 24 not taken.
✓ Branch 25 taken 28 times.
✓ Branch 26 taken 270 times.
✓ Branch 28 taken 16 times.
✗ Branch 29 not taken.
✓ Branch 33 taken 16 times.
✗ Branch 34 not taken.
✓ Branch 37 taken 321 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 35 times.
✗ Branch 41 not taken.
✓ Branch 45 taken 35 times.
✗ Branch 46 not taken.
✓ Branch 49 taken 358 times.
✗ Branch 50 not taken.
|
3206 | const FrameType & type = (FrameType)(JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR)) const; |
424 | |||
425 | /// | ||
426 | /// \brief Checks if a frame given by its name exists. | ||
427 | /// | ||
428 | /// \param[in] name Name of the frame. | ||
429 | /// \param[in] type Type of the frame. | ||
430 | /// | ||
431 | /// \return Returns true if the frame exists. | ||
432 | /// | ||
433 | bool existFrame( | ||
434 | const std::string & name, | ||
435 |
4/15✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 11 times.
✓ Branch 9 taken 35 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
|
73 | const FrameType & type = (FrameType)(JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR)) const; |
436 | |||
437 | /// | ||
438 | /// \brief Adds a frame to the kinematic tree. | ||
439 | /// The inertia stored within the frame will be happened to the inertia supported by the | ||
440 | /// joint (frame.parentJoint). | ||
441 | /// | ||
442 | /// \param[in] frame The frame to add to the kinematic tree. | ||
443 | /// \param[in] append_inertia Append the inertia contained in the Frame to the inertia supported | ||
444 | /// by the joint. | ||
445 | /// | ||
446 | /// \return Returns the index of the frame if it has been successfully added or if it already | ||
447 | /// exists in the kinematic tree. | ||
448 | /// | ||
449 | FrameIndex addFrame(const Frame & frame, const bool append_inertia = true); | ||
450 | |||
451 | /// | ||
452 | /// \brief Check the validity of the attributes of Model with respect to the specification of | ||
453 | /// some algorithms. | ||
454 | /// | ||
455 | /// The method is a template so that the checkers can be defined in each algorithms. | ||
456 | /// \param[in] checker a class, typically defined in the algorithm module, that | ||
457 | /// validates the attributes of model. | ||
458 | /// | ||
459 | /// \return true if the Model is valid, false otherwise. | ||
460 | /// | ||
461 | template<typename D> | ||
462 | ✗ | bool check(const AlgorithmCheckerBase<D> & checker = AlgorithmCheckerBase<D>()) const | |
463 | { | ||
464 | ✗ | return checker.checkModel(*this); | |
465 | } | ||
466 | |||
467 | /// | ||
468 | /// \brief Check if joints have configuration limits | ||
469 | /// | ||
470 | /// \return Returns list of boolean of size model.nq. | ||
471 | /// | ||
472 | std::vector<bool> hasConfigurationLimit(); | ||
473 | |||
474 | /// | ||
475 | /// \brief Check if joints have configuration limits | ||
476 | /// | ||
477 | /// \return Returns list of boolean of size model.nq. | ||
478 | /// | ||
479 | std::vector<bool> hasConfigurationLimitInTangent(); | ||
480 | |||
481 | /// Run check(fusion::list) with DEFAULT_CHECKERS as argument. | ||
482 | bool check() const; | ||
483 | |||
484 | /// | ||
485 | /// \brief Run checkData on data and current model. | ||
486 | /// | ||
487 | /// \param[in] data to be checked wrt *this. | ||
488 | /// | ||
489 | /// \return true if the data is valid, false otherwise. | ||
490 | /// | ||
491 | bool check(const Data & data) const; | ||
492 | |||
493 | protected: | ||
494 | /// | ||
495 | /// \brief Add the joint_id to its parent subtrees. | ||
496 | /// | ||
497 | /// \param[in] joint_id The id of the joint to add to the subtrees | ||
498 | /// | ||
499 | void addJointIndexToParentSubtrees(const JointIndex joint_id); | ||
500 | }; | ||
501 | |||
502 | } // namespace pinocchio | ||
503 | |||
504 | /* --- Details -------------------------------------------------------------- */ | ||
505 | /* --- Details -------------------------------------------------------------- */ | ||
506 | /* --- Details -------------------------------------------------------------- */ | ||
507 | #include "pinocchio/multibody/model.hxx" | ||
508 | |||
509 | #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
510 | #include "pinocchio/multibody/model.txx" | ||
511 | #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
512 | |||
513 | #endif // ifndef __pinocchio_multibody_model_hpp__ | ||
514 |