Klamp't  0.9.0
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 namespace Klampt {
14 
15 struct ODEObjectID;
16 struct ODEContactList;
17 struct ODEContactResult;
18 struct ODEJoint;
19 
24 {
26 
28  double gravity[3];
35 
36  //collision checking settings
53 
54  //contact detection settings
62 
63  //ODE constants, mostly relevant to tightness of robot constraints
68 
69  //Instability detection / correction parameters
75  double instabilityConstantEnergyThreshold,instabilityLinearEnergyThreshold;
76  double instabilityMaxEnergyThreshold;
81 };
82 
83 
84 
102 {
103  public:
116  enum Status { StatusNormal=0, StatusAdaptiveTimeStepping=1, StatusContactUnreliable=2, StatusUnstable=3, StatusError=4 };
117 
118  ODESimulator();
119  virtual ~ODESimulator();
120  void SetGravity(const Vector3& g);
121  void SetAutoDisable(bool autoDisable=true); //global auto disable -- see ODE docs
122  void SetERP(double erp); //global error reduction -- see ODE docs
123  void SetCFM(double erp); //global constraint force mixing -- see ODE docs
124  ODESimulatorSettings& GetSettings() { return settings; }
125  Status GetStatus() const;
126  void GetStatusHistory(vector<Status>& statuses,vector<Real>& statusChangeTimes) const;
127  void AddTerrain(TerrainModel& terr);
128  void AddRobot(RobotModel& robot);
129  void AddObject(RigidObjectModel& object);
132  bool CheckObjectOverlap(vector<pair<ODEObjectID,ODEObjectID> >& overlaps);
133  void Step(Real dt);
134  void StepDynamics(Real dt);
135  bool ReadState(File& f);
136  bool WriteState(File& f) const;
137 
138  size_t numTerrains() const { return terrains.size(); }
139  size_t numRobots() const { return robots.size(); }
140  size_t numObjects() const { return objects.size(); }
141  inline dWorldID world() const { return worldID; }
142  const TerrainModel* terrain(int i) const { return terrains[i]; }
143  ODEGeometry* terrainGeom(int i) const { return terrainGeoms[i]; }
144  ODERobot* robot(int i) const { return robots[i]; }
145  ODERigidObject* object(int i) const { return objects[i]; }
146 
147  string ObjectName(const ODEObjectID& obj) const;
148  dBodyID ObjectBody(const ODEObjectID& obj) const;
149  dGeomID ObjectGeom(const ODEObjectID& obj) const;
150  void EnableContactFeedback(const ODEObjectID& a,const ODEObjectID& b);
151  ODEContactList* GetContactFeedback(const ODEObjectID& a,const ODEObjectID& b);
152  void GetContactFeedback(const ODEObjectID& a,vector<ODEContactList*>& contacts);
153  void ClearContactFeedback();
154  bool InContact(const ODEObjectID& a) const;
155  bool InContact(const ODEObjectID& a,const ODEObjectID& b) const;
157  void DisableInstabilityCorrection();
159  void DisableInstabilityCorrection(const ODEObjectID& obj);
160 
162  ODEJoint* AddJoint(const ODEObjectID& obj);
164  ODEJoint* AddJoint(const ODEObjectID& a,const ODEObjectID& b);
166  void RemoveJoint(ODEJoint*);
168  void RemoveJoints(const ODEObjectID& obj);
170  void RemoveJoints(const ODEObjectID& a,const ODEObjectID& b);
171 
172  //used internally
173  bool ReadState_Internal(File& f);
174  bool WriteState_Internal(File& f) const;
175  void DetectCollisions();
176  void SetupContactResponse();
177  void SetupContactResponse(const ODEObjectID& a,const ODEObjectID& b,int feedbackIndex,ODEContactResult& c);
178  void ClearCollisions();
179  bool InstabilityCorrection();
180 
181  //overload this to have custom parameters for surface pairs
182  virtual void GetSurfaceParameters(const ODEObjectID& a,const ODEObjectID& b,dSurfaceParameters& surface) const;
183 
184  private:
185  vector<pair<Status,Real> > statusHistory;
186  ODESimulatorSettings settings;
187  dWorldID worldID;
188  dSpaceID envSpaceID;
189  vector<ODEGeometry*> terrainGeoms;
190  vector<const TerrainModel*> terrains;
191  vector<ODERobot*> robots;
192  vector<ODERigidObject*> objects;
193  map<pair<ODEObjectID,ODEObjectID>,ODEContactList> contactList;
194  dJointGroupID contactGroupID;
195  Real timestep;
196  Real simTime;
197  map<ODEObjectID,Real> energies;
198 
199 public:
200  //for adaptive time stepping
201  File lastState;
202  Real lastStateTimestep;
203  map<pair<ODEObjectID,ODEObjectID>,double> lastMarginsRemaining;
204  //joints
205  list<ODEJoint> joints;
206 };
207 
208 
214 {
215  inline ODEObjectID(int _t=-1,int _i=-1,int _b=-1)
216  :type(_t),index(_i),bodyIndex(_b) {}
217  inline void SetEnv(int _index=0) { type = 0; index = _index; }
218  inline void SetRobot(int _index=0) { type = 1; index = _index; bodyIndex = -1; }
219  inline void SetRobotBody(int _index,int _bodyIndex=-1) { type = 1; index = _index; bodyIndex = _bodyIndex; }
220  inline void SetRigidObject(int _index=0) { type = 2; index = _index; }
221  inline bool IsEnv() const { return type == 0; }
222  inline bool IsRobot() const { return type == 1; }
223  inline bool IsRigidObject() const { return type == 2; }
224  inline bool operator == (const ODEObjectID& rhs) const {
225  if(type != rhs.type) return false;
226  if(index != rhs.index) return false;
227  if(type == 1 && bodyIndex!=rhs.bodyIndex) return false;
228  return true;
229  }
230  inline bool operator < (const ODEObjectID& rhs) const {
231  if(type < rhs.type) return true;
232  else if(type > rhs.type) return false;
233  if(index < rhs.index) return true;
234  else if(index > rhs.index) return false;
235  return (bodyIndex<rhs.bodyIndex);
236  }
237 
238  int type; //0: static environment, 1: robot, 2: movable object
239  int index; //index in world
240  int bodyIndex; //for robots, this identifies a link
241 };
242 
250 {
251  ODEObjectID o1,o2;
252  //the contact points
253  vector<ContactPoint> points;
254  vector<Vector3> forces;
255  //whether the contact detector found excessive penetration
256  bool penetrating;
257 
258  vector<int> feedbackIndices; //internally used
259 };
260 
266 struct ODEJoint
267 {
268  ODEJoint();
269  ~ODEJoint();
271  void MakeFixed();
273  void MakeHinge(const Vector3& pt,const Vector3& axis);
275  void MakeSlider(const Vector3& dir);
277  void Destroy();
278 
280  Real GetPosition();
282  Real GetVelocity();
283 
285  void SetLimits(Real min,Real max);
287  void SetBounce(Real coeff);
289  void SetFriction(Real coeff);
291  void SetFixedVelocity(Real vel,Real fmax);
293  void AddForce(Real f);
295  void GetConstraintForces(Vector3& f1,Vector3& t1,Vector3& f2,Vector3& t2);
296 
297  int type; //<-1 not initialized, 0 fixed, 1 hinge, 2 slider
298  ODEObjectID o1,o2;
299  ODESimulator* sim;
300  dJointID joint;
301  dJointFeedback feedback;
302 };
303 
304 } //namespace Klampt
305 
306 #endif
double defaultEnvPadding
The default collision padding for environments.
Definition: ODESimulator.h:32
bool robotRobotCollisions
If true, robots can collide with other robots (default false)
Definition: ODESimulator.h:44
Global simulator settings.
Definition: ODESimulator.h:23
bool boundaryLayerCollisions
If true, uses boundary layers for collision detection (recommended true)
Definition: ODESimulator.h:38
An index that identifies some ODE object in the world. Environments, robots, robot bodies...
Definition: ODESimulator.h:213
ODESurfaceProperties defaultEnvSurface
The default surface property for environments.
Definition: ODESimulator.h:34
bool autoDisable
Whether to use ODE&#39;s auto-disable functionality for non-moving objects.
Definition: ODESimulator.h:30
A (static) rigid object that may be manipulated.
Definition: RigidObject.h:15
bool rigidObjectCollisions
If true, rigid objects collide with one another (default true)
Definition: ODESimulator.h:40
A robot simulated in an ODE "world".
Definition: ODERobot.h:30
double errorReductionParameter
ODE&#39;s global ERP parameter.
Definition: ODESimulator.h:65
A list of contacts between two objects, returned as feedback from the simulation. ...
Definition: ODESimulator.h:249
bool adaptiveTimeStepping
Definition: ODESimulator.h:50
An ODE-simulated rigid object.
Definition: ODERigidObject.h:19
double clusterNormalScale
Definition: ODESimulator.h:61
double dampedLeastSquaresParameter
ODE&#39;s global DLS parameter.
Definition: ODESimulator.h:67
A model of a static terrain with known friction.
Definition: Terrain.h:15
double instabilityPostCorrectionEnergy
Definition: ODESimulator.h:80
double gravity[3]
The gravity vector.
Definition: ODESimulator.h:28
Status
Definition: ODESimulator.h:116
The main robot type used in RobotSim.
Definition: Robot.h:83
surface properties for any ODE rigid object, robot link, or fixed object.
Definition: ODESurface.h:10
A joint between two objects.
Definition: ODESimulator.h:266
bool robotSelfCollisions
If true, robot self-collisions are detected (default false)
Definition: ODESimulator.h:42
double instabilityConstantEnergyThreshold
Definition: ODESimulator.h:75
double minimumAdaptiveTimeStep
The minimum time step used by adaptive time stepping (default 1e-6)
Definition: ODESimulator.h:52
Definition: ContactDistance.h:6
int maxContacts
Definition: ODESimulator.h:57
An interface to the ODE simulator.
Definition: ODESimulator.h:101
An ODE collision geometry.
Definition: ODEGeometry.h:19