Choreonoid  1.1
World.h
[詳解]
1 
6 #ifndef CNOID_BODY_WORLD_H_INCLUDED
7 #define CNOID_BODY_WORLD_H_INCLUDED
8 
9 #include "Body.h"
10 #include "ForwardDynamics.h"
11 #include <map>
12 #include "exportdecl.h"
13 
14 namespace cnoid {
15 
16  class Link;
17 
19  {
20  public:
21  WorldBase();
22  virtual ~WorldBase();
23 
28  inline int numBodies() { return bodyInfoArray.size(); }
29 
35  BodyPtr body(int index);
36 
42  BodyPtr body(const std::string& name);
43 
49  inline ForwardDynamicsPtr forwardDynamics(int index) {
50  return bodyInfoArray[index].forwardDynamics;
51  }
52 
58  int bodyIndex(const std::string& name);
59 
66  int addBody(BodyPtr body);
67 
71  void clearBodies();
72 
76  void clearCollisionPairs();
77 
82  void setTimeStep(double dt);
83 
88  double timeStep(void) const { return timeStep_; }
89 
94  void setCurrentTime(double tm);
95 
100  double currentTime(void) const { return currentTime_; }
101 
106  void setGravityAcceleration(const Vector3& g);
107 
112  inline const Vector3& gravityAcceleration() { return g; }
113 
119  void enableSensors(bool on);
120 
124  void setEulerMethod();
125 
129  void setRungeKuttaMethod();
130 
134  virtual void initialize();
135 
139  virtual void calcNextState();
140 
147  std::pair<int,bool> getIndexOfLinkPairs(Link* link1, Link* link2);
148 
149  protected:
150 
151  double currentTime_;
152  double timeStep_;
153 
154  struct BodyInfo {
157  };
158  std::vector<BodyInfo> bodyInfoArray;
159 
161 
162  private:
163  typedef std::map<std::string, int> NameToIndexMap;
164  NameToIndexMap nameToBodyIndexMap;
165 
166  typedef std::map<BodyPtr, int> BodyToIndexMap;
167  BodyToIndexMap bodyToIndexMap;
168 
169  struct LinkPairKey {
170  Link* link1;
171  Link* link2;
172  bool operator<(const LinkPairKey& pair2) const;
173  };
174  typedef std::map<LinkPairKey, int> LinkPairKeyToIndexMap;
175  LinkPairKeyToIndexMap linkPairKeyToIndexMap;
176 
177  int numRegisteredLinkPairs;
178 
179  Vector3 g;
180 
181  bool isEulerMethod; // Euler or Runge Kutta ?
182 
183  };
184 
185 
186  template <class TConstraintForceSolver> class World : public WorldBase
187  {
188  public:
189  TConstraintForceSolver constraintForceSolver;
190 
192 
193  virtual void initialize() {
195  constraintForceSolver.initialize();
196  }
197 
198  virtual void calcNextState(){
199  constraintForceSolver.solve();
201  }
202  };
203 
204 };
205 
206 #endif
bool sensorsAreEnabled
Definition: World.h:160
boost::intrusive_ptr< Body > BodyPtr
Definition: Body.h:22
virtual void calcNextState()
compute forward dynamics and update current state
Definition: World.h:198
virtual void initialize()
initialize this world. This must be called after all bodies are registered.
Definition: World.cpp:89
virtual void initialize()
initialize this world. This must be called after all bodies are registered.
Definition: World.h:193
double currentTime(void) const
get current time
Definition: World.h:100
virtual void calcNextState()
compute forward dynamics and update current state
Definition: World.cpp:125
int numBodies()
get the number of bodies in this world
Definition: World.h:28
ForwardDynamicsPtr forwardDynamics
Definition: World.h:156
ForwardDynamicsPtr forwardDynamics(int index)
get forward dynamics computation method for body
Definition: World.h:49
double timeStep(void) const
get time step
Definition: World.h:88
Definition: World.h:18
double timeStep_
Definition: World.h:152
BodyPtr body
Definition: World.h:155
const Vector3 & gravityAcceleration()
get gravity acceleration
Definition: World.h:112
Definition: World.h:186
double currentTime_
Definition: World.h:151
Definition: World.h:154
Eigen::Vector3d Vector3
Definition: EigenTypes.h:26
World()
Definition: World.h:191
TConstraintForceSolver constraintForceSolver
Definition: World.h:189
#define CNOID_EXPORT
Definition: Util/exportdecl.h:13
boost::shared_ptr< ForwardDynamics > ForwardDynamicsPtr
Definition: ForwardDynamics.h:77
std::vector< BodyInfo > bodyInfoArray
Definition: World.h:158
bool initialize()