Public Member Functions | Static Public Member Functions | Public Attributes | Protected Member Functions | List of all members
gazebo::physics::SimbodyPhysics Class Reference

Simbody physics engine. More...

#include <SimbodyPhysics.hh>

Inheritance diagram for gazebo::physics::SimbodyPhysics:
Inheritance graph
[legend]

Public Member Functions

 SimbodyPhysics (WorldPtr _world)
 Constructor. More...
 
virtual ~SimbodyPhysics ()
 Destructor. More...
 
virtual CollisionPtr CreateCollision (const std::string &_type, LinkPtr _body)
 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 _parent)
 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...
 
SimTK::MultibodySystem * GetDynamicsWorld () const
 Register a joint with the dynamics world. More...
 
virtual boost::any GetParam (const std::string &_key) const
 Get an parameter of the physics engine. More...
 
virtual std::string GetType () const
 Return the physics engine type (ode|bullet|dart|simbody). More...
 
virtual void Init ()
 Initialize the physics engine. More...
 
virtual void InitForThread ()
 Init the engine for threads. More...
 
void InitModel (const physics::ModelPtr _model)
 Add a Model to the Simbody system. More...
 
virtual void Load (sdf::ElementPtr _sdf)
 Load the physics engine. More...
 
virtual void Reset ()
 Rest the physics engine. More...
 
virtual void SetGravity (const gazebo::math::Vector3 &_gravity)
 Set the gavity vector. More...
 
virtual bool SetParam (const std::string &_key, const boost::any &_value)
 Set a parameter of the physics engine. More...
 
virtual void SetSeed (uint32_t _seed)
 Set the random number seed for the physics engine. More...
 
virtual void UpdateCollision ()
 Update the physics engine collision. More...
 
virtual void UpdatePhysics ()
 Update the physics engine. More...
 
- Public Member Functions inherited from gazebo::physics::PhysicsEngine
 PhysicsEngine (WorldPtr _world)
 Default constructor. More...
 
virtual ~PhysicsEngine ()
 Destructor. More...
 
CollisionPtr CreateCollision (const std::string &_shapeType, const std::string &_linkName)
 Create a collision. More...
 
virtual bool GetAutoDisableFlag ()
 : Remove this function, and replace it with a more generic property map More...
 
ContactManagerGetContactManager () const
 Get a pointer to the contact manger. More...
 
virtual double GetContactMaxCorrectingVel ()
 : Remove this function, and replace it with a more generic property map. More...
 
virtual double GetContactSurfaceLayer ()
 : Remove this function, and replace it with a more generic property map. More...
 
virtual math::Vector3 GetGravity () const
 Return the gavity vector. More...
 
virtual unsigned int GetMaxContacts ()
 : Remove this function, and replace it with a more generic property map. More...
 
double GetMaxStepSize () const
 Get max step size. More...
 
boost::recursive_mutex * GetPhysicsUpdateMutex () const
 returns a pointer to the PhysicsEngine::physicsUpdateMutex. More...
 
double GetRealTimeUpdateRate () const
 Get real time update rate. More...
 
double GetTargetRealTimeFactor () const
 Get target real time factor. More...
 
double GetUpdatePeriod ()
 Get the simulation update period. More...
 
virtual double GetWorldCFM ()
 : Remove this function, and replace it with a more generic property map More...
 
virtual double GetWorldERP ()
 : Remove this function, and replace it with a more generic property map More...
 
virtual void SetAutoDisableFlag (bool _autoDisable)
 : Remove this function, and replace it with a more generic property map More...
 
virtual void SetContactMaxCorrectingVel (double _vel)
 : Remove this function, and replace it with a more generic property map More...
 
virtual void SetContactSurfaceLayer (double _layerDepth)
 : Remove this function, and replace it with a more generic property map More...
 
virtual void SetMaxContacts (unsigned int _maxContacts)
 : Remove this function, and replace it with a more generic property map More...
 
void SetMaxStepSize (double _stepSize)
 Set max step size. More...
 
void SetRealTimeUpdateRate (double _rate)
 Set real time update rate. More...
 
void SetTargetRealTimeFactor (double _factor)
 Set target real time factor. More...
 
virtual void SetWorldCFM (double _cfm)
 : Remove this function, and replace it with a more generic property map More...
 
virtual void SetWorldERP (double _erp)
 : Remove this function, and replace it with a more generic property map More...
 

Static Public Member Functions

