DART  6.10.1
World.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, 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 code incorporates portions of Open Dynamics Engine
19  * (Copyright (c) 2001-2004, Russell L. Smith. All rights
20  * reserved.) and portions of FCL (Copyright (c) 2011, Willow
21  * Garage, Inc. All rights reserved.), which were released under
22  * the same BSD license as below
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
25  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
26  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
27  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
28  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
29  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
30  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
31  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
32  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
33  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  */
38 
39 #ifndef DART_SIMULATION_WORLD_HPP_
40 #define DART_SIMULATION_WORLD_HPP_
41 
42 #include <set>
43 #include <string>
44 #include <vector>
45 
46 #include <Eigen/Dense>
47 
51 #include "dart/common/Subject.hpp"
52 #include "dart/common/Timer.hpp"
58 
59 namespace dart {
60 
61 namespace integration {
62 class Integrator;
63 } // namespace integration
64 
65 namespace dynamics {
66 class Skeleton;
67 } // namespace dynamics
68 
69 namespace constraint {
70 class ConstraintSolver;
71 } // namespace constraint
72 
73 namespace collision {
74 class CollisionResult;
75 } // namespace collision
76 
77 namespace simulation {
78 
80 
83 class World : public virtual common::Subject
84 {
85 public:
87  const std::string& _oldName, const std::string& _newName)>;
88 
90  template <typename... Args>
91  static WorldPtr create(Args&&... args);
92 
93  //--------------------------------------------------------------------------
94  // Constructor and Destructor
95  //--------------------------------------------------------------------------
96 
98  static std::shared_ptr<World> create(const std::string& name = "world");
99 
101  World(const std::string& _name = "world");
102 
104  virtual ~World();
105 
108  std::shared_ptr<World> clone() const;
109 
110  //--------------------------------------------------------------------------
111  // Properties
112  //--------------------------------------------------------------------------
113 
115  const std::string& setName(const std::string& _newName);
116 
118  const std::string& getName() const;
119 
121  void setGravity(const Eigen::Vector3d& _gravity);
122 
124  const Eigen::Vector3d& getGravity() const;
125 
127  void setTimeStep(double _timeStep);
128 
130  double getTimeStep() const;
131 
132  //--------------------------------------------------------------------------
133  // Structural Properties
134  //--------------------------------------------------------------------------
135 
137  dynamics::SkeletonPtr getSkeleton(std::size_t _index) const;
138 
142  dynamics::SkeletonPtr getSkeleton(const std::string& _name) const;
143 
145  std::size_t getNumSkeletons() const;
146 
148  std::string addSkeleton(const dynamics::SkeletonPtr& _skeleton);
149 
151  void removeSkeleton(const dynamics::SkeletonPtr& _skeleton);
152 
155  std::set<dynamics::SkeletonPtr> removeAllSkeletons();
156 
158  bool hasSkeleton(const dynamics::ConstSkeletonPtr& skeleton) const;
159 
161  bool hasSkeleton(const std::string& skeletonName) const;
162 
164  int getIndex(int _index) const;
165 
167  dynamics::SimpleFramePtr getSimpleFrame(std::size_t _index) const;
168 
170  dynamics::SimpleFramePtr getSimpleFrame(const std::string& _name) const;
171 
173  std::size_t getNumSimpleFrames() const;
174 
176  std::string addSimpleFrame(const dynamics::SimpleFramePtr& _frame);
177 
179  void removeSimpleFrame(const dynamics::SimpleFramePtr& _frame);
180 
183  std::set<dynamics::SimpleFramePtr> removeAllSimpleFrames();
184 
185  //--------------------------------------------------------------------------
186  // Collision checking
187  //--------------------------------------------------------------------------
188 
190  DART_DEPRECATED(6.0)
191  bool checkCollision(bool checkAllCollisions);
192 
198  bool checkCollision(
199  const collision::CollisionOption& option
200  = collision::CollisionOption(false, 1u, nullptr),
201  collision::CollisionResult* result = nullptr);
202 
207  const collision::CollisionResult& getLastCollisionResult() const;
208 
209  //--------------------------------------------------------------------------
210  // Simulation
211  //--------------------------------------------------------------------------
212 
214  void reset();
215 
219  void step(bool _resetCommand = true);
220 
222  void setTime(double _time);
223 
225  double getTime() const;
226 
231  int getSimFrames() const;
232 
233  //--------------------------------------------------------------------------
234  // Constraint
235  //--------------------------------------------------------------------------
236 
241  void setConstraintSolver(constraint::UniqueConstraintSolverPtr solver);
242 
244  constraint::ConstraintSolver* getConstraintSolver();
245 
247  const constraint::ConstraintSolver* getConstraintSolver() const;
248 
250  void bake();
251 
254 
255 protected:
258  const dynamics::ConstMetaSkeletonPtr& _skeleton);
259 
261  void handleSimpleFrameNameChange(const dynamics::Entity* _entity);
262 
264  std::string mName;
265 
267  std::vector<dynamics::SkeletonPtr> mSkeletons;
268 
269  std::map<dynamics::ConstMetaSkeletonPtr, dynamics::SkeletonPtr>
271 
274  std::vector<common::Connection> mNameConnectionsForSkeletons;
275 
277  dart::common::NameManager<dynamics::SkeletonPtr> mNameMgrForSkeletons;
278 
280  std::vector<dynamics::SimpleFramePtr> mSimpleFrames;
281 
284  std::vector<common::Connection> mNameConnectionsForSimpleFrames;
285 
287  std::map<const dynamics::SimpleFrame*, dynamics::SimpleFramePtr>
289 
291  dart::common::NameManager<dynamics::SimpleFramePtr> mNameMgrForSimpleFrames;
292 
297  std::vector<int> mIndices;
298 
300  Eigen::Vector3d mGravity;
301 
303  double mTimeStep;
304 
306  double mTime;
307 
309  int mFrame;
310 
312  std::unique_ptr<constraint::ConstraintSolver> mConstraintSolver;
313 
316 
317  //--------------------------------------------------------------------------
318  // Signals
319  //--------------------------------------------------------------------------
321 
322 public:
323  //--------------------------------------------------------------------------
324  // Slot registers
325  //--------------------------------------------------------------------------
326  common::SlotRegister<NameChangedSignal> onNameChanged;
327 };
329 
330 } // namespace simulation
331 } // namespace dart
332 
333 #include "dart/simulation/detail/World-impl.hpp"
334 
335 #endif // DART_SIMULATION_WORLD_HPP_
#define DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_END
Definition: ClassWithVirtualBase.hpp:44
#define DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_BEGIN
Definition: ClassWithVirtualBase.hpp:43
#define DART_DEPRECATED(version)
Definition: Deprecated.hpp:51
const CollisionOption & option
Collision option of DART.
Definition: FCLCollisionDetector.cpp:157
CollisionResult * result
Collision result of DART.
Definition: FCLCollisionDetector.cpp:160
std::string * name
Definition: SkelParser.cpp:1697
The Subject class is a base class for any object that wants to report when it gets destroyed.
Definition: Subject.hpp:58
class Recording
Definition: Recording.hpp:58
class World
Definition: World.hpp:84
virtual ~World()
Destructor.
Definition: World.cpp:80
std::size_t getNumSkeletons() const
Get the number of skeletons.
Definition: World.cpp:283
std::map< dynamics::ConstMetaSkeletonPtr, dynamics::SkeletonPtr > mMapForSkeletons
Definition: World.hpp:270
std::unique_ptr< constraint::ConstraintSolver > mConstraintSolver
Constraint solver.
Definition: World.hpp:312
int getIndex(int _index) const
Get the dof index for the indexed skeleton.
Definition: World.cpp:418
const std::string & getName() const
Get the name of this World.
Definition: World.cpp:244
double getTimeStep() const
Get time step.
Definition: World.cpp:152
void handleSimpleFrameNameChange(const dynamics::Entity *_entity)
Register when a SimpleFrame's name is changed.
Definition: World.cpp:666
std::string mName
Name of this World.
Definition: World.hpp:264
dart::common::NameManager< dynamics::SkeletonPtr > mNameMgrForSkeletons
NameManager for keeping track of Skeletons.
Definition: World.hpp:277
std::map< const dynamics::SimpleFrame *, dynamics::SimpleFramePtr > mSimpleFrameToShared
Map from raw SimpleFrame pointers to their shared_ptrs.
Definition: World.hpp:288
void setTime(double _time)
Set current time.
Definition: World.cpp:209
std::set< dynamics::SkeletonPtr > removeAllSkeletons()
Remove all the skeletons in this world, and return a set of shared pointers to them,...
Definition: World.cpp:389
void step(bool _resetCommand=true)
Calculate the dynamics and integrate the world for one step.
Definition: World.cpp:167
void setConstraintSolver(constraint::UniqueConstraintSolverPtr solver)
Sets the constraint solver.
Definition: World.cpp:557
Eigen::Vector3d mGravity
Gravity.
Definition: World.hpp:300
std::vector< dynamics::SkeletonPtr > mSkeletons
Skeletons in this world.
Definition: World.hpp:267
void removeSkeleton(const dynamics::SkeletonPtr &_skeleton)
Remove a skeleton from this world.
Definition: World.cpp:330
common::SlotRegister< NameChangedSignal > onNameChanged
Definition: World.hpp:326
const collision::CollisionResult & getLastCollisionResult() const
Return the collision checking result of the last simulation step.
Definition: World.cpp:551
std::size_t getNumSimpleFrames() const
Get the number of Entities.
Definition: World.cpp:439
std::vector< dynamics::SimpleFramePtr > mSimpleFrames
Entities in this world.
Definition: World.hpp:280
void setTimeStep(double _timeStep)
Set time step.
Definition: World.cpp:135
double mTimeStep
Simulation time step.
Definition: World.hpp:303
std::vector< int > mIndices
The first indeices of each skeleton's dof in mDofs.
Definition: World.hpp:297
void bake()
Bake simulated current state and store it into mRecording.
Definition: World.cpp:586
std::string addSimpleFrame(const dynamics::SimpleFramePtr &_frame)
Add an Entity to this world.
Definition: World.cpp:445
void removeSimpleFrame(const dynamics::SimpleFramePtr &_frame)
Remove a SimpleFrame from this world.
Definition: World.cpp:479
int getSimFrames() const
Get the number of simulated frames.
Definition: World.cpp:221
const std::string & setName(const std::string &_newName)
Set the name of this World.
Definition: World.cpp:227
void reset()
Reset the time, frame counter and recorded histories.
Definition: World.cpp:158
std::vector< common::Connection > mNameConnectionsForSimpleFrames
Connections for noticing changes in Frame names TODO(MXG): Consider putting this functionality into N...
Definition: World.hpp:284
int mFrame
Current simulation frame number.
Definition: World.hpp:309
NameChangedSignal mNameChangedSignal
Definition: World.hpp:320
bool checkCollision(bool checkAllCollisions)
Deprecated. Please use checkCollision(~) instead.
Definition: World.cpp:530
const Eigen::Vector3d & getGravity() const
Get gravity.
Definition: World.cpp:262
void setGravity(const Eigen::Vector3d &_gravity)
Set gravity.
Definition: World.cpp:250
constraint::ConstraintSolver * getConstraintSolver()
Get the constraint solver.
Definition: World.cpp:574
std::shared_ptr< World > clone() const
Create a clone of this World.
Definition: World.cpp:92
World(const std::string &_name="world")
Constructor.
Definition: World.cpp:62
void handleSkeletonNameChange(const dynamics::ConstMetaSkeletonPtr &_skeleton)
Register when a Skeleton's name is changed.
Definition: World.cpp:616
dynamics::SkeletonPtr getSkeleton(std::size_t _index) const
Get the indexed skeleton.
Definition: World.cpp:268
std::set< dynamics::SimpleFramePtr > removeAllSimpleFrames()
Remove all SimpleFrames in this world, and return a set of shared pointers to them,...
Definition: World.cpp:513
dart::common::NameManager< dynamics::SimpleFramePtr > mNameMgrForSimpleFrames
NameManager for keeping track of Entities.
Definition: World.hpp:291
std::string addSkeleton(const dynamics::SkeletonPtr &_skeleton)
Add a skeleton to this world.
Definition: World.cpp:289
bool hasSkeleton(const dynamics::ConstSkeletonPtr &skeleton) const
Returns whether this World contains a Skeleton.
Definition: World.cpp:405
Recording * getRecording()
Get recording.
Definition: World.cpp:610
std::vector< common::Connection > mNameConnectionsForSkeletons
Connections for noticing changes in Skeleton names TODO(MXG): Consider putting this functionality int...
Definition: World.hpp:274
static WorldPtr create(Args &&... args)
Creates World as shared_ptr.
Definition: World-impl.hpp:49
Recording * mRecording
Definition: World.hpp:315
double getTime() const
Get current time.
Definition: World.cpp:215
dynamics::SimpleFramePtr getSimpleFrame(std::size_t _index) const
Get the indexed Entity.
Definition: World.cpp:424
double mTime
Current simulation time.
Definition: World.hpp:306
#define DART_COMMON_DECLARE_SHARED_WEAK(X)
Definition: SmartPointer.hpp:41
Definition: Random-impl.hpp:92
::fcl::CollisionResult CollisionResult
Definition: BackwardCompatibility.hpp:166
std::unique_ptr< ConstraintSolver > UniqueConstraintSolverPtr
Definition: SmartPointer.hpp:41
std::shared_ptr< const Skeleton > ConstSkeletonPtr
Definition: SmartPointer.hpp:60
std::shared_ptr< const MetaSkeleton > ConstMetaSkeletonPtr
Definition: SmartPointer.hpp:68
std::shared_ptr< SimpleFrame > SimpleFramePtr
Definition: SmartPointer.hpp:53
std::shared_ptr< Skeleton > SkeletonPtr
Definition: SmartPointer.hpp:60
std::shared_ptr< World > WorldPtr
Definition: SmartPointer.hpp:41
Integrator
This attribute selects the numerical integrator to be used.
Definition: Types.hpp:72
Definition: BulletCollisionDetector.cpp:65
Definition: SharedLibraryManager.hpp:46