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