Klamp't  0.9.0
WorldSimulation.h
1 #ifndef WORLD_SIMULATION_H
2 #define WORLD_SIMULATION_H
3 
5 #include "ODESimulator.h"
6 #include "ControlledSimulator.h"
7 #include <map>
8 
9 namespace Klampt {
10 
25 struct ContactFeedbackInfo
26 {
27  //summary information
28  bool accum;
29  int contactCount,separationCount;
30  bool inContact;
31  Vector3 meanForce,meanTorque,meanPoint;
32 
33  bool penetrating;
34  int penetrationCount;
35 
36  //full contact information over sub-steps
37  bool accumFull; //set to true if all ODEContactLists should be stored
38  vector<double> times;
39  vector<ODEContactList> contactLists;
40 };
41 
56 {
57  public:
58  WorldSimulationHook() : autokill(false) {}
59  virtual ~WorldSimulationHook() {}
60  virtual void Step(Real dt) {}
61  virtual bool ReadState(File& f) { return true; }
62  virtual bool WriteState(File& f) const { return true; }
63  bool autokill;
64 };
65 
70 {
71 public:
73  void Init(WorldModel* world);
75  void OnAddModel();
77  void SetController(int robot,shared_ptr<RobotController> c);
79  void Advance(Real dt);
81  void AdvanceFake(Real dt);
83  void UpdateModel();
85  void UpdateRobot(int index);
89  bool ReadState(File& f);
90  bool WriteState(File& f) const;
91  bool ReadState(const string& data);
92  bool WriteState(string& data) const;
93 
94  //contact querying routines
98  void EnableContactFeedback(int aid,int bid,bool accum=true,bool accumFull=false);
103  bool InContact(int aid,int bid=-1);
105  bool HadContact(int aid,int bid=-1);
107  bool HadSeparation(int aid,int bid=-1);
112  bool HadPenetration(int aid,int bid=-1);
114  ContactFeedbackInfo* GetContactFeedback(int aid,int bid);
116  ODEContactList* GetContactList(int aid,int bid);
118  Vector3 ContactForce(int aid,int bid=-1);
120  Vector3 ContactTorque(int aid,int bid=-1);
122  Vector3 MeanContactForce(int aid,int bid=-1);
124  Vector3 MeanContactTorque(int aid,int bid=-1);
125 
126  //helpers to convert indexing schemes
127  int ODEToWorldID(const ODEObjectID& odeid) const;
128  ODEObjectID WorldToODEID(int id) const;
129 
130  WorldModel* world;
131  ODESimulator odesim;
132  Real time;
133  Real simStep;
134  bool fakeSimulation;
135  vector<ControlledRobotSimulator> controlSimulators;
136  vector<shared_ptr<RobotController> > robotControllers;
137  vector<shared_ptr<WorldSimulationHook> > hooks;
138  typedef map<pair<ODEObjectID,ODEObjectID>,ContactFeedbackInfo> ContactFeedbackMap;
139  ContactFeedbackMap contactFeedback;
142 };
143 
147 class ForceHook : public WorldSimulationHook
148 {
149  public:
150  ForceHook(dBodyID body,const Vector3& worldpt,const Vector3& f);
151  virtual void Step(Real dt);
152  virtual bool ReadState(File& f);
153  virtual bool WriteState(File& f) const;
154 
155  dBodyID body;
156  Vector3 worldpt,f;
157 };
158 
163 class LocalForceHook : public WorldSimulationHook
164 {
165  public:
166  LocalForceHook(dBodyID body,const Vector3& localpt,const Vector3& f);
167  virtual void Step(Real dt);
168  virtual bool ReadState(File& f);
169  virtual bool WriteState(File& f) const;
170 
171  dBodyID body;
172  Vector3 localpt,f;
173 };
174 
175 
180 class WrenchHook : public WorldSimulationHook
181 {
182  public:
183  WrenchHook(dBodyID body,const Vector3& f,const Vector3& m);
184  virtual void Step(Real dt);
185  virtual bool ReadState(File& f);
186  virtual bool WriteState(File& f) const;
187 
188  dBodyID body;
189  Vector3 f,m;
190 };
191 
195 class SpringHook : public WorldSimulationHook
196 {
197  public:
198  SpringHook(dBodyID body,const Vector3& worldpt,const Vector3& target,Real kP,Real kD=0);
199  virtual void Step(Real dt);
200  virtual bool ReadState(File& f);
201  virtual bool WriteState(File& f) const;
202 
203  dBodyID body;
204  Vector3 localpt,target;
205  Real kP,kD;
206 };
207 
211 class JointForceHook : public WorldSimulationHook
212 {
213 public:
214  JointForceHook(ODEJoint* joint,Real f);
215  virtual void Step(Real dt);
216  virtual bool ReadState(File& f);
217  virtual bool WriteState(File& f) const;
218 
219  ODEJoint* joint;
220  Real f;
221 };
222 
227 {
228 public:
229  JointSpringHook(ODEJoint* joint,Real target,Real kP,Real kD=0);
230  virtual void Step(Real dt);
231  virtual bool ReadState(File& f);
232  virtual bool WriteState(File& f) const;
233 
234  ODEJoint* joint;
235  Real target;
236  Real kP,kD;
237 };
238 
239 } //namespace Klampt
240 
241 #endif
A hook that adds a constant force to a body.
Definition: Simulator.h:147
A hook that adds a constant wrench (force f and moment m) to the body.
Definition: Simulator.h:180
ODESimulator::Status worstStatus
Worst simulation status over the last Advance() call.
Definition: WorldSimulation.h:141
A hook that adds a constant force in world coordinates to a point on a body given in local coordinate...
Definition: Simulator.h:163
Any function that should be run per sub-step of the simulation needs to be a WorldSimulationHook subc...
Definition: WorldSimulation.h:55
An index that identifies some ODE object in the world. Environments, robots, robot bodies...
Definition: ODESimulator.h:213
Defines the WorldModel class.
int separationCount
number of sub-steps in which contact was made / object was separated during the outer simulation inte...
Definition: Simulator.h:29
bool accum
set this to true if you want to accumulate summary feedback over sub-steps
Definition: Simulator.h:28
A hook that acts as a Hookean (optionally damped) spring to a given fixed target point.
Definition: Simulator.h:195
A list of contacts between two objects, returned as feedback from the simulation. ...
Definition: ODESimulator.h:249
Container for information about contacts regarding a certain object. Can be set to accumulate a summa...
Definition: Simulator.h:25
A hook that adds a Hookean (optionally damped) virtual spring to a joint.
Definition: Simulator.h:226
bool inContact
true if contact exists at the end of the outer simulation interval
Definition: Simulator.h:30
A physical simulator for a WorldModel.
Definition: WorldSimulation.h:69
Status
Definition: ODESimulator.h:116
int penetrationCount
the number of sub-steps in which the objects were penetrating during the outer simulation interval ...
Definition: Simulator.h:34
A joint between two objects.
Definition: ODESimulator.h:266
A hook that adds a constant force to a joint.
Definition: Simulator.h:211
Definition: ContactDistance.h:6
An interface to the ODE simulator.
Definition: ODESimulator.h:101
The main world class containing multiple robots, objects, and static geometries (terrains). Lights and other viewport information may also be stored here.
Definition: World.h:24
bool penetrating
true if the objects are currently penetrating
Definition: Simulator.h:33