static SimTK::Transform GetPose (sdf::ElementPtr _element)
 If the given element contains a <pose> element, return it as a Transform. More...
 
static std::string GetTypeString (unsigned int _type)
 Convert Base::GetType() to string, this is needed by the MultibodyGraphMaker. More...
 
static std::string GetTypeString (physics::Base::EntityType _type)
 Convert Base::GetType() to string, this is needed by the MultibodyGraphMaker. More...
 
static SimTK::Transform Pose2Transform (const math::Pose &_pose)
 Convert the given pose in x,y,z,thetax,thetay,thetaz format to a Simbody Transform. More...
 
static SimTK::Quaternion QuadToQuad (const math::Quaternion &_q)
 Convert gazebo::math::Quaternion to SimTK::Quaternion. More...
 
static math::Quaternion QuadToQuad (const SimTK::Quaternion &_q)
 Convert SimTK::Quaternion to gazebo::math::Quaternion. More...
 
static math::Pose Transform2Pose (const SimTK::Transform &_xAB)
 Convert a Simbody transform to a pose in x,y,z, thetax,thetay,thetaz format. More...
 
static math::Vector3 Vec3ToVector3 (const SimTK::Vec3 &_v)
 Convert SimTK::Vec3 to gazebo::math::Vector3. More...
 
static SimTK::Vec3 Vector3ToVec3 (const math::Vector3 &_v)
 Convert gazebo::math::Vector3 to SimTK::Vec3. More...
 

Public Attributes

SimTK::CompliantContactSubsystem contact
 
SimTK::Force::DiscreteForces discreteForces
 
SimTK::GeneralForceSubsystem forces
 
SimTK::Force::Gravity gravity
 
SimTK::Integrator * integ
 
SimTK::SimbodyMatterSubsystem matter
 
bool simbodyPhysicsInitialized
 true if initialized More...
 
bool simbodyPhysicsStepped
 
SimTK::MultibodySystem system
 
SimTK::ContactTrackerSubsystem tracker
 

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...
 

Additional Inherited Members

- Protected Attributes inherited from gazebo::physics::PhysicsEngine
ContactManagercontactManager
 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...
 
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...
 
sdf::ElementPtr sdf
 Our SDF values. More...
 
double targetRealTimeFactor
 Target real time factor. More...
 
WorldPtr world
 Pointer to the world. More...
 

Detailed Description

Simbody physics engine.

Constructor & Destructor Documentation

gazebo::physics::SimbodyPhysics::SimbodyPhysics ( WorldPtr  _world)

Constructor.

virtual gazebo::physics::SimbodyPhysics::~SimbodyPhysics ( )
virtual

Destructor.

Member Function Documentation

virtual CollisionPtr gazebo::physics::SimbodyPhysics::CreateCollision ( const std::string &  _shapeType,
LinkPtr  _link 
)
virtual

Create a collision.

Parameters
[in]_shapeTypeType of collision to create.
[in]_linkParent link.

Implements gazebo::physics::PhysicsEngine.

virtual JointPtr gazebo::physics::SimbodyPhysics::CreateJoint ( const std::string &  _type,
ModelPtr  _parent 
)
virtual

Create a new joint.

Parameters
[in]_typeType of joint to create.
[in]_parentModel parent.

Implements gazebo::physics::PhysicsEngine.

virtual LinkPtr gazebo::physics::SimbodyPhysics::CreateLink ( ModelPtr  _parent)
virtual

Create a new body.

Parameters
[in]_parentParent model for the link.

Implements gazebo::physics::PhysicsEngine.

virtual ModelPtr gazebo::physics::SimbodyPhysics::CreateModel ( BasePtr  _base)
virtual

Create a new model.

Parameters
[in]_baseBoost shared pointer to a new model.

Reimplemented from gazebo::physics::PhysicsEngine.

virtual ShapePtr gazebo::physics::SimbodyPhysics::CreateShape ( const std::string &  _shapeType,
CollisionPtr  _collision 
)
virtual

Create a physics::Shape object.

Parameters
[in]_shapeTypeType of shape to create.
[in]_collisionCollision parent.

Implements gazebo::physics::PhysicsEngine.

virtual void gazebo::physics::SimbodyPhysics::DebugPrint ( ) const
virtual

Debug print out of the physic engine state.

Implements gazebo::physics::PhysicsEngine.

virtual void gazebo::physics::SimbodyPhysics::Fini ( )
virtual

