| 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 |