GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/frame.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 57 57 100.0%
Branches: 15 28 53.6%
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 2409 FrameTpl()
76 : Base()
77 2409 , parent(Base::parentJoint)
78 2409 , previousFrame(Base::parentFrame)
79 2409 , type()
80
1/2
✓ Branch 2 taken 1813 times.
✗ Branch 3 not taken.
 2409 , inertia(Inertia::Zero())
81 {
82 2409 } // 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 2 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 2 , parent(Base::parentJoint)
104 2 , previousFrame(Base::parentFrame)
105 2 , type(type)
106
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 , inertia(inertia)
107 {
108 2 }
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 34691 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 34691 , parent(Base::parentJoint)
132 34691 , previousFrame(Base::parentFrame)
133 34691 , type(type)
134
1/2
✓ Branch 2 taken 33433 times.
✗ Branch 3 not taken.
 34691 , inertia(inertia)
135 {
136 34691 }
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 119440 FrameTpl(const FrameTpl & other)
147 119440 : Base(other.name, other.parentJoint, other.parentFrame, other.placement)
148 119440 , parent(Base::parentJoint)
149 119440 , previousFrame(Base::parentFrame)
150 119440 , type(other.type)
151
1/2
✓ Branch 2 taken 110009 times.
✗ Branch 3 not taken.
 119440 , inertia(other.inertia)
152 {
153 119440 }
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 1 explicit FrameTpl(const FrameTpl<S2, O2> & other)
165 : Base(
166 1 other.name, other.parentJoint, other.parentFrame, other.placement.template cast<Scalar>())
167 1 , parent(Base::parentJoint)
168 1 , previousFrame(Base::parentFrame)
169 1 , type(other.type)
170
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 , inertia(other.inertia.template cast<Scalar>())
171 {
172 1 }
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 2164 FrameTpl<Scalar, Options> & operator=(const FrameTpl<Scalar, Options> & other)
183 {
184 2164 name = other.name;
185 2164 parentJoint = other.parentJoint;
186 2164 parentFrame = other.parentFrame;
187 2164 placement = other.placement;
188 2164 type = other.type;
189 2164 inertia = other.inertia;
190 2164 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 6843 bool operator==(const FrameTpl<S2, O2> & other) const
202 {
203
1/2
✓ Branch 1 taken 4244 times.
✗ Branch 2 not taken.
 13684 return name == other.name && parentJoint == other.parentJoint
204
2/4
✓ Branch 0 taken 4244 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 4244 times.
✗ Branch 4 not taken.
 6841 && parentFrame == other.parentFrame && placement == other.placement
205
4/6
✓ Branch 0 taken 4244 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 4244 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4244 times.
✗ Branch 6 not taken.
 13684 && 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 2 bool operator!=(const FrameTpl<S2, O2> & other) const
213 {
214 2 return !(*this == other);
215 }
216
217 /// \returns An expression of *this with the Scalar type casted to NewScalar.
218 template<typename NewScalar>
219 2724 FrameTpl<NewScalar, Options> cast() const
220 {
221 typedef FrameTpl<NewScalar, Options> ReturnType;
222 3555 ReturnType res(
223
2/4
✓ Branch 1 taken 2110 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2110 times.
✗ Branch 5 not taken.
 2724 name, parentJoint, parentFrame, placement.template cast<NewScalar>(), type,
224 inertia.template cast<NewScalar>());
225 2724 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 1 inline std::ostream & operator<<(std::ostream & os, const FrameTpl<Scalar, Options> & f)
254 {
255 1 os << "Frame name: " << f.name << " paired to (parent joint/ parent frame)"
256 1 << "(" << f.parentJoint << "/" << f.parentFrame << ")" << std::endl
257 1 << "with relative placement wrt parent joint:\n"
258 1 << f.placement << "containing inertia:\n"
259 1 << f.inertia << std::endl;
260
261 1 return os;
262 }
263
264 } // namespace pinocchio
265
266 #endif // ifndef __pinocchio_multibody_frame_hpp__
267