Choreonoid  1.5
DyWorld.h
Go to the documentation of this file.
1 
6 #ifndef CNOID_BODY_DYWORLD_H
7 #define CNOID_BODY_DYWORLD_H
8 
9 #include "ForwardDynamics.h"
10 #include <cnoid/TimeMeasure>
11 #include <map>
12 #include "exportdecl.h"
13 
14 namespace cnoid {
15 
16 class DyLink;
17 class DyBody;
18 typedef ref_ptr<DyBody> DyBodyPtr;
19 
20 #ifdef ENABLE_SIMULATION_PROFILING
21 const bool BODY_SIMULATION_PROFILING = true;
22 #else
23 const bool BODY_SIMULATION_PROFILING = false;
24 #endif
25 
27 {
28 public:
29  WorldBase();
30  virtual ~WorldBase();
31 
36  int numBodies() const { return bodyInfoArray.size(); }
37 
43  DyBody* body(int index) const;
44 
50  DyBody* body(const std::string& name) const;
51 
58  return bodyInfoArray[index].forwardDynamics;
59  }
60 
66  int bodyIndex(const std::string& name) const;
67 
74  int addBody(DyBody* body);
75 
80  int addBody(DyBody* body, const ForwardDynamicsPtr& forwardDynamics);
81 
85  void clearBodies();
86 
90  void clearCollisionPairs();
91 
96  void setTimeStep(double dt);
97 
102  double timeStep(void) const { return timeStep_; }
103 
108  void setCurrentTime(double tm);
109 
114  double currentTime(void) const { return currentTime_; }
115 
120  void setGravityAcceleration(const Vector3& g);
121 
126  inline const Vector3& gravityAcceleration() const { return g; }
127 
128 
134  void enableSensors(bool on);
135 
136  void setOldAccelSensorCalcMode(bool on);
137 
141  void setEulerMethod();
142 
146  void setRungeKuttaMethod();
147 
151  virtual void initialize();
152 
153  void setVirtualJointForces();
154 
158  virtual void calcNextState();
159 
166  std::pair<int,bool> getIndexOfLinkPairs(DyLink* link1, DyLink* link2);
167 
168 protected:
169 
170  double currentTime_;
171  double timeStep_;
172 
173  struct BodyInfo {
174  DyBodyPtr body;
177  };
178  std::vector<BodyInfo> bodyInfoArray;
179 
182 
183 private:
184  typedef std::map<std::string, int> NameToIndexMap;
185  NameToIndexMap nameToBodyIndexMap;
186 
187  typedef std::map<DyBodyPtr, int> BodyToIndexMap;
188  BodyToIndexMap bodyToIndexMap;
189 
190  Vector3 g;
191 
192  bool isEulerMethod; // Euler or Runge Kutta ?
193 
194  struct LinkPairKey {
195  DyLink* link1;
196  DyLink* link2;
197  bool operator<(const LinkPairKey& pair2) const;
198  };
199  typedef std::map<LinkPairKey, int> LinkPairKeyToIndexMap;
200  LinkPairKeyToIndexMap linkPairKeyToIndexMap;
201 
202  int numRegisteredLinkPairs;
203 
204 };
205 
206 template <class TConstraintForceSolver> class World : public WorldBase
207 {
208 public:
209  TConstraintForceSolver constraintForceSolver;
210 
211 #ifdef ENABLE_SIMULATION_PROFILING
212  double forceSolveTime;
213  double forwardDynamicsTime;
214  double customizerTime;
215  TimeMeasure timer;
216 #endif
217 
218  World() : constraintForceSolver(*this) { }
219 
220  virtual void initialize() {
222  constraintForceSolver.initialize();
223  }
224 
225  virtual void calcNextState(){
226 #ifdef ENABLE_SIMULATION_PROFILING
227  timer.begin();
228 #endif
230 #ifdef ENABLE_SIMULATION_PROFILING
231  customizerTime = timer.measure();
232  timer.begin();
233 #endif
234  constraintForceSolver.solve();
235 #ifdef ENABLE_SIMULATION_PROFILING
236  forceSolveTime = timer.measure();
237  timer.begin();
238 #endif
240 #ifdef ENABLE_SIMULATION_PROFILING
241  forwardDynamicsTime = timer.measure();
242 #endif
243  }
244 };
245 
246 };
247 
248 #endif
ForwardDynamicsPtr & forwardDynamics(int index)
get forward dynamics computation method for body
Definition: DyWorld.h:57
bool sensorsAreEnabled
Definition: DyWorld.h:180
void setVirtualJointForces()
Definition: DyWorld.cpp:124
bool isOldAccelSensorCalcMode
Definition: DyWorld.h:181
bool operator<(ref_ptr< T > const &a, ref_ptr< T > const &b)
Definition: Referenced.h:260
double timeStep(void) const
get time step
Definition: DyWorld.h:102
Definition: DyBody.h:115
virtual void calcNextState()
compute forward dynamics and update current state
Definition: DyWorld.h:225
double currentTime(void) const
get current time
Definition: DyWorld.h:114
void begin()
Definition: TimeMeasure.h:33
int numBodies() const
get the number of bodies in this world
Definition: DyWorld.h:36
virtual void initialize()
initialize this world. This must be called after all bodies are registered.
Definition: DyWorld.cpp:98
const bool BODY_SIMULATION_PROFILING
Definition: DyWorld.h:23
virtual void initialize()
initialize this world. This must be called after all bodies are registered.
Definition: DyWorld.h:220
Definition: TimeMeasure.h:16
virtual void calcNextState()
compute forward dynamics and update current state
Definition: DyWorld.cpp:135
ForwardDynamicsPtr forwardDynamics
Definition: DyWorld.h:175
double measure()
Definition: TimeMeasure.h:56
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
Definition: DyWorld.h:26
double timeStep_
Definition: DyWorld.h:171
DyBodyPtr body
Definition: DyWorld.h:174
bool hasVirtualJointForces
Definition: DyWorld.h:176
ref_ptr< DyBody > DyBodyPtr
Definition: DyBody.h:143
Definition: DyWorld.h:206
double currentTime_
Definition: DyWorld.h:170
Definition: DyWorld.h:173
Eigen::Vector3d Vector3
Definition: EigenTypes.h:58
World()
Definition: DyWorld.h:218
TConstraintForceSolver constraintForceSolver
Definition: DyWorld.h:209
#define CNOID_EXPORT
Definition: Util/exportdecl.h:37
boost::shared_ptr< ForwardDynamics > ForwardDynamicsPtr
Definition: ForwardDynamics.h:67
std::vector< BodyInfo > bodyInfoArray
Definition: DyWorld.h:178
const Vector3 & gravityAcceleration() const
get gravity acceleration
Definition: DyWorld.h:126