DART  6.7.3
Frame.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2019, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_DYNAMICS_FRAME_HPP_
34 #define DART_DYNAMICS_FRAME_HPP_
35 
36 #include <set>
37 
38 #include <Eigen/Geometry>
39 
40 #include "dart/dynamics/Entity.hpp"
41 #include "dart/math/MathTypes.hpp"
42 
43 namespace dart {
44 namespace dynamics {
45 
56 class Frame : public virtual Entity
57 {
58 public:
59  friend class Entity;
60  friend class WorldFrame;
61  friend class ShapeFrame;
62 
63  Frame(const Frame&) = delete;
64 
66  virtual ~Frame();
67 
68  static Frame* World();
69 
70  //--------------------------------------------------------------------------
71  // Transform
72  //--------------------------------------------------------------------------
73 
75  virtual const Eigen::Isometry3d& getRelativeTransform() const = 0;
76 
78  const Eigen::Isometry3d& getWorldTransform() const;
79 
81  Eigen::Isometry3d getTransform(
82  const Frame* _withRespectTo = Frame::World()) const;
83 
86  Eigen::Isometry3d getTransform(const Frame* withRespectTo,
87  const Frame* inCoordinatesOf) const;
88 
89  //-------------------------------------------------------------------------
90  // Velocity
91  //-------------------------------------------------------------------------
92 
95  virtual const Eigen::Vector6d& getRelativeSpatialVelocity() const = 0;
96 
99  const Eigen::Vector6d& getSpatialVelocity() const;
100 
103  Eigen::Vector6d getSpatialVelocity(const Frame* _relativeTo,
104  const Frame* _inCoordinatesOf) const;
105 
108  Eigen::Vector6d getSpatialVelocity(const Eigen::Vector3d& _offset) const;
109 
111  Eigen::Vector6d getSpatialVelocity(const Eigen::Vector3d& _offset,
112  const Frame* _relativeTo,
113  const Frame* _inCoordinatesOf) const;
114 
117  Eigen::Vector3d getLinearVelocity(
118  const Frame* _relativeTo = Frame::World(),
119  const Frame* _inCoordinatesOf = Frame::World()) const;
120 
124  Eigen::Vector3d getLinearVelocity(
125  const Eigen::Vector3d& _offset,
126  const Frame* _relativeTo = Frame::World(),
127  const Frame* _inCoordinatesOf = Frame::World()) const;
128 
131  Eigen::Vector3d getAngularVelocity(
132  const Frame* _relativeTo = Frame::World(),
133  const Frame* _inCoordinatesOf = Frame::World()) const;
134 
135  //--------------------------------------------------------------------------
136  // Acceleration
137  //--------------------------------------------------------------------------
138 
142 
154 
158  virtual const Eigen::Vector6d& getPartialAcceleration() const = 0;
159 
163 
166  Eigen::Vector6d getSpatialAcceleration(const Frame* _relativeTo,
167  const Frame* _inCoordinatesOf) const;
168 
172  Eigen::Vector6d getSpatialAcceleration(const Eigen::Vector3d& _offset) const;
173 
175  Eigen::Vector6d getSpatialAcceleration(const Eigen::Vector3d& _offset,
176  const Frame* _relativeTo,
177  const Frame* _inCoordinatesOf) const;
178 
181  Eigen::Vector3d getLinearAcceleration(
182  const Frame* _relativeTo=Frame::World(),
183  const Frame* _inCoordinatesOf=Frame::World()) const;
184 
185  Eigen::Vector3d getLinearAcceleration(
186  const Eigen::Vector3d& _offset,
187  const Frame* _relativeTo=Frame::World(),
188  const Frame* _inCoordinatesOf=Frame::World()) const;
189 
192  Eigen::Vector3d getAngularAcceleration(
193  const Frame* _relativeTo=Frame::World(),
194  const Frame* _inCoordinatesOf=Frame::World()) const;
195 
196  //--------------------------------------------------------------------------
197  // Relationships
198  //--------------------------------------------------------------------------
199 
204  const std::set<Entity*>& getChildEntities();
205 
210  const std::set<const Entity*> getChildEntities() const;
211 
213  std::size_t getNumChildEntities() const;
214 
218  const std::set<Frame*>& getChildFrames();
219 
223  std::set<const Frame*> getChildFrames() const;
224 
226  std::size_t getNumChildFrames() const;
227 
229  bool isShapeFrame() const;
230 
233  virtual ShapeFrame* asShapeFrame();
234 
237  virtual const ShapeFrame* asShapeFrame() const;
238 
240  bool isWorld() const;
241 
244  virtual void dirtyTransform() override;
245 
247  virtual void dirtyVelocity() override;
248 
251  virtual void dirtyAcceleration() override;
252 
253 protected:
254 
258 
260  explicit Frame(Frame* _refFrame);
261 
263  Frame();
264 
266  explicit Frame(ConstructAbstractTag);
267 
268  // Documentation inherited
269  virtual void changeParentFrame(Frame* _newParentFrame) override;
270 
274  virtual void processNewEntity(Entity* _newChildEntity);
275 
278  virtual void processRemovedEntity(Entity* _oldChildEntity);
279 
280 private:
281 
284 
286  explicit Frame(ConstructWorldTag);
287 
288 protected:
293  mutable Eigen::Isometry3d mWorldTransform;
294 
299 
304 
306  std::set<Frame*> mChildFrames;
307 
309  std::set<Entity*> mChildEntities;
310 
311 private:
313  const bool mAmWorld;
314 
317 
318 public:
319  // To get byte-aligned Eigen vectors
320  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
321 };
322 
327 class WorldFrame : public Frame
328 {
329 public:
330  friend class Frame;
331 
333  const Eigen::Isometry3d& getRelativeTransform() const override final;
334 
336  const Eigen::Vector6d& getRelativeSpatialVelocity() const override final;
337 
339  const Eigen::Vector6d& getRelativeSpatialAcceleration() const override final;
340 
342  const Eigen::Vector6d& getPrimaryRelativeAcceleration() const override final;
343 
345  const Eigen::Vector6d& getPartialAcceleration() const override final;
346 
347  const std::string& setName(const std::string& name) override final;
348 
349  const std::string& getName() const override final;
350 
351 private:
353  explicit WorldFrame();
354 
355 private:
357  const Eigen::Isometry3d mRelativeTf;
358 
360  static const Eigen::Vector6d mZero;
361 
362 public:
363  // To get byte-aligned Eigen vectors
364  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
365 };
366 
367 } // namespace dynamics
368 } // namespace dart
369 
370 #endif // DART_DYNAMICS_FRAME_HPP_
std::string * name
Definition: SkelParser.cpp:1642
Entity class is a base class for any objects that exist in the kinematic tree structure of DART.
Definition: Entity.hpp:60
ConstructAbstractTag
Used when constructing a pure abstract class, because calling the Entity constructor is just a formal...
Definition: Entity.hpp:163
The Frame class serves as the backbone of DART's kinematic tree structure.
Definition: Frame.hpp:57
std::set< Entity * > mChildEntities
Container of this Frame's child Entities.
Definition: Frame.hpp:309
bool isWorld() const
Returns true if this Frame is the World Frame.
Definition: Frame.cpp:445
std::size_t getNumChildEntities() const
Get the number of Entities that are currently children of this Frame.
Definition: Frame.cpp:403
virtual ShapeFrame * asShapeFrame()
Convert 'this' into a ShapeFrame pointer if Frame is a ShapeFrame, otherwise return nullptr.
Definition: Frame.cpp:433
const std::set< Entity * > & getChildEntities()
Get a container with the Entities that are children of this Frame.
Definition: Frame.cpp:391
bool mAmShapeFrame
Contains whether or not this is a ShapeFrame.
Definition: Frame.hpp:316
virtual void changeParentFrame(Frame *_newParentFrame) override
Used by derived classes to change their parent frames.
Definition: Frame.cpp:538
virtual void processNewEntity(Entity *_newChildEntity)
Called during a parent Frame change to allow extensions of the Frame class to handle new children in ...
Definition: Frame.cpp:579
virtual const Eigen::Vector6d & getPartialAcceleration() const =0
The Featherstone ABI algorithm exploits a component of the spatial acceleration which we refer to as ...
Eigen::Vector3d getAngularAcceleration(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const
Get the angular portion of classical acceleration of this Frame relative to some other Frame.
Definition: Frame.cpp:373
ConstructAbstractTag
Used when constructing a pure abstract class, because calling the Frame constructor is just a formali...
Definition: Frame.hpp:257
@ ConstructAbstract
Definition: Frame.hpp:257
const bool mAmWorld
Contains whether or not this is the World Frame.
Definition: Frame.hpp:313
Eigen::Isometry3d mWorldTransform
World transform of this Frame.
Definition: Frame.hpp:293
Eigen::Vector3d getLinearVelocity(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const
Get the linear portion of classical velocity of this Frame relative to some other Frame.
Definition: Frame.cpp:208
virtual const Eigen::Vector6d & getRelativeSpatialVelocity() const =0
Get the spatial velocity of this Frame relative to its parent Frame, in its own coordinates.
std::set< Frame * > mChildFrames
Container of this Frame's child Frames.
Definition: Frame.hpp:306
Eigen::Isometry3d getTransform(const Frame *_withRespectTo=Frame::World()) const
Get the transform of this Frame with respect to some other Frame.
Definition: Frame.cpp:94
virtual void dirtyVelocity() override
Notify the velocity updates of this Frame and all its children are needed.
Definition: Frame.cpp:470
static Frame * World()
Definition: Frame.cpp:72
virtual void processRemovedEntity(Entity *_oldChildEntity)
Called when a child Entity is removed from its parent Frame.
Definition: Frame.cpp:585
std::size_t getNumChildFrames() const
Get the number of Frames that are currently children of this Frame.
Definition: Frame.cpp:421
const std::set< Frame * > & getChildFrames()
Get a container with the Frames that are children of this Frame.
Definition: Frame.cpp:409
ConstructWorldTag
Used when constructing the World.
Definition: Frame.hpp:283
@ ConstructWorld
Definition: Frame.hpp:283
virtual const Eigen::Vector6d & getPrimaryRelativeAcceleration() const =0
The Featherstone ABI algorithm exploits a component of the spatial acceleration which we refer to as ...
const Eigen::Vector6d & getSpatialVelocity() const
Get the total spatial velocity of this Frame in the coordinates of this Frame.
Definition: Frame.cpp:125
const Eigen::Isometry3d & getWorldTransform() const
Get the transform of this Frame with respect to the World Frame.
Definition: Frame.cpp:79
Eigen::Vector3d getLinearAcceleration(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const
Get the linear portion of classical acceleration of this Frame relative to some other Frame.
Definition: Frame.cpp:334
Eigen::Vector6d mVelocity
Total velocity of this Frame, in the coordinates of this Frame.
Definition: Frame.hpp:298
Frame()
Default constructor, delegates to Frame(ConstructAbstract_t)
Definition: Frame.cpp:519
Eigen::Vector3d getAngularVelocity(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const
Get the angular portion of classical velocity of this Frame relative to some other Frame.
Definition: Frame.cpp:223
bool isShapeFrame() const
Returns true if this Frame is a ShapeFrame.
Definition: Frame.cpp:427
Frame(const Frame &)=delete
const Eigen::Vector6d & getSpatialAcceleration() const
Get the total spatial acceleration of this Frame in the coordinates of this Frame.
Definition: Frame.cpp:230
virtual const Eigen::Vector6d & getRelativeSpatialAcceleration() const =0
Get the spatial acceleration of this Frame relative to its parent Frame, in the coordinates of this F...
virtual const Eigen::Isometry3d & getRelativeTransform() const =0
Get the transform of this Frame with respect to its parent Frame.
virtual ~Frame()
Destructor.
Definition: Frame.cpp:45
Eigen::Vector6d mAcceleration
Total acceleration of this Frame, in the coordinates of this Frame.
Definition: Frame.hpp:303
virtual void dirtyAcceleration() override
Notify the acceleration updates of this Frame and all its children are needed.
Definition: Frame.cpp:489
virtual void dirtyTransform() override
Notify the transformation updates of this Frame and all its children are needed.
Definition: Frame.cpp:451
Definition: ShapeFrame.hpp:164
The WorldFrame class is a class that is used internally to create the singleton World Frame.
Definition: Frame.hpp:328
const Eigen::Isometry3d mRelativeTf
This is set to Identity and never changes.
Definition: Frame.hpp:357
const Eigen::Vector6d & getRelativeSpatialVelocity() const override final
Always returns a zero vector.
Definition: Frame.cpp:612
const std::string & getName() const override final
Return the name of this Entity.
Definition: Frame.cpp:645
const Eigen::Isometry3d & getRelativeTransform() const override final
Always returns the Identity Transform.
Definition: Frame.cpp:606
const Eigen::Vector6d & getRelativeSpatialAcceleration() const override final
Always returns a zero vector.
Definition: Frame.cpp:618
static const Eigen::Vector6d mZero
This is set to a Zero vector and never changes.
Definition: Frame.hpp:360
const Eigen::Vector6d & getPartialAcceleration() const override final
Always returns a zero vector.
Definition: Frame.cpp:630
const Eigen::Vector6d & getPrimaryRelativeAcceleration() const override final
Always returns a zero vector.
Definition: Frame.cpp:624
const std::string & setName(const std::string &name) override final
Set name.
Definition: Frame.cpp:636
Definition: Random-impl.hpp:92
Matrix< double, 6, 1 > Vector6d
Definition: MathTypes.hpp:49
Definition: BulletCollisionDetector.cpp:63
Definition: SharedLibraryManager.hpp:43