Finilize the physics engine.

Reimplemented from gazebo::physics::PhysicsEngine.

SimTK::MultibodySystem* gazebo::physics::SimbodyPhysics::GetDynamicsWorld ( ) const

Register a joint with the dynamics world.

virtual boost::any gazebo::physics::SimbodyPhysics::GetParam ( const std::string &  _key) const
virtual

Get an parameter of the physics engine.

Parameters
[in]_attrString key
See Also
SetParam
Returns
The value of the parameter

Reimplemented from gazebo::physics::PhysicsEngine.

static SimTK::Transform gazebo::physics::SimbodyPhysics::GetPose ( sdf::ElementPtr  _element)
static

If the given element contains a <pose> element, return it as a Transform.

Otherwise return the identity Transform. If there is more than one <pose> element, only the first one is processed.

virtual std::string gazebo::physics::SimbodyPhysics::GetType ( ) const
virtual

Return the physics engine type (ode|bullet|dart|simbody).

Returns
Type of the physics engine.

Implements gazebo::physics::PhysicsEngine.

static std::string gazebo::physics::SimbodyPhysics::GetTypeString ( unsigned int  _type)
static

Convert Base::GetType() to string, this is needed by the MultibodyGraphMaker.

Parameters
[in]_typeJoint type returned by Joint::GetType().
Returns
a hard-coded string needed by the MultibodyGraphMaker.
static std::string gazebo::physics::SimbodyPhysics::GetTypeString ( physics::Base::EntityType  _type)
static

Convert Base::GetType() to string, this is needed by the MultibodyGraphMaker.

Parameters
[in]_typeJoint type returned by Joint::GetType().
Returns
a hard-coded string needed by the MultibodyGraphMaker.
virtual void gazebo::physics::SimbodyPhysics::Init ( )
virtual

Initialize the physics engine.

Implements gazebo::physics::PhysicsEngine.

virtual void gazebo::physics::SimbodyPhysics::InitForThread ( )
virtual

Init the engine for threads.

Implements gazebo::physics::PhysicsEngine.

void gazebo::physics::SimbodyPhysics::InitModel ( const physics::ModelPtr  _model)

Add a Model to the Simbody system.

Parameters
[in]_modelPointer to the model to add into Simbody.
virtual void gazebo::physics::SimbodyPhysics::Load ( sdf::ElementPtr  _sdf)
virtual

Load the physics engine.

Parameters
[in]_sdfPointer to the SDF parameters.

Reimplemented from gazebo::physics::PhysicsEngine.

virtual void gazebo::physics::SimbodyPhysics::OnPhysicsMsg ( ConstPhysicsPtr &  _msg)
protectedvirtual

virtual callback for gztopic "~/physics".

Parameters
[in]_msgPhysics message.

Reimplemented from gazebo::physics::PhysicsEngine.

virtual void gazebo::physics::SimbodyPhysics::OnRequest ( ConstRequestPtr &  _msg)
protectedvirtual

virtual callback for gztopic "~/request".

Parameters
[in]_msgRequest message.

Reimplemented from gazebo::physics::PhysicsEngine.

static SimTK::Transform gazebo::physics::SimbodyPhysics::Pose2Transform ( const math::Pose _pose)
static

Convert the given pose in x,y,z,thetax,thetay,thetaz format to a Simbody Transform.

The rotation angles are interpreted as a body-fixed sequence, meaning we rotation about x, then about the new y, then about the now twice-rotated z.

Parameters
[in]_poseGazeb's math::Pose object
Returns
Simbody's SimTK::Transform object
static SimTK::Quaternion gazebo::physics::SimbodyPhysics::QuadToQuad ( const math::Quaternion _q)
static

Convert gazebo::math::Quaternion to SimTK::Quaternion.

Parameters
[in]_qGazeb's math::Quaternion object
Returns
Simbody's SimTK::Quaternion object
static math::Quaternion gazebo::physics::SimbodyPhysics::QuadToQuad ( const SimTK::Quaternion &  _q)
static

Convert SimTK::Quaternion to gazebo::math::Quaternion.

Parameters
[in]_qSimbody's SimTK::Quaternion object
Returns
Gazeb's math::Quaternion object
virtual void gazebo::physics::SimbodyPhysics::Reset ( )
virtual

Rest the physics engine.

Reimplemented from gazebo::physics::PhysicsEngine.

