GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 <iostream> |
||
24 |
#include <map> |
||
25 |
#include <iterator> |
||
26 |
|||
27 |
namespace pinocchio |
||
28 |
{ |
||
29 |
|||
30 |
template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl> |
||
31 |
struct ModelTpl |
||
32 |
: serialization::Serializable< ModelTpl<_Scalar,_Options,JointCollectionTpl> > |
||
33 |
{ |
||
34 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
35 |
|||
36 |
typedef _Scalar Scalar; |
||
37 |
enum { Options = _Options }; |
||
38 |
|||
39 |
typedef JointCollectionTpl<Scalar,Options> JointCollection; |
||
40 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
41 |
|||
42 |
typedef SE3Tpl<Scalar,Options> SE3; |
||
43 |
typedef MotionTpl<Scalar,Options> Motion; |
||
44 |
typedef ForceTpl<Scalar,Options> Force; |
||
45 |
typedef InertiaTpl<Scalar,Options> Inertia; |
||
46 |
typedef FrameTpl<Scalar,Options> Frame; |
||
47 |
|||
48 |
typedef pinocchio::Index Index; |
||
49 |
typedef pinocchio::JointIndex JointIndex; |
||
50 |
typedef pinocchio::GeomIndex GeomIndex; |
||
51 |
typedef pinocchio::FrameIndex FrameIndex; |
||
52 |
typedef std::vector<Index> IndexVector; |
||
53 |
|||
54 |
typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel; |
||
55 |
typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointData; |
||
56 |
|||
57 |
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector; |
||
58 |
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector; |
||
59 |
|||
60 |
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Frame) FrameVector; |
||
61 |
|||
62 |
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXs; |
||
63 |
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3; |
||
64 |
|||
65 |
/// \brief Dense vectorized version of a joint configuration vector. |
||
66 |
typedef VectorXs ConfigVectorType; |
||
67 |
|||
68 |
/// \brief Map between a string (key) and a configuration vector |
||
69 |
typedef std::map<std::string, ConfigVectorType> ConfigVectorMap; |
||
70 |
|||
71 |
/// \brief Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc). |
||
72 |
/// It also handles the notion of co-tangent vector (e.g. torque, etc). |
||
73 |
typedef VectorXs TangentVectorType; |
||
74 |
|||
75 |
/// \brief Dimension of the configuration vector representation. |
||
76 |
int nq; |
||
77 |
|||
78 |
/// \brief Dimension of the velocity vector space. |
||
79 |
int nv; |
||
80 |
|||
81 |
/// \brief Number of joints. |
||
82 |
int njoints; |
||
83 |
|||
84 |
/// \brief Number of bodies. |
||
85 |
int nbodies; |
||
86 |
|||
87 |
/// \brief Number of operational frames. |
||
88 |
int nframes; |
||
89 |
|||
90 |
/// \brief Spatial inertias of the body *i* expressed in the supporting joint frame *i*. |
||
91 |
PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) inertias; |
||
92 |
|||
93 |
/// \brief Placement (SE3) of the input of joint *i* regarding to the parent joint output *li*. |
||
94 |
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) jointPlacements; |
||
95 |
|||
96 |
/// \brief Model of joint *i*, encapsulated in a JointModelAccessor. |
||
97 |
JointModelVector joints; |
||
98 |
|||
99 |
/// \brief Starting index of the joint *i* in the configuration space |
||
100 |
std::vector<int> idx_qs; |
||
101 |
|||
102 |
/// \brief Dimension of the joint *i* configuration subspace |
||
103 |
std::vector<int> nqs; |
||
104 |
|||
105 |
/// \brief Starting index of the joint *i* in the tangent configuration space |
||
106 |
std::vector<int> idx_vs; |
||
107 |
|||
108 |
/// \brief Dimension of the joint *i* tangent subspace |
||
109 |
std::vector<int> nvs; |
||
110 |
|||
111 |
/// \brief Joint parent of joint *i*, denoted *li* (li==parents[i]). |
||
112 |
std::vector<JointIndex> parents; |
||
113 |
|||
114 |
/// \brief Name of joint *i* |
||
115 |
std::vector<std::string> names; |
||
116 |
|||
117 |
/// \brief Map of reference configurations, indexed by user given names. |
||
118 |
ConfigVectorMap referenceConfigurations; |
||
119 |
|||
120 |
/// \brief Vector of rotor inertia parameters |
||
121 |
TangentVectorType rotorInertia; |
||
122 |
|||
123 |
/// \brief Vector of rotor gear ratio parameters |
||
124 |
TangentVectorType rotorGearRatio; |
||
125 |
|||
126 |
/// \brief Vector of joint friction parameters |
||
127 |
TangentVectorType friction; |
||
128 |
|||
129 |
/// \brief Vector of joint damping parameters |
||
130 |
TangentVectorType damping; |
||
131 |
|||
132 |
/// \brief Vector of maximal joint torques |
||
133 |
TangentVectorType effortLimit; |
||
134 |
|||
135 |
/// \brief Vector of maximal joint velocities |
||
136 |
TangentVectorType velocityLimit; |
||
137 |
|||
138 |
/// \brief Lower joint configuration limit |
||
139 |
ConfigVectorType lowerPositionLimit; |
||
140 |
|||
141 |
/// \brief Upper joint configuration limit |
||
142 |
ConfigVectorType upperPositionLimit; |
||
143 |
|||
144 |
/// \brief Vector of operational frames registered on the model. |
||
145 |
FrameVector frames; |
||
146 |
|||
147 |
/// \brief Vector of joint supports. |
||
148 |
/// supports[j] corresponds to the collection of all joints located on the path between body *j* and the root. |
||
149 |
/// The last element of supports[j] is the index of the joint *j* itself. |
||
150 |
std::vector<IndexVector> supports; |
||
151 |
|||
152 |
/// \brief Vector of joint subtrees. |
||
153 |
/// subtree[j] corresponds to the subtree supported by the joint *j*. |
||
154 |
/// The first element of subtree[j] is the index of the joint *j* itself. |
||
155 |
std::vector<IndexVector> subtrees; |
||
156 |
|||
157 |
/// \brief Spatial gravity of the model. |
||
158 |
Motion gravity; |
||
159 |
|||
160 |
/// \brief Default 3D gravity vector (=(0,0,-9.81)). |
||
161 |
static const Vector3 gravity981; |
||
162 |
|||
163 |
/// \brief Model name; |
||
164 |
std::string name; |
||
165 |
|||
166 |
/// \brief Default constructor. Builds an empty model with no joints. |
||
167 |
604 |
ModelTpl() |
|
168 |
: nq(0) |
||
169 |
, nv(0) |
||
170 |
, njoints(1) |
||
171 |
, nbodies(1) |
||
172 |
, nframes(0) |
||
173 |
, inertias(1, Inertia::Zero()) |
||
174 |
, jointPlacements(1, SE3::Identity()) |
||
175 |
, joints(1) |
||
176 |
, idx_qs(1,0) |
||
177 |
, nqs(1,0) |
||
178 |
, idx_vs(1,0) |
||
179 |
, nvs(1,0) |
||
180 |
, parents(1, 0) |
||
181 |
, names(1) |
||
182 |
, supports(1,IndexVector(1,0)) |
||
183 |
, subtrees(1) |
||
184 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
604 |
, gravity(gravity981,Vector3::Zero()) |
185 |
{ |
||
186 |
✓✗ | 604 |
names[0] = "universe"; // Should be "universe joint (trivial)" |
187 |
// FIXME Should the universe joint be a FIXED_JOINT even if it is |
||
188 |
// in the list of joints ? See comment in definition of |
||
189 |
// Model::addJointFrame and Model::addBodyFrame |
||
190 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
604 |
addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT)); |
191 |
604 |
} |
|
192 |
835 |
~ModelTpl() {} // std::cout << "Destroy model" << std::endl; } |
|
193 |
|||
194 |
/// \returns An expression of *this with the Scalar type casted to NewScalar. |
||
195 |
template<typename NewScalar> |
||
196 |
10 |
ModelTpl<NewScalar,Options,JointCollectionTpl> cast() const |
|
197 |
{ |
||
198 |
typedef ModelTpl<NewScalar,Options,JointCollectionTpl> ReturnType; |
||
199 |
✓✗ | 10 |
ReturnType res; |
200 |
10 |
res.nq = nq; res.nv = nv; |
|
201 |
10 |
res.njoints = njoints; |
|
202 |
10 |
res.nbodies = nbodies; |
|
203 |
10 |
res.nframes = nframes; |
|
204 |
✓✗ | 10 |
res.parents = parents; |
205 |
✓✗ | 10 |
res.names = names; |
206 |
✓✗ | 10 |
res.supports = supports; |
207 |
✓✗ | 10 |
res.subtrees = subtrees; |
208 |
✓✗ | 10 |
res.gravity = gravity.template cast<NewScalar>(); |
209 |
✓✗ | 10 |
res.name = name; |
210 |
|||
211 |
✓✗ | 10 |
res.idx_qs = idx_qs; |
212 |
✓✗ | 10 |
res.nqs = nqs; |
213 |
✓✗ | 10 |
res.idx_vs = idx_vs; |
214 |
✓✗ | 10 |
res.nvs = nvs; |
215 |
|||
216 |
// Eigen Vectors |
||
217 |
✓✗✓✗ |
10 |
res.rotorInertia = rotorInertia.template cast<NewScalar>(); |
218 |
✓✗✓✗ |
10 |
res.rotorGearRatio = rotorGearRatio.template cast<NewScalar>(); |
219 |
✓✗✓✗ |
10 |
res.friction = friction.template cast<NewScalar>(); |
220 |
✓✗✓✗ |
10 |
res.damping = damping.template cast<NewScalar>(); |
221 |
✓✗✓✗ |
10 |
res.effortLimit = effortLimit.template cast<NewScalar>(); |
222 |
✓✗✓✗ |
10 |
res.velocityLimit = velocityLimit.template cast<NewScalar>(); |
223 |
✓✗✓✗ |
10 |
res.lowerPositionLimit = lowerPositionLimit.template cast<NewScalar>(); |
224 |
✓✗✓✗ |
10 |
res.upperPositionLimit = upperPositionLimit.template cast<NewScalar>(); |
225 |
|||
226 |
10 |
typename ConfigVectorMap::const_iterator it; |
|
227 |
10 |
for (it = referenceConfigurations.begin(); |
|
228 |
✗✓ | 10 |
it != referenceConfigurations.end(); it++ ) |
229 |
{ |
||
230 |
res.referenceConfigurations.insert(std::make_pair(it->first, it->second.template cast<NewScalar>())); |
||
231 |
} |
||
232 |
|||
233 |
// reserve vectors |
||
234 |
✓✗ | 10 |
res.inertias.resize(inertias.size()); |
235 |
✓✗ | 10 |
res.jointPlacements.resize(jointPlacements.size()); |
236 |
✓✗ | 10 |
res.joints.resize(joints.size()); |
237 |
✓✗ | 10 |
res.frames.resize(frames.size()); |
238 |
|||
239 |
/// copy into vectors |
||
240 |
✓✓ | 290 |
for(size_t k = 0; k < joints.size(); ++k) |
241 |
{ |
||
242 |
✓✗✓✗ |
280 |
res.inertias[k] = inertias[k].template cast<NewScalar>(); |
243 |
✓✗ | 280 |
res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>(); |
244 |
✓✗✓✗ |
280 |
res.joints[k] = joints[k].template cast<NewScalar>(); |
245 |
} |
||
246 |
|||
247 |
✓✓ | 560 |
for(size_t k = 0; k < frames.size(); ++k) |
248 |
{ |
||
249 |
✓✗✓✗ |
550 |
res.frames[k] = frames[k].template cast<NewScalar>(); |
250 |
} |
||
251 |
|||
252 |
20 |
return res; |
|
253 |
} |
||
254 |
|||
255 |
/// |
||
256 |
/// \brief Equality comparison operator. |
||
257 |
/// |
||
258 |
/// \returns true if *this is equal to other. |
||
259 |
/// |
||
260 |
75 |
bool operator==(const ModelTpl & other) const |
|
261 |
{ |
||
262 |
75 |
bool res = |
|
263 |
75 |
other.nq == nq |
|
264 |
✓✗ | 73 |
&& other.nv == nv |
265 |
✓✗ | 73 |
&& other.njoints == njoints |
266 |
✓✗ | 73 |
&& other.nbodies == nbodies |
267 |
✓✗ | 73 |
&& other.nframes == nframes |
268 |
✓✗✓✗ |
73 |
&& other.parents == parents |
269 |
✓✗✓✗ |
73 |
&& other.names == names |
270 |
✓✗✓✗ |
73 |
&& other.subtrees == subtrees |
271 |
✓✗✓✗ |
73 |
&& other.gravity == gravity |
272 |
✓✓✓✗ |
148 |
&& other.name == name; |
273 |
|||
274 |
75 |
res &= |
|
275 |
✓✗ | 75 |
other.idx_qs == idx_qs |
276 |
✓✗✓✗ |
73 |
&& other.nqs == nqs |
277 |
✓✗✓✗ |
73 |
&& other.idx_vs == idx_vs |
278 |
✓✓✓✗ ✓✗ |
148 |
&& other.nvs == nvs; |
279 |
|||
280 |
✗✓ | 75 |
if(other.referenceConfigurations.size() != referenceConfigurations.size()) |
281 |
return false; |
||
282 |
|||
283 |
75 |
typename ConfigVectorMap::const_iterator it = referenceConfigurations.begin(); |
|
284 |
75 |
typename ConfigVectorMap::const_iterator it_other = other.referenceConfigurations.begin(); |
|
285 |
✓✓ | 81 |
for(long k = 0; k < (long)referenceConfigurations.size(); ++k) |
286 |
{ |
||
287 |
✓✗✓✗ |
8 |
std::advance(it,k); std::advance(it_other,k); |
288 |
|||
289 |
✓✗✓✗ ✓✓ |
8 |
if(it->second.size() != it_other->second.size()) |
290 |
2 |
return false; |
|
291 |
✓✗✗✓ |
6 |
if(it->second != it_other->second) |
292 |
return false; |
||
293 |
} |
||
294 |
|||
295 |
✓✗✓✗ ✗✓ |
73 |
if(other.rotorInertia.size() != rotorInertia.size()) |
296 |
return false; |
||
297 |
✓✗ | 73 |
res &= other.rotorInertia == rotorInertia; |
298 |
✗✓ | 73 |
if(!res) return res; |
299 |
|||
300 |
✓✗✓✗ ✗✓ |
73 |
if(other.friction.size() != friction.size()) |
301 |
return false; |
||
302 |
✓✗ | 73 |
res &= other.friction == friction; |
303 |
✗✓ | 73 |
if(!res) return res; |
304 |
|||
305 |
✓✗✓✗ ✗✓ |
73 |
if(other.damping.size() != damping.size()) |
306 |
return false; |
||
307 |
✓✗ | 73 |
res &= other.damping == damping; |
308 |
✗✓ | 73 |
if(!res) return res; |
309 |
|||
310 |
✓✗✓✗ ✗✓ |
73 |
if(other.rotorGearRatio.size() != rotorGearRatio.size()) |
311 |
return false; |
||
312 |
✓✗ | 73 |
res &= other.rotorGearRatio == rotorGearRatio; |
313 |
✗✓ | 73 |
if(!res) return res; |
314 |
|||
315 |
✓✗✓✗ ✗✓ |
73 |
if(other.effortLimit.size() != effortLimit.size()) |
316 |
return false; |
||
317 |
✓✗ | 73 |
res &= other.effortLimit == effortLimit; |
318 |
✗✓ | 73 |
if(!res) return res; |
319 |
|||
320 |
✓✗✓✗ ✗✓ |
73 |
if(other.velocityLimit.size() != velocityLimit.size()) |
321 |
return false; |
||
322 |
✓✗ | 73 |
res &= other.velocityLimit == velocityLimit; |
323 |
✗✓ | 73 |
if(!res) return res; |
324 |
|||
325 |
✓✗✓✗ ✗✓ |
73 |
if(other.lowerPositionLimit.size() != lowerPositionLimit.size()) |
326 |
return false; |
||
327 |
✓✗ | 73 |
res &= other.lowerPositionLimit == lowerPositionLimit; |
328 |
✗✓ | 73 |
if(!res) return res; |
329 |
|||
330 |
✓✗✓✗ ✗✓ |
73 |
if(other.upperPositionLimit.size() != upperPositionLimit.size()) |
331 |
return false; |
||
332 |
✓✗ | 73 |
res &= other.upperPositionLimit == upperPositionLimit; |
333 |
✗✓ | 73 |
if(!res) return res; |
334 |
|||
335 |
✓✓ | 2232 |
for(size_t k = 1; k < inertias.size(); ++k) |
336 |
{ |
||
337 |
✓✗ | 2159 |
res &= other.inertias[k] == inertias[k]; |
338 |
✗✓ | 2159 |
if(!res) return res; |
339 |
} |
||
340 |
|||
341 |
✓✓ | 2232 |
for(size_t k = 1; k < other.jointPlacements.size(); ++k) |
342 |
{ |
||
343 |
✓✗ | 2159 |
res &= other.jointPlacements[k] == jointPlacements[k]; |
344 |
✗✓ | 2159 |
if(!res) return res; |
345 |
} |
||
346 |
|||
347 |
73 |
res &= |
|
348 |
✓✗ | 73 |
other.joints == joints |
349 |
✓✗✓✗ ✓✗ |
73 |
&& other.frames == frames; |
350 |
|||
351 |
73 |
return res; |
|
352 |
} |
||
353 |
|||
354 |
/// |
||
355 |
/// \returns true if *this is NOT equal to other. |
||
356 |
/// |
||
357 |
1 |
bool operator!=(const ModelTpl & other) const |
|
358 |
1 |
{ return !(*this == other); } |
|
359 |
|||
360 |
/// |
||
361 |
/// \brief Add a joint to the kinematic tree with infinite bounds. |
||
362 |
/// |
||
363 |
/// \remark This method also adds a Frame of same name to the vector of frames. |
||
364 |
/// \remark The inertia supported by the joint is set to Zero. |
||
365 |
/// \remark Joints need to be added to the tree in a depth-first order. |
||
366 |
/// |
||
367 |
/// \tparam JointModelDerived The type of the joint model. |
||
368 |
/// |
||
369 |
/// \param[in] parent Index of the parent joint. |
||
370 |
/// \param[in] joint_model The joint model. |
||
371 |
/// \param[in] joint_placement Placement of the joint inside its parent joint. |
||
372 |
/// \param[in] joint_name Name of the joint. If empty, the name is random. |
||
373 |
/// |
||
374 |
/// \return The index of the new joint. |
||
375 |
/// |
||
376 |
/// \sa Model::appendBodyToJoint |
||
377 |
/// |
||
378 |
JointIndex addJoint(const JointIndex parent, |
||
379 |
const JointModel & joint_model, |
||
380 |
const SE3 & joint_placement, |
||
381 |
const std::string & joint_name); |
||
382 |
|||
383 |
/// |
||
384 |
/// \copydoc ModelTpl::addJoint(const JointIndex,const JointModel &,const SE3 &,const std::string &) |
||
385 |
/// |
||
386 |
/// \param[in] max_effort Maximal joint torque. |
||
387 |
/// \param[in] max_velocity Maximal joint velocity. |
||
388 |
/// \param[in] min_config Lower joint configuration. |
||
389 |
/// \param[in] max_config Upper joint configuration. |
||
390 |
/// |
||
391 |
JointIndex addJoint(const JointIndex parent, |
||
392 |
const JointModel & joint_model, |
||
393 |
const SE3 & joint_placement, |
||
394 |
const std::string & joint_name, |
||
395 |
const VectorXs & max_effort, |
||
396 |
const VectorXs & max_velocity, |
||
397 |
const VectorXs & min_config, |
||
398 |
const VectorXs & max_config); |
||
399 |
|||
400 |
/// |
||
401 |
/// \copydoc ModelTpl::addJoint(const JointIndex,const JointModel &,const SE3 &,const std::string &,const VectorXs &,const VectorXs &,const VectorXs &,const VectorXs &) |
||
402 |
/// |
||
403 |
/// \param[in] friction Joint friction parameters. |
||
404 |
/// \param[in] damping Joint damping parameters. |
||
405 |
/// |
||
406 |
JointIndex addJoint(const JointIndex parent, |
||
407 |
const JointModel & joint_model, |
||
408 |
const SE3 & joint_placement, |
||
409 |
const std::string & joint_name, |
||
410 |
const VectorXs & max_effort, |
||
411 |
const VectorXs & max_velocity, |
||
412 |
const VectorXs & min_config, |
||
413 |
const VectorXs & max_config, |
||
414 |
const VectorXs & friction, |
||
415 |
const VectorXs & damping); |
||
416 |
|||
417 |
/// |
||
418 |
/// \brief Add a joint to the frame tree. |
||
419 |
/// |
||
420 |
/// \param[in] jointIndex Index of the joint. |
||
421 |
/// \param[in] frameIndex Index of the parent frame. If negative, |
||
422 |
/// the parent frame is the frame of the parent joint. |
||
423 |
/// |
||
424 |
/// \return The index of the new frame |
||
425 |
/// |
||
426 |
FrameIndex addJointFrame(const JointIndex & joint_index, |
||
427 |
int previous_frame_index = -1); |
||
428 |
|||
429 |
/// |
||
430 |
/// \brief Append a body to a given joint of the kinematic tree. |
||
431 |
/// |
||
432 |
/// \param[in] joint_index Index of the supporting joint. |
||
433 |
/// \param[in] Y Spatial inertia of the body. |
||
434 |
/// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement. |
||
435 |
/// |
||
436 |
/// \sa Model::addJoint |
||
437 |
/// |
||
438 |
void appendBodyToJoint(const JointIndex joint_index, const Inertia & Y, |
||
439 |
const SE3 & body_placement = SE3::Identity()); |
||
440 |
|||
441 |
/// |
||
442 |
/// \brief Add a body to the frame tree. |
||
443 |
/// |
||
444 |
/// \param[in] body_name Name of the body. |
||
445 |
/// \param[in] parentJoint Index of the parent joint. |
||
446 |
/// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement. |
||
447 |
/// \param[in] previousFrame Index of the parent frame. If negative, |
||
448 |
/// the parent frame is the frame of the parent joint. |
||
449 |
/// |
||
450 |
/// \return The index of the new frame |
||
451 |
/// |
||
452 |
FrameIndex addBodyFrame(const std::string & body_name, |
||
453 |
const JointIndex & parentJoint, |
||
454 |
const SE3 & body_placement = SE3::Identity(), |
||
455 |
int previousFrame = -1); |
||
456 |
|||
457 |
/// |
||
458 |
/// \brief Return the index of a body given by its name. |
||
459 |
/// |
||
460 |
/// \warning If no body is found, return the number of elements at time T. |
||
461 |
/// This can lead to errors if the model is expanded after this method is called |
||
462 |
/// (for example to get the id of a parent body) |
||
463 |
/// |
||
464 |
/// \param[in] name Name of the body. |
||
465 |
/// |
||
466 |
/// \return Index of the body. |
||
467 |
/// |
||
468 |
FrameIndex getBodyId(const std::string & name) const; |
||
469 |
|||
470 |
/// |
||
471 |
/// \brief Check if a body given by its name exists. |
||
472 |
/// |
||
473 |
/// \param[in] name Name of the body. |
||
474 |
/// |
||
475 |
/// \return True if the body exists in the kinematic tree. |
||
476 |
/// |
||
477 |
bool existBodyName(const std::string & name) const; |
||
478 |
|||
479 |
|||
480 |
/// |
||
481 |
/// \brief Return the index of a joint given by its name. |
||
482 |
/// |
||
483 |
/// \warning If no joint is found, return the number of elements at time T. |
||
484 |
/// This can lead to errors if the model is expanded after this method is called |
||
485 |
/// (for example to get the id of a parent joint) |
||
486 |
/// \param[in] name Name of the joint. |
||
487 |
/// |
||
488 |
/// \return Index of the joint. |
||
489 |
/// |
||
490 |
JointIndex getJointId(const std::string & name) const; |
||
491 |
|||
492 |
/// |
||
493 |
/// \brief Check if a joint given by its name exists. |
||
494 |
/// |
||
495 |
/// \param[in] name Name of the joint. |
||
496 |
/// |
||
497 |
/// \return True if the joint exists in the kinematic tree. |
||
498 |
/// |
||
499 |
bool existJointName(const std::string & name) const; |
||
500 |
|||
501 |
/// |
||
502 |
/// \brief Returns the index of a frame given by its name. |
||
503 |
/// \sa Model::existFrame to check if the frame exists or not. |
||
504 |
/// |
||
505 |
/// \warning If no frame is found, returns the size of the vector of Model::frames. |
||
506 |
/// This can lead to errors if the model is expanded after this method is called |
||
507 |
/// (for example to get the id of a parent frame). |
||
508 |
/// |
||
509 |
/// \param[in] name Name of the frame. |
||
510 |
/// \param[in] type Type of the frame. |
||
511 |
/// |
||
512 |
/// \return Index of the frame. |
||
513 |
/// |
||
514 |
FrameIndex getFrameId(const std::string & name, |
||
515 |
✓✗✓✓ ✓✓✗✓ ✓✗✓✓ ✓✓✓✓ ✓✓✓✓ ✓✓✓✓ ✓✓✓✓ ✗✓✓✗ ✓✓✗✓ ✗✓✗✓ ✓✗✓✓ ✗ |
3029 |
const FrameType & type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const; |
516 |
|||
517 |
/// |
||
518 |
/// \brief Checks if a frame given by its name exists. |
||
519 |
/// |
||
520 |
/// \param[in] name Name of the frame. |
||
521 |
/// \param[in] type Type of the frame. |
||
522 |
/// |
||
523 |
/// \return Returns true if the frame exists. |
||
524 |
/// |
||
525 |
bool existFrame(const std::string & name, |
||
526 |
✓✗✓✓ ✓✗✗✗ ✓✓✗✓ ✗✗✗✗ ✓✗ |
239 |
const FrameType& type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const; |
527 |
|||
528 |
/// |
||
529 |
/// \brief Adds a frame to the kinematic tree. |
||
530 |
/// The inertia stored within the frame will be happened to the inertia supported by the joint (frame.parent). |
||
531 |
/// |
||
532 |
/// \param[in] frame The frame to add to the kinematic tree. |
||
533 |
/// \param[in] append_inertia Append the inertia contained in the Frame to the inertia supported by the joint. |
||
534 |
/// |
||
535 |
/// \return Returns the index of the frame if it has been successfully added or if it already exists in the kinematic tree. |
||
536 |
/// |
||
537 |
FrameIndex addFrame(const Frame & frame, |
||
538 |
const bool append_inertia = true); |
||
539 |
|||
540 |
/// |
||
541 |
/// \brief Check the validity of the attributes of Model with respect to the specification of some |
||
542 |
/// algorithms. |
||
543 |
/// |
||
544 |
/// The method is a template so that the checkers can be defined in each algorithms. |
||
545 |
/// \param[in] checker a class, typically defined in the algorithm module, that |
||
546 |
/// validates the attributes of model. |
||
547 |
/// |
||
548 |
/// \return true if the Model is valid, false otherwise. |
||
549 |
/// |
||
550 |
template<typename D> |
||
551 |
12 |
inline bool check(const AlgorithmCheckerBase<D> & checker = AlgorithmCheckerBase<D>()) const |
|
552 |
12 |
{ return checker.checkModel(*this); } |
|
553 |
|||
554 |
/// |
||
555 |
/// \brief Check if joints have configuration limits |
||
556 |
/// |
||
557 |
/// \return Returns list of boolean of size model.nq. |
||
558 |
/// |
||
559 |
std::vector<bool> hasConfigurationLimit(); |
||
560 |
|||
561 |
/// |
||
562 |
/// \brief Check if joints have configuration limits |
||
563 |
/// |
||
564 |
/// \return Returns list of boolean of size model.nq. |
||
565 |
/// |
||
566 |
std::vector<bool> hasConfigurationLimitInTangent(); |
||
567 |
|||
568 |
/// Run check(fusion::list) with DEFAULT_CHECKERS as argument. |
||
569 |
inline bool check() const; |
||
570 |
|||
571 |
/// |
||
572 |
/// \brief Run checkData on data and current model. |
||
573 |
/// |
||
574 |
/// \param[in] data to be checked wrt *this. |
||
575 |
/// |
||
576 |
/// \return true if the data is valid, false otherwise. |
||
577 |
/// |
||
578 |
inline bool check(const Data & data) const; |
||
579 |
|||
580 |
protected: |
||
581 |
|||
582 |
/// |
||
583 |
/// \brief Add the joint_id to its parent subtrees. |
||
584 |
/// |
||
585 |
/// \param[in] joint_id The id of the joint to add to the subtrees |
||
586 |
/// |
||
587 |
void addJointIndexToParentSubtrees(const JointIndex joint_id); |
||
588 |
}; |
||
589 |
|||
590 |
} // namespace pinocchio |
||
591 |
|||
592 |
/* --- Details -------------------------------------------------------------- */ |
||
593 |
/* --- Details -------------------------------------------------------------- */ |
||
594 |
/* --- Details -------------------------------------------------------------- */ |
||
595 |
#include "pinocchio/multibody/model.hxx" |
||
596 |
|||
597 |
#endif // ifndef __pinocchio_multibody_model_hpp__ |
Generated by: GCOVR (Version 4.2) |