Klamp't  0.8.1
ODESimulator.h
1 #ifndef ODE_INTERFACE_SIMULATOR_H
2 #define ODE_INTERFACE_SIMULATOR_H
3 
4 #include "ODERobot.h"
5 #include "ODERigidObject.h"
6 #include "ODESurface.h"
7 #include <Klampt/Modeling/Terrain.h>
8 #include <Klampt/Modeling/RigidObject.h>
9 #include <KrisLibrary/robotics/Contact.h>
10 #include <ode/contact.h>
11 #include <map>
12 
13 struct ODEObjectID;
14 struct ODEContactList;
15 struct ODEContactResult;
16 
21 {
23 
25  double gravity[3];
30 
31  //collision checking settings
45 
46  //contact detection settings
54 
55  //ODE constants, mostly relevant to tightness of robot constraints
60 
61  //Instability detection / correction parameters
67  double instabilityConstantEnergyThreshold,instabilityLinearEnergyThreshold;
68  double instabilityMaxEnergyThreshold;
73 };
74 
75 
76 
94 {
95  public:
108  enum Status { StatusNormal=0, StatusAdaptiveTimeStepping=1, StatusContactUnreliable=2, StatusUnstable=3, StatusError=4 };
109 
110  ODESimulator();
111  virtual ~ODESimulator();
112  void SetGravity(const Vector3& g);
113  void SetERP(double erp); //global error reduction -- see ODE docs
114  void SetCFM(double erp); //global constraint force mixing -- see ODE docs
115  ODESimulatorSettings& GetSettings() { return settings; }
116  Status GetStatus() const;
117  void GetStatusHistory(vector<Status>& statuses,vector<Real>& statusChangeTimes) const;
118  void AddTerrain(Terrain& terr);
119  void AddRobot(Robot& robot);
120  void AddObject(RigidObject& object);
123  bool CheckObjectOverlap(vector<pair<ODEObjectID,ODEObjectID> >& overlaps);
124  void Step(Real dt);
125  void StepDynamics(Real dt);
126  bool ReadState(File& f);
127  bool WriteState(File& f) const;
128 
129  size_t numTerrains() const { return terrains.size(); }
130  size_t numRobots() const { return robots.size(); }
131  size_t numObjects() const { return objects.size(); }
132  inline dWorldID world() const { return worldID; }
133  const Terrain* terrain(int i) const { return terrains[i]; }
134  ODEGeometry* terrainGeom(int i) const { return terrainGeoms[i]; }
135  ODERobot* robot(int i) const { return robots[i]; }
136  ODERigidObject* object(int i) const { return objects[i]; }
137 
138  string ObjectName(const ODEObjectID& obj) const;
139  dBodyID ObjectBody(const ODEObjectID& obj) const;
140  dGeomID ObjectGeom(const ODEObjectID& obj) const;
141  void EnableContactFeedback(const ODEObjectID& a,const ODEObjectID& b);
142  ODEContactList* GetContactFeedback(const ODEObjectID& a,const ODEObjectID& b);
143  void GetContactFeedback(const ODEObjectID& a,vector<ODEContactList*>& contacts);
144  void ClearContactFeedback();
145  bool InContact(const ODEObjectID& a) const;
146  bool InContact(const ODEObjectID& a,const ODEObjectID& b) const;
148  void DisableInstabilityCorrection();
150  void DisableInstabilityCorrection(const ODEObjectID& obj);
151 
152  //used internally
153  bool ReadState_Internal(File& f);
154  bool WriteState_Internal(File& f) const;
155  void DetectCollisions();
156  void SetupContactResponse();
157  void SetupContactResponse(const ODEObjectID& a,const ODEObjectID& b,int feedbackIndex,ODEContactResult& c);
158  void ClearCollisions();
159  bool InstabilityCorrection();
160 
161  //overload this to have custom parameters for surface pairs
162  virtual void GetSurfaceParameters(const ODEObjectID& a,const ODEObjectID& b,dSurfaceParameters& surface) const;
163 
164  private:
165  vector<pair<Status,Real> > statusHistory;
166  ODESimulatorSettings settings;
167  dWorldID worldID;
168  dSpaceID envSpaceID;
169  vector<ODEGeometry*> terrainGeoms;
170  vector<const Terrain*> terrains;
171  vector<ODERobot*> robots;
172  vector<ODERigidObject*> objects;
173  map<pair<ODEObjectID,ODEObjectID>,ODEContactList> contactList;
174  dJointGroupID contactGroupID;
175  Real timestep;
176  Real simTime;
177  map<ODEObjectID,Real> energies;
178 
179 public:
180  //for adaptive time stepping
181  File lastState;
182  Real lastStateTimestep;
183  map<pair<ODEObjectID,ODEObjectID>,double> lastMarginsRemaining;
184 };
185 
186 
192 {
193  inline ODEObjectID(int _t=-1,int _i=-1,int _b=-1)
194  :type(_t),index(_i),bodyIndex(_b) {}
195  inline void SetEnv(int _index=0) { type = 0; index = _index; }
196  inline void SetRobot(int _index=0) { type = 1; index = _index; bodyIndex = -1; }
197  inline void SetRobotBody(int _index,int _bodyIndex=-1) { type = 1; index = _index; bodyIndex = _bodyIndex; }
198  inline void SetRigidObject(int _index=0) { type = 2; index = _index; }
199  inline bool IsEnv() const { return type == 0; }
200  inline bool IsRobot() const { return type == 1; }
201  inline bool IsRigidObject() const { return type == 2; }
202  inline bool operator == (const ODEObjectID& rhs) const {
203  if(type != rhs.type) return false;
204  if(index != rhs.index) return false;
205  if(type == 1 && bodyIndex!=rhs.bodyIndex) return false;
206  return true;
207  }
208  inline bool operator < (const ODEObjectID& rhs) const {
209  if(type < rhs.type) return true;
210  else if(type > rhs.type) return false;
211  if(index < rhs.index) return true;
212  else if(index > rhs.index) return false;
213  return (bodyIndex<rhs.bodyIndex);
214  }
215 
216  int type; //0: static environment, 1: robot, 2: movable object
217  int index; //index in world
218  int bodyIndex; //for robots, this identifies a link
219 };
220 
228 {
229  ODEObjectID o1,o2;
230  //the contact points
231  vector<ContactPoint> points;
232  vector<Vector3> forces;
233  //whether the contact detector found excessive penetration
234  bool penetrating;
235 
236  vector<int> feedbackIndices; //internally used
237 };
238 
239 #endif
int maxContacts
Definition: ODESimulator.h:49
double instabilityPostCorrectionEnergy
Definition: ODESimulator.h:72
A robot simulated in an ODE "world".
Definition: ODERobot.h:28
The main robot type used in RobotSim.
Definition: Robot.h:79
double clusterNormalScale
Definition: ODESimulator.h:53
ODESurfaceProperties defaultEnvSurface
The default surface property for environments.
Definition: ODESimulator.h:29
double dampedLeastSquaresParameter
ODE&#39;s global DLS parameter.
Definition: ODESimulator.h:59
An interface to the ODE simulator.
Definition: ODESimulator.h:93
bool adaptiveTimeStepping
Definition: ODESimulator.h:42
Status
Definition: ODESimulator.h:108
double minimumAdaptiveTimeStep
The minimum time step used by adaptive time stepping (default 1e-6)
Definition: ODESimulator.h:44
double gravity[3]
The gravity vector.
Definition: ODESimulator.h:25
A list of contacts between two objects, returned as feedback from the simulation. ...
Definition: ODESimulator.h:227
A model of a static terrain with known friction.
Definition: Terrain.h:13
double instabilityConstantEnergyThreshold
Definition: ODESimulator.h:67
double defaultEnvPadding
The default collision padding for environments.
Definition: ODESimulator.h:27
An ODE collision geometry.
Definition: ODEGeometry.h:17
An ODE-simulated rigid object.
Definition: ODERigidObject.h:17
bool robotRobotCollisions
If true, robots can collide with other robots (default false)
Definition: ODESimulator.h:39
bool rigidObjectCollisions
If true, rigid objects collide with one another (default true)
Definition: ODESimulator.h:35
Global simulator settings.
Definition: ODESimulator.h:20
An index that identifies some ODE object in the world. Environments, robots, robot bodies...
Definition: ODESimulator.h:191
double errorReductionParameter
ODE&#39;s global ERP parameter.
Definition: ODESimulator.h:57
A (static) rigid object that may be manipulated.
Definition: RigidObject.h:13
surface properties for any ODE rigid object, robot link, or fixed object.
Definition: ODESurface.h:8
bool robotSelfCollisions
If true, robot self-collisions are detected (default false)
Definition: ODESimulator.h:37
bool boundaryLayerCollisions
If true, uses boundary layers for collision detection (recommended true)
Definition: ODESimulator.h:33