ODE physics engine. More...
#include <ODEPhysics.hh>
Inherits PhysicsEngine.
Public Types | |
enum | ODEParam { SOLVER_TYPE, GLOBAL_CFM, GLOBAL_ERP, SOR_PRECON_ITERS, PGS_ITERS, SOR, CONTACT_MAX_CORRECTING_VEL, CONTACT_SURFACE_LAYER, MAX_CONTACTS, MIN_STEP_SIZE, INERTIA_RATIO_REDUCTION, FRICTION_MODEL, WORLD_SOLVER_TYPE } |
ODE Physics parameter types. More... | |
Public Member Functions | |
ODEPhysics (WorldPtr _world) | |
Constructor. More... | |
virtual | ~ODEPhysics () |
Destructor. More... | |
void | Collide (ODECollision *_collision1, ODECollision *_collision2, dContactGeom *_contactCollisions) |
Collide two collision objects. More... | |
virtual CollisionPtr | CreateCollision (const std::string &_shapeType, LinkPtr _parent) |
Create a collision. More... | |
CollisionPtr | CreateCollision (const std::string &_shapeType, const std::string &_linkName) |
Create a collision. More... | |
virtual JointPtr | CreateJoint (const std::string &_type, ModelPtr _parent) |
Create a new joint. More... | |
virtual LinkPtr | CreateLink (ModelPtr _parent) |
Create a new body. More... | |
virtual ModelPtr | CreateModel (BasePtr _base) |
Create a new model. More... | |
virtual ShapePtr | CreateShape (const std::string &_shapeType, CollisionPtr _collision) |
Create a physics::Shape object. More... | |
virtual void | DebugPrint () const |
Debug print out of the physic engine state. More... | |
virtual void | Fini () |
Finilize the physics engine. More... | |
virtual bool | GetAutoDisableFlag () |
: Remove this function, and replace it with a more generic property map More... | |
ContactManager * | GetContactManager () const |
Get a pointer to the contact manger. More... | |
virtual double | GetContactMaxCorrectingVel () |
virtual double | GetContactSurfaceLayer () |
virtual std::string | GetFrictionModel () const |
Get friction model. More... | |
virtual unsigned int | GetMaxContacts () |
double | GetMaxStepSize () const |
Get max step size. More... | |
virtual boost::any | GetParam (const std::string &_key) const |
Documentation inherited. More... | |
virtual bool | GetParam (const std::string &_key, boost::any &_value) const |
Documentation inherited. More... | |
boost::recursive_mutex * | GetPhysicsUpdateMutex () const |
returns a pointer to the PhysicsEngine::physicsUpdateMutex. More... | |
double | GetRealTimeUpdateRate () const |
Get real time update rate. More... | |
sdf::ElementPtr | GetSDF () const |
Get a pointer to the SDF element for this physics engine. More... | |
virtual int | GetSORPGSIters () |
virtual int | GetSORPGSPreconIters () |
virtual double | GetSORPGSW () |
dSpaceID | GetSpaceId () const |
Return the world space id. More... | |
virtual std::string | GetStepType () const |
Get the step type (quick, world). More... | |
double | GetTargetRealTimeFactor () const |
Get target real time factor. More... | |
virtual std::string | GetType () const |
Return the physics engine type (ode|bullet|dart|simbody). More... | |
double | GetUpdatePeriod () |
Get the simulation update period. More... | |
virtual double | GetWorldCFM () |
virtual double | GetWorldERP () |
dWorldID | GetWorldId () |
Get the world id. More... | |
virtual std::string | GetWorldStepSolverType () const |
Get solver type for world step. More... | |
virtual void | Init () |
Initialize the physics engine. More... | |
virtual void | InitForThread () |
Init the engine for threads. More... | |
virtual void | Load (sdf::ElementPtr _sdf) |
Load the physics engine. More... | |
void | ProcessJointFeedback (ODEJointFeedback *_feedback) |
process joint feedbacks. More... | |
virtual void | Reset () |
Rest the physics engine. More... | |
virtual void | SetAutoDisableFlag (bool _autoDisable) |
: Remove this function, and replace it with a more generic property map More... | |
virtual void | SetContactMaxCorrectingVel (double vel) |
virtual void | SetContactSurfaceLayer (double layer_depth) |
virtual void | SetFrictionModel (const std::string &_fricModel) |
Set friction model type. More... | |
virtual void | SetGravity (const ignition::math::Vector3d &_gravity) |
Set the gravity vector. More... | |
virtual void | SetMaxContacts (unsigned int max_contacts) |
: Remove this function, and replace it with a more generic property map More... | |
void | SetMaxStepSize (double _stepSize) |
Set max step size. More... | |
virtual bool | SetParam (const std::string &_key, const boost::any &_value) |
Documentation inherited. More... | |
void | SetRealTimeUpdateRate (double _rate) |
Set real time update rate. More... | |
virtual void | SetSeed (uint32_t _seed) |
Set the random number seed for the physics engine. More... | |
virtual void | SetSORPGSIters (unsigned int iters) |
virtual void | SetSORPGSPreconIters (unsigned int iters) |
virtual void | SetSORPGSW (double w) |
virtual void | SetStepType (const std::string &_type) |
Set the step type (quick, world). More... | |
void | SetTargetRealTimeFactor (double _factor) |
Set target real time factor. More... | |
virtual void | SetWorldCFM (double cfm) |
virtual void | SetWorldERP (double erp) |
virtual void | SetWorldStepSolverType (const std::string &_worldSolverType) |
Set world step solver type. More... | |
virtual void | UpdateCollision () |
Update the physics engine collision. More... | |
virtual void | UpdatePhysics () |
Update the physics engine. More... | |
WorldPtr | World () const |
Get a pointer to the world. More... | |
Static Public Member Functions | |
template<typename T > | |
static T | any_cast (const boost::any &_value) |
Helper function for performing any_cast operations in SetParam. More... | |
static Friction_Model | ConvertFrictionModel (const std::string &_fricModel) |
Convert a string to a Friction_Model enum. More... | |
static std::string | ConvertFrictionModel (const Friction_Model _fricModel) |
Convert a Friction_Model enum to a string. More... | |
static void | ConvertMass (InertialPtr _interial, void *_odeMass) |
Convert an ODE mass to Inertial. More... | |
static void | ConvertMass (void *_odeMass, InertialPtr _inertial) |
Convert an Inertial to ODE mass. More... | |
static std::string | ConvertWorldStepSolverType (const World_Solver_Type _solverType) |
Convert a World_Solver_Type enum to a string. More... | |
static World_Solver_Type | ConvertWorldStepSolverType (const std::string &_solverType) |
Convert a string to a World_Solver_Type enum. More... | |
Protected Member Functions | |
virtual void | OnPhysicsMsg (ConstPhysicsPtr &_msg) |
virtual callback for gztopic "~/physics". More... | |
virtual void | OnRequest (ConstRequestPtr &_msg) |
virtual callback for gztopic "~/request". More... | |
Protected Attributes | |
ContactManager * | contactManager |
Class that handles all contacts generated by the physics engine. More... | |
double | maxStepSize |
Real time update rate. More... | |
transport::NodePtr | node |
Node for communication. More... | |
ignition::transport::Node | nodeIgn |
Ignition node for communication. More... | |
transport::SubscriberPtr | physicsSub |
Subscribe to the physics topic. More... | |
boost::recursive_mutex * | physicsUpdateMutex |
Mutex to protect the update cycle. More... | |
double | realTimeUpdateRate |
Real time update rate. More... | |
transport::SubscriberPtr | requestSub |
Subscribe to the request topic. More... | |
transport::PublisherPtr | responsePub |
Response publisher. More... | |
ignition::transport::Node::Publisher | responsePubIgn |
Response publisher. More... | |
sdf::ElementPtr | sdf |
Our SDF values. More... | |
double | targetRealTimeFactor |
Target real time factor. More... | |
WorldPtr | world |
Pointer to the world. More... | |
ODE physics engine.
enum ODEParam |
ODE Physics parameter types.
|
explicit |
Constructor.
[in] | _world | The World that uses this physics engine. |
|
virtual |
Destructor.
|
inlinestaticinherited |
Helper function for performing any_cast operations in SetParam.
This is useful because the PresetManager stores the output of sdf::Element::GetAny as boost::any values in its parameterMap and calls SetParam with these values. Prior to libsdformat8, the GetAny function returned boost::any, but it now returns std::any. This helper is used in SetParam to first check if a boost::any value contains a std::any, and if so, perform a std::any_cast<T>. Otherwise, it returns boost::any_cast<T>.
[in] | _value | Value to cast to type T. |
References PhysicsEngine::OnPhysicsMsg(), and PhysicsEngine::OnRequest().
void Collide | ( | ODECollision * | _collision1, |
ODECollision * | _collision2, | ||
dContactGeom * | _contactCollisions | ||
) |
Collide two collision objects.
[in] | _collision1 | First collision object. |
[in] | _collision2 | Second collision object. |
[in,out] | _contactCollision | Array of contacts. |
Referenced by ODEPhysics::GetType().
|
static |
Convert a string to a Friction_Model enum.
[in] | _fricModel | Friction model string. |
Referenced by ODEPhysics::GetType().
|
static |
Convert a Friction_Model enum to a string.
[in] | _fricModel | Friction_Model enum. |
|
static |
Convert an ODE mass to Inertial.
[out] | _intertial | Pointer to an Inertial object. |
[in] | _odeMass | Pointer to an ODE mass that will be converted. |
Referenced by ODEPhysics::GetType().
|
static |
|
static |
Convert a World_Solver_Type enum to a string.
[in] | _solverType | World_Solver_Type enum. |
Referenced by ODEPhysics::GetType().
|
static |
Convert a string to a World_Solver_Type enum.
[in] | _solverType | World solver type string. |
|
virtual |
Create a collision.
[in] | _shapeType | Type of collision to create. |
[in] | _link | Parent link. |
Implements PhysicsEngine.
Referenced by ODEPhysics::GetType().
|
inherited |
Create a collision.
[in] | _shapeType | Type of collision to create. |
[in] | _linkName | Name of the parent link. |
Create a new joint.
[in] | _type | Type of joint to create. |
[in] | _parent | Model parent. |
Implements PhysicsEngine.
Referenced by ODEPhysics::GetType().
Create a new body.
[in] | _parent | Parent model for the link. |
Implements PhysicsEngine.
Referenced by ODEPhysics::GetType().
Create a new model.
[in] | _base | Boost shared pointer to a new model. |
Reimplemented in DARTPhysics, and SimbodyPhysics.
Referenced by PhysicsEngine::UpdatePhysics().
|
virtual |
Create a physics::Shape object.
[in] | _shapeType | Type of shape to create. |
[in] | _collision | Collision parent. |
Implements PhysicsEngine.
Referenced by ODEPhysics::GetType().
|
virtual |
Debug print out of the physic engine state.
Implements PhysicsEngine.
Referenced by ODEPhysics::GetType().
|
virtual |
Finilize the physics engine.
Reimplemented from PhysicsEngine.
|
inlinevirtualinherited |
: Remove this function, and replace it with a more generic property map
access functions to set ODE parameters..
References PhysicsEngine::DebugPrint(), PhysicsEngine::GetContactManager(), PhysicsEngine::GetParam(), PhysicsEngine::SetParam(), and PhysicsEngine::World().
|
inherited |
Get a pointer to the contact manger.
Referenced by PhysicsEngine::GetAutoDisableFlag().
|
virtual |
Referenced by ODEPhysics::GetType().
|
virtual |
Referenced by ODEPhysics::GetType().
|
virtual |
|
virtual |
Referenced by ODEPhysics::GetType().
|
inherited |
|
virtual |
|
virtual |
Documentation inherited.
Reimplemented from PhysicsEngine.
|
inlineinherited |
returns a pointer to the PhysicsEngine::physicsUpdateMutex.
References PhysicsEngine::GetSDF(), and PhysicsEngine::physicsUpdateMutex.
|
inherited |
|
inherited |
Get a pointer to the SDF element for this physics engine.
Referenced by PhysicsEngine::GetPhysicsUpdateMutex().
|
virtual |
Referenced by ODEPhysics::GetType().
|
virtual |
Referenced by ODEPhysics::GetType().
|
virtual |
Referenced by ODEPhysics::GetType().
dSpaceID GetSpaceId | ( | ) | const |
|
virtual |
|
inherited |
|
inlinevirtual |
Return the physics engine type (ode|bullet|dart|simbody).
Implements PhysicsEngine.
References ODEPhysics::Collide(), ODEPhysics::ConvertFrictionModel(), ODEPhysics::ConvertMass(), ODEPhysics::ConvertWorldStepSolverType(), ODEPhysics::CreateCollision(), ODEPhysics::CreateJoint(), ODEPhysics::CreateLink(), ODEPhysics::CreateShape(), ODEPhysics::DebugPrint(), ODEPhysics::GetContactMaxCorrectingVel(), ODEPhysics::GetContactSurfaceLayer(), ODEPhysics::GetFrictionModel(), ODEPhysics::GetMaxContacts(), ODEPhysics::GetParam(), ODEPhysics::GetSORPGSIters(), ODEPhysics::GetSORPGSPreconIters(), ODEPhysics::GetSORPGSW(), ODEPhysics::GetSpaceId(), ODEPhysics::GetStepType(), ODEPhysics::GetWorldCFM(), ODEPhysics::GetWorldERP(), ODEPhysics::GetWorldId(), ODEPhysics::GetWorldStepSolverType(), ODEPhysics::OnPhysicsMsg(), ODEPhysics::OnRequest(), ODEPhysics::ProcessJointFeedback(), ODEPhysics::SetContactMaxCorrectingVel(), ODEPhysics::SetContactSurfaceLayer(), ODEPhysics::SetFrictionModel(), ODEPhysics::SetGravity(), ODEPhysics::SetMaxContacts(), ODEPhysics::SetParam(), ODEPhysics::SetSeed(), ODEPhysics::SetSORPGSIters(), ODEPhysics::SetSORPGSPreconIters(), ODEPhysics::SetSORPGSW(), ODEPhysics::SetStepType(), ODEPhysics::SetWorldCFM(), ODEPhysics::SetWorldERP(), and ODEPhysics::SetWorldStepSolverType().
|
inherited |
Get the simulation update period.
Referenced by PhysicsEngine::Reset().
|
virtual |
Referenced by ODEPhysics::GetType().
|
virtual |
Referenced by ODEPhysics::GetType().
dWorldID GetWorldId | ( | ) |
|
virtual |
Get solver type for world step.
Referenced by ODEPhysics::GetType().
|
virtual |
Initialize the physics engine.
Implements PhysicsEngine.
|
virtual |
Init the engine for threads.
Implements PhysicsEngine.
|
virtual |
Load the physics engine.
[in] | _sdf | Pointer to the SDF parameters. |
Reimplemented from PhysicsEngine.
|
protectedvirtual |
virtual callback for gztopic "~/physics".
[in] | _msg | Physics message. |
Reimplemented from PhysicsEngine.
Referenced by ODEPhysics::GetType().
|
protectedvirtual |
virtual callback for gztopic "~/request".
[in] | _msg | Request message. |
Reimplemented from PhysicsEngine.
Referenced by ODEPhysics::GetType().
void ProcessJointFeedback | ( | ODEJointFeedback * | _feedback | ) |
process joint feedbacks.
Referenced by ODEPhysics::GetType().
|
virtual |
Rest the physics engine.
Reimplemented from PhysicsEngine.
|
virtualinherited |
: Remove this function, and replace it with a more generic property map
Access functions to set ODE parameters.
[in] | _autoDisable | True to enable auto disabling of bodies. |
Referenced by PhysicsEngine::UpdatePhysics().
|
virtual |
Referenced by ODEPhysics::GetType().
|
virtual |
Referenced by ODEPhysics::GetType().
|
virtual |
Set friction model type.
[in] | _fricModel | Type of friction model. |
Referenced by ODEPhysics::GetType().
|
virtual |
Set the gravity vector.
[in] | _gravity | New gravity vector. |
Implements PhysicsEngine.
Referenced by ODEPhysics::GetType().
|
virtual |
: Remove this function, and replace it with a more generic property map
access functions to set ODE parameters
[in] | _maxContacts | Maximum number of contacts. |
Reimplemented from PhysicsEngine.
Referenced by ODEPhysics::GetType().
|
inherited |
|
virtual |
|
inherited |
|
virtual |
Set the random number seed for the physics engine.
[in] | _seed | The random number seed. |
Implements PhysicsEngine.
Referenced by ODEPhysics::GetType().
|
virtual |
Referenced by ODEPhysics::GetType().
|
virtual |
Referenced by ODEPhysics::GetType().
|
virtual |
Referenced by ODEPhysics::GetType().
|
virtual |
Set the step type (quick, world).
[in] | _type | The step type (quick or world). |
Referenced by ODEPhysics::GetType().
|
inherited |
Set target real time factor.
[in] | _factor | Target real time factor |
Referenced by PhysicsEngine::Reset().
|
virtual |
Referenced by ODEPhysics::GetType().
|
virtual |
Referenced by ODEPhysics::GetType().
|
virtual |
Set world step solver type.
[in] | _worldSolverType | Type of solver used by world step. |
Referenced by ODEPhysics::GetType().
|
virtual |
Update the physics engine collision.
This function works in tandem with PhysicsEngine::UpdatePhysics() to update the world. This function will be called even if the physics is disabled (when World::PhysicsEnabled()) returns false). Which updates are done in which of the two functions PhysicsEngine::UpdateCollision() and PhysicsEngine::UpdatePhysics() is to some extent left to the implementing physics engine. The intention is that PhysicsEngine::UpdateCollision() will update the collision states of the world, including contact information, and PhysicsEngine::UpdatePhysics() will update the dynamics of the world, i.e. advance the world and react to the collision state. However for some physics engines, both is done in one step, or providing the contact information separately in UpdateCollision() would mean double work, as it can't be avoided to be done again in PhysicsEngine::UpdatePhysics() - in this case it is better that PhysicsEngine::UpdateCollision does not actually update collision and contact information, and instead leaves it to UpdatePhysics(). There should be one exception however when it still does make this update: If World::PhysicsEnabled() returns false, and therefore PhysicsEngine::UpdatePhysics() will not be called in the update step, then PhysicsEngine::UpdateCollision will need to ensure that collision and contact information will still be updated.
Implements PhysicsEngine.
|
virtual |
Update the physics engine.
Will only be called if the physics are enabled, which is the case when World::PhysicsEnabled() returns true.
Reimplemented from PhysicsEngine.
Get a pointer to the world.
Referenced by PhysicsEngine::GetAutoDisableFlag().
|
protectedinherited |
Class that handles all contacts generated by the physics engine.
|
protectedinherited |
Real time update rate.
|
protectedinherited |
Node for communication.
|
protectedinherited |
Ignition node for communication.
|
protectedinherited |
Subscribe to the physics topic.
|
protectedinherited |
Mutex to protect the update cycle.
Referenced by PhysicsEngine::GetPhysicsUpdateMutex().
|
protectedinherited |
Real time update rate.
|
protectedinherited |
Subscribe to the request topic.
|
protectedinherited |
Response publisher.
|
protectedinherited |
Response publisher.
|
protectedinherited |
Our SDF values.
|
protectedinherited |
Target real time factor.
|
protectedinherited |
Pointer to the world.