Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/frame.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 21 | 55 | 38.2% |
Branches: | 10 | 28 | 35.7% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2016-2021 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_multibody_frame_hpp__ | ||
6 | #define __pinocchio_multibody_frame_hpp__ | ||
7 | |||
8 | #include "pinocchio/spatial/se3.hpp" | ||
9 | #include "pinocchio/spatial/inertia.hpp" | ||
10 | #include "pinocchio/multibody/fwd.hpp" | ||
11 | #include "pinocchio/multibody/model-item.hpp" | ||
12 | |||
13 | #include <string> | ||
14 | |||
15 | namespace pinocchio | ||
16 | { | ||
17 | /// | ||
18 | /// \brief Enum on the possible types of frames. | ||
19 | /// | ||
20 | /// \note | ||
21 | /// In Pinocchio, the FIXED joints are not included in the kinematic tree but we keep track of | ||
22 | /// them via the vector of frames contained in ModelTpl. The JOINT frames are duplicate | ||
23 | /// information with respect to the joint information contained in ModelTpl. | ||
24 | /// | ||
25 | /// All other frame types are defined for user convenience and code | ||
26 | /// readability, to also keep track of the information usually stored within URDF models. | ||
27 | /// | ||
28 | /// See also https://wiki.ros.org/urdf/XML/joint, https://wiki.ros.org/urdf/XML/link and | ||
29 | /// https://wiki.ros.org/urdf/XML/sensor. | ||
30 | /// | ||
31 | enum FrameType | ||
32 | { | ||
33 | OP_FRAME = 0x1 << 0, ///< operational frame: user-defined frames that are defined at runtime | ||
34 | JOINT = 0x1 << 1, ///< joint frame: attached to the child body of a joint (a.k.a. child frame) | ||
35 | FIXED_JOINT = 0x1 << 2, ///< fixed joint frame: joint frame but for a fixed joint | ||
36 | BODY = | ||
37 | 0x1 << 3, ///< body frame: attached to the collision, inertial or visual properties of a link | ||
38 | SENSOR = 0x1 << 4 ///< sensor frame: defined in a sensor element | ||
39 | }; | ||
40 | |||
41 | template<typename _Scalar, int _Options> | ||
42 | struct traits<FrameTpl<_Scalar, _Options>> | ||
43 | { | ||
44 | typedef _Scalar Scalar; | ||
45 | enum | ||
46 | { | ||
47 | Options = _Options | ||
48 | }; | ||
49 | }; | ||
50 | |||
51 | /// | ||
52 | /// \brief A Plucker coordinate frame attached to a parent joint inside a kinematic tree | ||
53 | /// | ||
54 | template<typename _Scalar, int _Options> | ||
55 | struct FrameTpl : ModelItem<FrameTpl<_Scalar, _Options>> | ||
56 | { | ||
57 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
58 | typedef FrameTpl<_Scalar, _Options> ModelItemDerived; | ||
59 | typedef typename traits<ModelItemDerived>::Scalar Scalar; | ||
60 | enum | ||
61 | { | ||
62 | Options = traits<ModelItemDerived>::Options | ||
63 | }; | ||
64 | typedef ModelItem<ModelItemDerived> Base; | ||
65 | |||
66 | typedef SE3Tpl<Scalar, Options> SE3; | ||
67 | typedef InertiaTpl<Scalar, Options> Inertia; | ||
68 | typedef pinocchio::JointIndex JointIndex; | ||
69 | |||
70 | /// | ||
71 | /// \brief Default constructor of a frame. | ||
72 | /// | ||
73 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
74 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
75 | ✗ | FrameTpl() | |
76 | : Base() | ||
77 | ✗ | , parent(Base::parentJoint) | |
78 | ✗ | , previousFrame(Base::parentFrame) | |
79 | ✗ | , type() | |
80 | ✗ | , inertia(Inertia::Zero()) | |
81 | { | ||
82 | ✗ | } // needed by std::vector | |
83 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
84 | |||
85 | /// | ||
86 | /// \brief Builds a frame defined by its name, its joint parent id, its placement and its type. | ||
87 | /// | ||
88 | /// \param[in] name Name of the frame. | ||
89 | /// \param[in] parent Index of the parent joint in the kinematic tree. | ||
90 | /// \param[in] frame_placement Placement of the frame wrt the parent joint frame. | ||
91 | /// \param[in] type The type of the frame, see the enum FrameType. | ||
92 | /// \param[in] inertia Inertia info attached to the frame. | ||
93 | /// | ||
94 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
95 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
96 | ✗ | FrameTpl( | |
97 | const std::string & name, | ||
98 | const JointIndex parentJoint, | ||
99 | const SE3 & frame_placement, | ||
100 | const FrameType type, | ||
101 | const Inertia & inertia = Inertia::Zero()) | ||
102 | : Base(name, parentJoint, 0, frame_placement) | ||
103 | ✗ | , parent(Base::parentJoint) | |
104 | ✗ | , previousFrame(Base::parentFrame) | |
105 | ✗ | , type(type) | |
106 | ✗ | , inertia(inertia) | |
107 | { | ||
108 | } | ||
109 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
110 | |||
111 | /// | ||
112 | /// \brief Builds a frame defined by its name, its joint parent id, its placement and its type. | ||
113 | /// | ||
114 | /// \param[in] name Name of the frame. | ||
115 | /// \param[in] parent Index of the parent joint in the kinematic tree. | ||
116 | /// \param[in] parentFrame Index of the parent frame in the kinematic tree. | ||
117 | /// \param[in] frame_placement Placement of the frame wrt the parent joint frame. | ||
118 | /// \param[in] type The type of the frame, see the enum FrameType. | ||
119 | /// \param[in] inertia Inertia info attached to the frame. | ||
120 | /// | ||
121 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
122 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
123 | 10692 | FrameTpl( | |
124 | const std::string & name, | ||
125 | const JointIndex parent_joint, | ||
126 | const FrameIndex parent_frame, | ||
127 | const SE3 & frame_placement, | ||
128 | const FrameType type, | ||
129 | const Inertia & inertia = Inertia::Zero()) | ||
130 | : Base(name, parent_joint, parent_frame, frame_placement) | ||
131 | 10692 | , parent(Base::parentJoint) | |
132 | 10692 | , previousFrame(Base::parentFrame) | |
133 | 10692 | , type(type) | |
134 |
1/2✓ Branch 2 taken 10692 times.
✗ Branch 3 not taken.
|
10692 | , inertia(inertia) |
135 | { | ||
136 | 10692 | } | |
137 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
138 | |||
139 | /// | ||
140 | /// \brief Copy constructor | ||
141 | /// | ||
142 | /// \param[in] other Frame to copy | ||
143 | /// | ||
144 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
145 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
146 | 22205 | FrameTpl(const FrameTpl & other) | |
147 | 22205 | : Base(other.name, other.parentJoint, other.parentFrame, other.placement) | |
148 | 22205 | , parent(Base::parentJoint) | |
149 | 22205 | , previousFrame(Base::parentFrame) | |
150 | 22205 | , type(other.type) | |
151 |
1/2✓ Branch 2 taken 22205 times.
✗ Branch 3 not taken.
|
22205 | , inertia(other.inertia) |
152 | { | ||
153 | 22205 | } | |
154 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
155 | |||
156 | /// | ||
157 | /// \brief Copy constructor by casting | ||
158 | /// | ||
159 | /// \param[in] other Frame to copy | ||
160 | /// | ||
161 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
162 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
163 | template<typename S2, int O2> | ||
164 | ✗ | explicit FrameTpl(const FrameTpl<S2, O2> & other) | |
165 | : Base( | ||
166 | ✗ | other.name, other.parentJoint, other.parentFrame, other.placement.template cast<Scalar>()) | |
167 | ✗ | , parent(Base::parentJoint) | |
168 | ✗ | , previousFrame(Base::parentFrame) | |
169 | ✗ | , type(other.type) | |
170 | ✗ | , inertia(other.inertia.template cast<Scalar>()) | |
171 | { | ||
172 | } | ||
173 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
174 | |||
175 | /// | ||
176 | /// \brief Copy assignment operator. It needs to be user-define because references cannot be | ||
177 | /// re-assigned during copy | ||
178 | /// | ||
179 | /// \param[in] other Frame to copy | ||
180 | /// | ||
181 | |||
182 | ✗ | FrameTpl<Scalar, Options> & operator=(const FrameTpl<Scalar, Options> & other) | |
183 | { | ||
184 | ✗ | name = other.name; | |
185 | ✗ | parentJoint = other.parentJoint; | |
186 | ✗ | parentFrame = other.parentFrame; | |
187 | ✗ | placement = other.placement; | |
188 | ✗ | type = other.type; | |
189 | ✗ | inertia = other.inertia; | |
190 | ✗ | return *this; | |
191 | } | ||
192 | |||
193 | /// | ||
194 | /// \brief Equality comparison operator. | ||
195 | /// | ||
196 | /// \returns true if *this is equal to other. | ||
197 | /// | ||
198 | /// \param[in] other The frame to which the current frame is compared. | ||
199 | /// | ||
200 | template<typename S2, int O2> | ||
201 | 60 | bool operator==(const FrameTpl<S2, O2> & other) const | |
202 | { | ||
203 |
1/2✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
|
120 | return name == other.name && parentJoint == other.parentJoint |
204 |
2/4✓ Branch 0 taken 60 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 60 times.
✗ Branch 4 not taken.
|
60 | && parentFrame == other.parentFrame && placement == other.placement |
205 |
3/6✓ Branch 0 taken 60 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 60 times.
✗ Branch 6 not taken.
|
120 | && type == other.type && inertia == other.inertia; |
206 | } | ||
207 | |||
208 | /// | ||
209 | /// \returns true if *this is NOT equal to other. | ||
210 | /// | ||
211 | template<typename S2, int O2> | ||
212 | ✗ | bool operator!=(const FrameTpl<S2, O2> & other) const | |
213 | { | ||
214 | ✗ | return !(*this == other); | |
215 | } | ||
216 | |||
217 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
218 | template<typename NewScalar> | ||
219 | 514 | FrameTpl<NewScalar, Options> cast() const | |
220 | { | ||
221 | typedef FrameTpl<NewScalar, Options> ReturnType; | ||
222 | 514 | ReturnType res( | |
223 |
2/4✓ Branch 1 taken 514 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 514 times.
✗ Branch 5 not taken.
|
514 | name, parentJoint, parentFrame, placement.template cast<NewScalar>(), type, |
224 | inertia.template cast<NewScalar>()); | ||
225 | 514 | return res; | |
226 | } | ||
227 | |||
228 | // data | ||
229 | /// \brief Index of the parent joint. | ||
230 | /// \deprecated use \ref parentJoint instead | ||
231 | PINOCCHIO_DEPRECATED JointIndex & parent; | ||
232 | |||
233 | /// \brief Index of the previous frame. | ||
234 | /// \deprecated use \ref parentFrame instead | ||
235 | PINOCCHIO_DEPRECATED FrameIndex & previousFrame; | ||
236 | |||
237 | using Base::name; | ||
238 | using Base::parentFrame; | ||
239 | using Base::parentJoint; | ||
240 | using Base::placement; | ||
241 | |||
242 | /// \brief Type of the frame. | ||
243 | FrameType type; | ||
244 | |||
245 | /// \brief Inertia information attached to the frame. | ||
246 | /// This inertia will be appended to the inertia supported by the parent joint when | ||
247 | /// calling ModelTpl::addFrame. It won't be processed otherwise by the algorithms. | ||
248 | Inertia inertia; | ||
249 | |||
250 | }; // struct FrameTpl | ||
251 | |||
252 | template<typename Scalar, int Options> | ||
253 | ✗ | inline std::ostream & operator<<(std::ostream & os, const FrameTpl<Scalar, Options> & f) | |
254 | { | ||
255 | ✗ | os << "Frame name: " << f.name << " paired to (parent joint/ parent frame)" | |
256 | ✗ | << "(" << f.parentJoint << "/" << f.parentFrame << ")" << std::endl | |
257 | ✗ | << "with relative placement wrt parent joint:\n" | |
258 | ✗ | << f.placement << "containing inertia:\n" | |
259 | ✗ | << f.inertia << std::endl; | |
260 | |||
261 | ✗ | return os; | |
262 | } | ||
263 | |||
264 | } // namespace pinocchio | ||
265 | |||
266 | #endif // ifndef __pinocchio_multibody_frame_hpp__ | ||
267 |