GCC Code Coverage Report

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