Branch data Line data Source code
1 : : // Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
2 : : // RDL - Robot Dynamics Library
3 : : // Licensed under the zlib license. See LICENSE for more details.
4 : :
5 : : #ifndef __RDL_REFERENCE_FRAME_HPP__
6 : : #define __RDL_REFERENCE_FRAME_HPP__
7 : :
8 : : /**
9 : : * @file ReferenceFrame.hpp
10 : : */
11 : :
12 : : #include <climits>
13 : : #include <memory>
14 : : #include <string>
15 : : #include <utility>
16 : : #include <vector>
17 : :
18 : : #include "rdl_dynamics/FrameExceptions.hpp"
19 : : #include <rdl_dynamics/rdl_eigenmath.hpp>
20 : :
21 : : namespace RobotDynamics
22 : : {
23 : : /**
24 : : * \page reference_frame_page ReferenceFrame
25 : : * Detailed information on reference frames can be found in the @ref reference_frame "Reference Frame Module", or you can follow
26 : : * these direct links,
27 : : *
28 : : * @li @subpage frame_exceptions An exception that is thrown when reference frame rules are violated
29 : : * @li @subpage frame_object The class inhereted by generally all classes that provide operations involving reference frames
30 : : * @li @subpage frame_orientation Object for expressing an orientation relative to a reference frame
31 : : * @li @subpage frame_point A 3D point expressed in a reference frame
32 : : * @li @subpage frame_vector A 3D vector expressed in a reference frame
33 : : * @li @subpage frame_vector_pair A pair of 3D vectors expressed in a reference frame
34 : : * @li @subpage reference_frame The key class that enables easy frame operations
35 : : * @li @subpage spatial_acceleration 6D acceleration vector
36 : : * @li @subpage spatial_force 6D force vector
37 : : * @li @subpage spatial_momentum 6D momentum, which is also a type of force vector
38 : : * @li @subpage spatial_motion 6D motion vector
39 : : * @li @subpage spatial_inertia A 6D spatial inertia matrix with an associated reference frame
40 : : *
41 : : * @defgroup reference_frame Reference Frame
42 : : * @{
43 : : *
44 : : * The ReferenceFrame object is the foundation of the way kinematics are handled. Each
45 : : * time a body is added via RobotDynamics::Model::addBody(), a pointer to a ReferenceFrame
46 : : * is created for that body and added to either RobotDynamics::Model::bodyFrames or RobotDynamics::Model::fixedBodyFrames
47 : : * depending on the type of body that is added. Additionally a reference frame is placed on the center of mass of each
48 : : * body and is stored in RobotDynamics::Model::bodyCenteredFrames.
49 : : *
50 : : * These reference frames can then be used to explicitly express geometric entities(SpatialMotion, SpatialMomentum,
51 : : **SpatialForce, etc)
52 : : * in a reference frame. To query the name of a reference frame a geometric object is expressed in, you may call the
53 : : **ReferenceFrame::getName()
54 : : * method on the objects ReferenceFrame. Furthermore, to transform a framed geometric entity(anything that inheritcs from
55 : : **the
56 : : * RobotDynamics::FrameObject) into a different reference frame, you may simply call the
57 : : **RobotDynamics::FrameObject::changeFrame
58 : : * method and supply it a pointer to the frame you want the geometric object to be transformed to.
59 : : *
60 : : * \note If you create your own ReferenceFrame outside of those stored in the RobotDynamics::Model, you are required to call
61 : : * RobotDynamics::ReferenceFrame::update every time the created frames' ReferenceFrame::transformFromParent changes.
62 : : * The only exception to this is for RobotDynamics::FixedReferenceFrame objects. By definition, these frames are fixed and
63 : : * thus their ReferenceFrame::transformFromParent is set on construction and never changes, so after construction you do
64 : : * not need to call RobotDynamics::ReferenceFrame::update.
65 : : */
66 : :
67 : : class ReferenceFrame;
68 : : class FixedReferenceFrame;
69 : : typedef std::shared_ptr<ReferenceFrame> ReferenceFramePtr;
70 : : typedef std::shared_ptr<FixedReferenceFrame> FixedReferenceFramePtr;
71 : : typedef std::vector<ReferenceFramePtr> ReferenceFramePtrV;
72 : : typedef std::vector<FixedReferenceFramePtr> FixedReferenceFramePtrV;
73 : : /**
74 : : * @class ReferenceFrame
75 : : * @brief ReferenceFrame object used to tell what frame objects are expressed in. Every ReferenceFrame has a pointer to its
76 : : **parent
77 : : * ReferenceFrame. This parent frame is NOT allowed to be nullptr. The ONLY ReferenceFrame that is allowed to have
78 : : **parentFrame=nullptr is the world frame. There is
79 : : * only one world frame and it can be accessed by the static method ReferenceFrame::getWorldFrame() which will return a
80 : : **shared_ptr to the world frame.
81 : : * This class and its implementation are an adaptation of ReferenceFrame.java by <a href="http://robots.ihmc.us/">Jerry
82 : : **Pratt and the IHMC Robotics Group</a>.
83 : : */
84 : : class ReferenceFrame
85 : : {
86 : : public:
87 : : /**
88 : : * @brief Copy constructor
89 : : * @param referenceFrameToCopy
90 : : */
91 : 1 : ReferenceFrame(const ReferenceFrame& referenceFrameToCopy)
92 : 1 : : frameName(referenceFrameToCopy.frameName)
93 : 1 : , parentFrame(referenceFrameToCopy.parentFrame)
94 : 1 : , transformFromParent(referenceFrameToCopy.transformFromParent)
95 : 1 : , transformToRoot(referenceFrameToCopy.transformToRoot)
96 : 1 : , inverseTransformToRoot(referenceFrameToCopy.inverseTransformToRoot)
97 : 1 : , rootFrame(referenceFrameToCopy.rootFrame)
98 : 1 : , isWorldFrame(referenceFrameToCopy.isWorldFrame)
99 : 1 : , isBodyFrame(referenceFrameToCopy.isBodyFrame)
100 : 1 : , movableBodyId(referenceFrameToCopy.movableBodyId)
101 : : {
102 : 1 : }
103 : :
104 : : /**
105 : : * @brief Constructor
106 : : * @param frameName Name of frame
107 : : * @param parentFrame Pointer to this frames parent ReferenceFrame
108 : : * @param transformFromParent Spatial transform from the parent frames perspective to this frame
109 : : * @param isBodyFrame Boolean true if this frame is a body frame(i.e. will be stored in Model::bodyFrames), false
110 : : **otherwise
111 : : * @param movableBodyId The ID of the movable body this frame is attached to. For frames attached to fixed bodies, this
112 : : **should be FixedBody::mMovableParent
113 : : */
114 : : // cppcheck-suppress passedByValue
115 : 17953 : ReferenceFrame(const std::string& frameName, ReferenceFramePtr parentFrame, const RobotDynamics::Math::SpatialTransform& transformFromParent, bool isBodyFrame,
116 : : unsigned int movableBodyId)
117 : 17953 : : frameName(frameName)
118 : 17953 : , parentFrame(parentFrame)
119 : 17953 : , transformFromParent(transformFromParent)
120 : 17953 : , isWorldFrame(false)
121 : 17953 : , isBodyFrame(isBodyFrame)
122 : 35906 : , movableBodyId(movableBodyId)
123 : : {
124 : 17953 : if (parentFrame == nullptr)
125 : : {
126 : 1 : throw ReferenceFrameException("You are not allowed to create a frame with parentFrame=nullptr. Only a root frame and the world frame may have "
127 : 2 : "parentFrame=nullptr");
128 : : }
129 : :
130 : 17952 : ReferenceFramePtr root = parentFrame;
131 : :
132 : 112032 : while (root->getParentFrame() != nullptr)
133 : : {
134 : 94080 : root = root->getParentFrame();
135 : : }
136 : :
137 : 17952 : rootFrame = root.get();
138 : :
139 : 17952 : update();
140 : 17954 : }
141 : :
142 : : /**
143 : : * @brief Empty constructor. All contained ptrs will be initialize to nullptr
144 : : */
145 : : ReferenceFrame()
146 : : {
147 : : }
148 : :
149 : 16311 : ReferenceFrame* getRootFrame()
150 : : {
151 : 16311 : return rootFrame;
152 : : }
153 : :
154 : : /**
155 : : * @brief Destructor
156 : : */
157 : 23613 : virtual ~ReferenceFrame()
158 : 18039 : {
159 : 23613 : }
160 : :
161 : : /**
162 : : * @brief Recalculates this frames ReferenceFrame::transformToRoot and ReferenceFrame::inverseTransformToRoot which are
163 : : * used by FrameObject::changeFrame to change the ReferenceFrame FrameObjects are expressed in. If
164 : : * you create a ReferenceFrame you MUST call update every tic each time the frames ReferenceFrame::transformFromParent
165 : : * changes. Each time ReferenceFrame::setTransformFromParent changes, you need to call ReferenceFrame::transformFromParent
166 : : * before you call ReferenceFrame::update().
167 : : */
168 : : void update();
169 : :
170 : : /**
171 : : * @brief Get the spatial transform from this frame to desiredFrame and store it in transformToPack
172 : : * @param transformToPack Resulting transform to the desired frame will be stored here
173 : : * @param desiredFrame The resulting transform will transform vectors into desiredFrame
174 : : */
175 : : // cppcheck-suppress passedByValue
176 : : inline void getTransformToDesiredFrame(RobotDynamics::Math::SpatialTransform& transformToPack, ReferenceFramePtr desiredFrame)
177 : : {
178 : : transformToPack = getTransformToDesiredFrame(desiredFrame);
179 : : }
180 : :
181 : : /**
182 : : * @brief Get the spatial transform from this frame to desiredFrame and store it in transformToPack
183 : : * @param desiredFrame The resulting transform will transform vectors into desiredFrame
184 : : * @return Spatial transform that will transform vectors into the ReferenceFrame desiredFrame
185 : : */
186 : : virtual RobotDynamics::Math::SpatialTransform getTransformToDesiredFrame(ReferenceFramePtr desiredFrame);
187 : :
188 : : /**
189 : : * @brief Check if two frames have the same roots
190 : : * @throws ReferenceFrameException if the frames do not have the same roots
191 : : * @param frame
192 : : */
193 : : void verifyFramesHaveSameRoot(ReferenceFramePtr frame);
194 : :
195 : : /**
196 : : * @brief Set a frames ReferenceFrame::transformToParent. For frames connected by a joint, this needs to be
197 : : * updated every tic BEFORE calling the ReferenceFrame::update method
198 : : * @param transformFromParent The new ReferenceFrame::transformFromParent
199 : : */
200 : 13981 : inline void setTransformFromParent(const RobotDynamics::Math::SpatialTransform& transformFromParent)
201 : : {
202 : 13981 : this->transformFromParent = transformFromParent;
203 : 13981 : }
204 : :
205 : : /**
206 : : * @brief Check if the argument ReferenceFrame equals this
207 : : * @param referenceFrame
208 : : */
209 : : void checkReferenceFramesMatch(ReferenceFramePtr referenceFrame) const;
210 : :
211 : : void checkReferenceFramesMatch(ReferenceFrame* referenceFrame) const;
212 : :
213 : : /**
214 : : * @brief Get this frames ReferenceFrame::transformToRoot
215 : : * @return ReferenceFrame::transformToRoot
216 : : */
217 : 29423 : virtual RobotDynamics::Math::SpatialTransform getTransformToRoot()
218 : : {
219 : 29423 : return this->transformToRoot;
220 : : }
221 : :
222 : : /**
223 : : * @brief Get this frames ReferenceFrame::inverseTransformToRoot
224 : : * @return ReferenceFrame::inverseTransformToRoot
225 : : */
226 : 50113 : virtual RobotDynamics::Math::SpatialTransform getInverseTransformToRoot()
227 : : {
228 : 50113 : return this->inverseTransformToRoot;
229 : : }
230 : :
231 : : /**
232 : : * @brief get a pointer to this frames parent
233 : : * @return ReferenceFrame::parentFrame
234 : : */
235 : 206156 : inline ReferenceFramePtr getParentFrame()
236 : : {
237 : 206156 : return this->parentFrame;
238 : : }
239 : :
240 : : /**
241 : : * @brief Get the frame name
242 : : * @return ReferenceFrame::framenName
243 : : */
244 : 708 : inline std::string getName() const
245 : : {
246 : 708 : return this->frameName;
247 : : }
248 : :
249 : : /**
250 : : * @brief Creates a root frame with ReferenceFrame::parentFrame=nullptr
251 : : * @param frameName
252 : : * @return pointer to the created root frame
253 : : */
254 : 46 : static ReferenceFramePtr createARootFrame(const std::string& frameName)
255 : : {
256 : 46 : return ReferenceFramePtr(new ReferenceFrame(frameName, false, 0, true));
257 : : }
258 : :
259 : : /**
260 : : * @brief Get a pointer to the world frame
261 : : * @return Pointer to world frame
262 : : */
263 : 12127 : static ReferenceFramePtr getWorldFrame()
264 : : {
265 : 12127 : return worldFrame;
266 : : }
267 : :
268 : : /**
269 : : * @brief Get spatial transform from parent to this frame
270 : : * @return SpatialTransform from parent to this frame
271 : : */
272 : 27362 : inline RobotDynamics::Math::SpatialTransform getTransformFromParent()
273 : : {
274 : 27362 : return this->transformFromParent;
275 : : }
276 : :
277 : : /**
278 : : * @brief Get spatial transform this frame to its parent
279 : : * @return SpatialTransform from this frame to parent
280 : : */
281 : 19868 : inline RobotDynamics::Math::SpatialTransform getTransformToParent()
282 : : {
283 : 19868 : return this->transformFromParent.inverse();
284 : : }
285 : :
286 : : /**
287 : : * @brief Get a boolean telling if this frame is the world frame
288 : : * @return Boolean true if this frame is world frame, false otherwise
289 : : */
290 : 2 : inline bool getIsWorldFrame() const
291 : : {
292 : 2 : return this->isWorldFrame;
293 : : }
294 : :
295 : : /**
296 : : * @brief Get the ID of the movable body this frame is attached to
297 : : * @return Unsigned int corresponding to the movable body this frame is attached to
298 : : */
299 : 11532 : inline unsigned int getMovableBodyId() const
300 : : {
301 : 11532 : return this->movableBodyId;
302 : : }
303 : :
304 : : /**
305 : : * @brief Get boolean telling if this frame is a body frame or not. If it is a body frame, A pointer
306 : : * to this frame would be stored in Model::bodyFrames vector
307 : : * @return Boolean true if this frame is a body frame, false otherwise
308 : : */
309 : 311 : inline bool getIsBodyFrame() const
310 : : {
311 : 311 : return this->isBodyFrame;
312 : : }
313 : :
314 : : ReferenceFrame& operator=(const ReferenceFrame& other)
315 : : {
316 : : worldFrame = other.worldFrame;
317 : : frameName = other.frameName;
318 : : parentFrame = other.parentFrame;
319 : : transformFromParent = other.transformFromParent;
320 : : transformToRoot = other.transformToRoot;
321 : : inverseTransformToRoot = other.inverseTransformToRoot;
322 : : isWorldFrame = other.isWorldFrame;
323 : : isBodyFrame = other.isBodyFrame;
324 : : movableBodyId = other.movableBodyId;
325 : : rootFrame = other.rootFrame;
326 : :
327 : : return *this;
328 : : }
329 : :
330 : : protected:
331 : : /**
332 : : * Constructor that creates a top level frame with parent=nullptr, and transforms to root being identity transform.
333 : : * @param frameName
334 : : * @param isWorldFrame True of creating the world frame, false if it's just a root frame
335 : : * @param movableBodyId
336 : : * @param isBodyFrame
337 : : */
338 : 87 : ReferenceFrame(const std::string& frameName, bool isWorldFrame, unsigned int movableBodyId, bool isBodyFrame)
339 : 87 : : frameName(frameName), parentFrame(nullptr), isWorldFrame(isWorldFrame), isBodyFrame(isBodyFrame), movableBodyId(movableBodyId)
340 : : {
341 : 87 : rootFrame = this;
342 : 87 : update();
343 : 87 : }
344 : :
345 : : /**
346 : : * @brief Helper method to create a world frame
347 : : * @param frameName
348 : : * @return Pointer to the created world frame
349 : : */
350 : 41 : static ReferenceFramePtr createAWorldFrame(const std::string& frameName = "world")
351 : : {
352 : 41 : return ReferenceFramePtr(new ReferenceFrame(frameName, true, 0, true));
353 : : }
354 : :
355 : : static ReferenceFramePtr worldFrame; /**< Static world frame pointer */
356 : : std::string frameName; /**< A frames name */
357 : : ReferenceFramePtr parentFrame; /**< Pointer to a frames parent frames */
358 : : RobotDynamics::Math::SpatialTransform transformFromParent; /**< SpatialTransform to a frame from its parent*/
359 : : RobotDynamics::Math::SpatialTransform transformToRoot; /**< SpatialTransform from a frame to the root frame
360 : : */
361 : : RobotDynamics::Math::SpatialTransform inverseTransformToRoot; /**< SpatialTransform to a frame from the root frame
362 : : */
363 : :
364 : : ReferenceFrame* rootFrame;
365 : : bool isWorldFrame; /**< True if a frame is the world frame, false
366 : : otherwise */
367 : : bool isBodyFrame; /**< True if a frame is a body frame, false
368 : : otherwise. Body frame pointers are stored in
369 : : Model::bodyFrames */
370 : : unsigned int movableBodyId; /**< The body ID of the movable body a frame is
371 : : attached to */
372 : : };
373 : :
374 : : class FixedReferenceFrame : public ReferenceFrame
375 : : {
376 : : FixedReferenceFrame(const FixedReferenceFrame&) = delete;
377 : : void operator=(const FixedReferenceFrame&) = delete;
378 : :
379 : : public:
380 : : // cppcheck-suppress passedByValue
381 : 12464 : FixedReferenceFrame(const std::string& frameName, ReferenceFramePtr parentFrame, const RobotDynamics::Math::SpatialTransform& transformFromParent,
382 : : unsigned int movableBodyId)
383 : 12464 : : ReferenceFrame(frameName, parentFrame, transformFromParent, false, movableBodyId)
384 : : {
385 : 12464 : }
386 : :
387 : 24928 : virtual ~FixedReferenceFrame()
388 : 12464 : {
389 : 24928 : }
390 : :
391 : 376 : RobotDynamics::Math::SpatialTransform getTransformToRoot() override
392 : : {
393 : 376 : transformToRoot = parentFrame->getTransformToRoot() * transformFromParent.inverse();
394 : 376 : return transformToRoot;
395 : : }
396 : :
397 : : /**
398 : : * @brief Get this frames ReferenceFrame::inverseTransformToRoot
399 : : * @return ReferenceFrame::inverseTransformToRoot
400 : : */
401 : 2343 : RobotDynamics::Math::SpatialTransform getInverseTransformToRoot() override
402 : : {
403 : 2343 : inverseTransformToRoot = transformFromParent * parentFrame->getInverseTransformToRoot();
404 : 2343 : return inverseTransformToRoot;
405 : : }
406 : : };
407 : : } // namespace RobotDynamics
408 : :
409 : : #endif // ifndef __RDL_REFERENCE_FRAME_HPP__
|
