| Directory: | ./ |
|---|---|
| File: | include/pinocchio/multibody/model-item.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 15 | 15 | 100.0% |
| Branches: | 6 | 12 | 50.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2021-2022 INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_multibody_model_item_hpp__ | ||
| 6 | #define __pinocchio_multibody_model_item_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/multibody/fwd.hpp" | ||
| 9 | |||
| 10 | namespace pinocchio | ||
| 11 | { | ||
| 12 | template<typename Derived> | ||
| 13 | struct ModelItem : NumericalBase<Derived> | ||
| 14 | { | ||
| 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 16 | |||
| 17 | typedef typename traits<Derived>::Scalar Scalar; | ||
| 18 | enum | ||
| 19 | { | ||
| 20 | Options = traits<Derived>::Options | ||
| 21 | }; | ||
| 22 | typedef SE3Tpl<Scalar, Options> SE3; | ||
| 23 | |||
| 24 | /// \brief Name of the kinematic element | ||
| 25 | std::string name; | ||
| 26 | |||
| 27 | /// \brief Index of the parent joint | ||
| 28 | JointIndex parentJoint; | ||
| 29 | |||
| 30 | /// \brief Index of the parent frame | ||
| 31 | /// | ||
| 32 | /// Parent frame may be unset (to the std::numeric_limits<FrameIndex>::max() value) as it is | ||
| 33 | /// mostly used as a documentation of the tree, or in third-party libraries. The URDF parser of | ||
| 34 | /// Pinocchio is setting it to the proper value according to the urdf link-joint tree. In | ||
| 35 | /// particular, anchor joints of URDF would cause parent frame to be different to joint frame. | ||
| 36 | FrameIndex parentFrame; | ||
| 37 | |||
| 38 | /// \brief Position of kinematic element in parent joint frame | ||
| 39 | SE3 placement; | ||
| 40 | |||
| 41 | /// | ||
| 42 | /// \brief Default constructor of ModelItem. | ||
| 43 | /// | ||
| 44 | 3214 | ModelItem() | |
| 45 | 3214 | : name() | |
| 46 | 3214 | , parentJoint() | |
| 47 | 3214 | , parentFrame() | |
| 48 |
1/2✓ Branch 1 taken 2023 times.
✗ Branch 2 not taken.
|
3214 | , placement() |
| 49 | { | ||
| 50 | 3214 | } | |
| 51 | |||
| 52 | /// | ||
| 53 | /// \brief Builds a kinematic element defined by its name, its joint parent id, its parent frame | ||
| 54 | /// id and its placement. | ||
| 55 | /// | ||
| 56 | /// \param[in] name Name of the frame. | ||
| 57 | /// \param[in] parent_joint Index of the parent joint in the kinematic tree. | ||
| 58 | /// \param[in] parent_frame Index of the parent frame in the kinematic tree. | ||
| 59 | /// \param[in] frame_placement Placement of the frame wrt the parent joint frame. | ||
| 60 | /// | ||
| 61 | 157083 | ModelItem( | |
| 62 | const std::string & name, | ||
| 63 | const JointIndex parent_joint, | ||
| 64 | const FrameIndex parent_frame, | ||
| 65 | const SE3 & frame_placement) | ||
| 66 | 157083 | : name(name) | |
| 67 | 157083 | , parentJoint(parent_joint) | |
| 68 | 157083 | , parentFrame(parent_frame) | |
| 69 |
1/2✓ Branch 1 taken 145595 times.
✗ Branch 2 not taken.
|
157083 | , placement(frame_placement) |
| 70 | { | ||
| 71 | 157083 | } | |
| 72 | |||
| 73 | 7 | bool operator==(const ModelItem & other) const | |
| 74 | { | ||
| 75 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
14 | return name == other.name && parentJoint == other.parentJoint |
| 76 |
3/6✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
|
14 | && parentFrame == other.parentFrame && placement == other.placement; |
| 77 | } | ||
| 78 | }; | ||
| 79 | } // namespace pinocchio | ||
| 80 | |||
| 81 | #endif // ifndef __pinocchio_multibody_model_item_hpp__ | ||
| 82 |