virtual void gazebo::physics::SimbodyPhysics::SetGravity ( const gazebo::math::Vector3 _gravity)
virtual

Set the gavity vector.

Parameters
[in]_gravityNew gravity vector.

Implements gazebo::physics::PhysicsEngine.

virtual bool gazebo::physics::SimbodyPhysics::SetParam ( const std::string &  _key,
const boost::any &  _value 
)
virtual

Set a parameter of the physics engine.

See SetParam documentation for descriptions of duplicate parameters.

Parameters
[in]_keyString key Below is a list of _key parameter definitions:
  1. "solver_type" (string) - returns solver used by engine, e.g. "sequential_impulse' for Bullet, "quick" for ODE "Featherstone and Lemkes" for DART and "Spatial Algebra and Elastic Foundation" for Simbody. -# "cfm" (double) - global CFM -# "erp" (double) - global ERP -# "precon_iters" (bool) - precondition iterations (experimental).
  2. "iters" (int) - number of LCP PGS iterations. If sor_lcp_tolerance is negative, full iteration count is executed. Otherwise, PGS may stop iteration early if sor_lcp_tolerance is satisfied by the total RMS residual.
  3. "sor" (double) - relaxation parameter for Projected Gauss-Seidel (PGS) updates.
  4. "contact_max_correcting_vel" (double) - truncates correction impulses from ERP by this value.
  5. "contact_surface_layer" (double) - ERP is 0 for interpenetration depths below this value.
  6. "max_contacts" (int) - max number of contact constraints between any pair of collision bodies.
  7. "min_step_size" (double) - minimum internal step size. (defined but not used in ode).
  8. "max_step_size" (double) - maximum physics step size when physics update step must return.
[in]_valueThe value to set to
Returns
true if SetParam is successful, false if operation fails.

Reimplemented from gazebo::physics::PhysicsEngine.

virtual void gazebo::physics::SimbodyPhysics::SetSeed ( uint32_t  _seed)
virtual

Set the random number seed for the physics engine.

Parameters
[in]_seedThe random number seed.

Implements gazebo::physics::PhysicsEngine.

static math::Pose gazebo::physics::SimbodyPhysics::Transform2Pose ( const SimTK::Transform &  _xAB)
static

Convert a Simbody transform to a pose in x,y,z, thetax,thetay,thetaz format.

Parameters
[in]_xABSimbody's SimTK::Transform object
Returns
Gazeb's math::Pose object
virtual void gazebo::physics::SimbodyPhysics::UpdateCollision ( )
virtual

Update the physics engine collision.

Implements gazebo::physics::PhysicsEngine.

virtual void gazebo::physics::SimbodyPhysics::UpdatePhysics ( )
virtual

Update the physics engine.

Reimplemented from gazebo::physics::PhysicsEngine.

static math::Vector3 gazebo::physics::SimbodyPhysics::Vec3ToVector3 ( const SimTK::Vec3 &  _v)
static

Convert SimTK::Vec3 to gazebo::math::Vector3.

Parameters
[in]_vSimbody's SimTK::Vec3 object
Returns
Gazeb's math::Vector3 object
static SimTK::Vec3 gazebo::physics::SimbodyPhysics::Vector3ToVec3 ( const math::Vector3 _v)
static

Convert gazebo::math::Vector3 to SimTK::Vec3.

Parameters
[in]_vGazeb's math::Vector3 object
Returns
Simbody's SimTK::Vec3 object

Member Data Documentation

SimTK::CompliantContactSubsystem gazebo::physics::SimbodyPhysics::contact
SimTK::Force::DiscreteForces gazebo::physics::SimbodyPhysics::discreteForces
SimTK::GeneralForceSubsystem gazebo::physics::SimbodyPhysics::forces
SimTK::Force::Gravity gazebo::physics::SimbodyPhysics::gravity
SimTK:: Integrator* gazebo::physics::SimbodyPhysics::integ
SimTK::SimbodyMatterSubsystem gazebo::physics::SimbodyPhysics::matter
bool gazebo::physics::SimbodyPhysics::simbodyPhysicsInitialized

true if initialized

bool gazebo::physics::SimbodyPhysics::simbodyPhysicsStepped
SimTK::MultibodySystem gazebo::physics::SimbodyPhysics::system
SimTK::ContactTrackerSubsystem gazebo::physics::SimbodyPhysics::tracker

The documentation for this class was generated from the following file: