1 #ifndef ODE_INTERFACE_SIMULATOR_H 2 #define ODE_INTERFACE_SIMULATOR_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> 16 struct ODEContactList;
17 struct ODEContactResult;
76 double instabilityMaxEnergyThreshold;
116 enum Status { StatusNormal=0, StatusAdaptiveTimeStepping=1, StatusContactUnreliable=2, StatusUnstable=3, StatusError=4 };
120 void SetGravity(
const Vector3& g);
122 void SetERP(
double erp);
123 void SetCFM(
double erp);
126 void GetStatusHistory(vector<Status>& statuses,vector<Real>& statusChangeTimes)
const;
132 bool CheckObjectOverlap(vector<pair<ODEObjectID,ODEObjectID> >& overlaps);
134 void StepDynamics(Real dt);
135 bool ReadState(File& f);
136 bool WriteState(File& f)
const;
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]; }
152 void GetContactFeedback(
const ODEObjectID& a,vector<ODEContactList*>& contacts);
153 void ClearContactFeedback();
157 void DisableInstabilityCorrection();
159 void DisableInstabilityCorrection(
const ODEObjectID& obj);
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();
182 virtual void GetSurfaceParameters(
const ODEObjectID& a,
const ODEObjectID& b,dSurfaceParameters& surface)
const;
185 vector<pair<Status,Real> > statusHistory;
189 vector<ODEGeometry*> terrainGeoms;
190 vector<const TerrainModel*> terrains;
191 vector<ODERobot*> robots;
192 vector<ODERigidObject*> objects;
194 dJointGroupID contactGroupID;
197 map<ODEObjectID,Real> energies;
202 Real lastStateTimestep;
203 map<pair<ODEObjectID,ODEObjectID>,
double> lastMarginsRemaining;
205 list<ODEJoint> joints;
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;
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);
253 vector<ContactPoint> points;
254 vector<Vector3> forces;
258 vector<int> feedbackIndices;
273 void MakeHinge(
const Vector3& pt,
const Vector3& axis);
275 void MakeSlider(
const Vector3& dir);
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);
301 dJointFeedback feedback;
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'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's global ERP parameter.
Definition: ODESimulator.h:65
bool adaptiveTimeStepping
Definition: ODESimulator.h:50
An ODE-simulated rigid object.
Definition: ODERigidObject.h:19
double clusterNormalScale
Definition: ODESimulator.h:61
double dampedLeastSquaresParameter
ODE'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