Go to the documentation of this file.
17 #ifndef GAZEBO_PHYSICS_SIMBODY_SIMBODYLINK_HH_
18 #define GAZEBO_PHYSICS_SIMBODY_SIMBODYLINK_HH_
47 public:
virtual void Load(sdf::ElementPtr _ptr);
50 public:
virtual void Init();
53 public:
virtual void Fini();
56 public:
virtual void OnPoseChange();
59 public:
virtual void SetEnabled(
bool enable)
const;
62 public:
virtual bool GetEnabled()
const;
65 public:
virtual void SetLinearVel(
const ignition::math::Vector3d &_vel);
68 public:
virtual void SetAngularVel(
const ignition::math::Vector3d &_vel);
71 public:
virtual void SetForce(
const ignition::math::Vector3d &_force);
74 public:
virtual void SetTorque(
const ignition::math::Vector3d &_force);
77 public:
virtual ignition::math::Vector3d WorldLinearVel(
78 const ignition::math::Vector3d &_vector3)
const;
81 public:
virtual ignition::math::Vector3d WorldLinearVel(
82 const ignition::math::Vector3d &_offset,
83 const ignition::math::Quaterniond &_q)
const;
86 public:
virtual ignition::math::Vector3d WorldCoGLinearVel()
const;
89 public:
virtual ignition::math::Vector3d WorldAngularVel()
const;
92 public:
virtual ignition::math::Vector3d WorldForce()
const;
95 public:
virtual ignition::math::Vector3d WorldTorque()
const;
98 public:
virtual void SetGravityMode(
bool _mode);
101 public:
virtual bool GetGravityMode()
const;
104 public:
virtual void SetSelfCollide(
bool _collide);
107 public:
virtual void SetLinearDamping(
double _damping);
110 public:
virtual void SetAngularDamping(
double _damping);
113 public:
virtual void AddForce(
const ignition::math::Vector3d &_force);
116 public:
virtual void AddRelativeForce(
117 const ignition::math::Vector3d &_force);
120 public:
virtual void AddForceAtWorldPosition(
121 const ignition::math::Vector3d &_force,
122 const ignition::math::Vector3d &_pos);
125 public:
virtual void AddForceAtRelativePosition(
126 const ignition::math::Vector3d &_force,
127 const ignition::math::Vector3d &_relpos);
130 public:
virtual void AddLinkForce(
const ignition::math::Vector3d &_force,
131 const ignition::math::Vector3d &_offset =
132 ignition::math::Vector3d::Zero);
135 public:
virtual void AddTorque(
const ignition::math::Vector3d &_torque);
138 public:
virtual void AddRelativeTorque(
139 const ignition::math::Vector3d &_torque);
142 public:
virtual void SetAutoDisable(
bool _disable);
145 public:
virtual void SaveSimbodyState(
const SimTK::State &_state);
148 public:
virtual void RestoreSimbodyState(SimTK::State &_state);
155 public:
virtual void SetLinkStatic(
bool _static);
160 public: SimTK::MassProperties GetMassProperties()
const;
162 public: SimTK::MassProperties GetEffectiveMassProps(
163 int _numFragments)
const;
167 public:
void SetDirtyPose(
const ignition::math::Pose3d &_pose);
170 public:
virtual void UpdateMass();
174 private:
void ProcessSetGravityMode();
178 private:
void ProcessSetLinkStatic();
199 private:
bool gravityMode;
202 private:
bool staticLinkDirty;
205 private:
bool gravityModeDirty;
208 private:
bool staticLink;
217 private: std::vector<double> simbodyQ;
220 private: std::vector<double> simbodyU;
Simbody Link class.
Definition: SimbodyLink.hh:38
void SetDirtyPose(const ignition::math::Pose3d &_pose)
Set the dirty pose.
virtual void UpdateMass()
Update the mass matrix.
virtual void SetLinearDamping(double _damping)
Set the linear damping factor.
virtual ~SimbodyLink()
Destructor.
virtual void SetGravityMode(bool _mode)
Set whether gravity affects this body.
virtual void AddRelativeTorque(const ignition::math::Vector3d &_torque)
Add a torque to the body, components are relative to the body's own frame of reference.
virtual void AddRelativeForce(const ignition::math::Vector3d &_force)
Add a force to the body, components are relative to the body's own frame of reference.
virtual ignition::math::Vector3d WorldLinearVel() const
Get the linear velocity of the origin of the link frame, expressed in the world frame.
Forward declarations for the common classes.
Definition: Animation.hh:26
SimTK::MassProperties GetEffectiveMassProps(int _numFragments) const
virtual void Load(sdf::ElementPtr _ptr)
Load the body based on an SDF element.
virtual void SetAutoDisable(bool _disable)
Allow the link to auto disable.
void enable()
Enable sensors.
virtual void SetEnabled(bool enable) const
Set whether this body is enabled.
virtual void SetLinkStatic(bool _static)
If the inboard body of this link is ground, simply lock the inboard joint to freeze it to ground.
virtual void Init()
Initialize the body.
virtual bool GetEnabled() const
Get whether this body is enabled in the physics engine.
bool mustBeBaseLink
: Force this link to be a base body, where its inboard body is the world with 6DOF.
Definition: SimbodyLink.hh:182
Link class defines a rigid body entity, containing information on inertia, visual and collision prope...
Definition: Link.hh:58
virtual void AddForce(const ignition::math::Vector3d &_force)
Add a force to the body.
virtual void SetSelfCollide(bool _collide)
Set whether this body will collide with others in the model.
virtual void SetTorque(const ignition::math::Vector3d &_force)
Set the torque applied to the body.
bool physicsInitialized
Definition: SimbodyLink.hh:189
virtual ignition::math::Vector3d WorldTorque() const
Get the torque applied to the body about the center of mass in world frame coordinates.
SimTK::MassProperties GetMassProperties() const
Convert Gazebo Inertia to Simbody MassProperties Where Simbody MassProperties contains mass,...
std::vector< SimTK::Constraint::Weld > slaveWelds
Definition: SimbodyLink.hh:196
std::vector< SimTK::MobilizedBody > slaveMobods
Definition: SimbodyLink.hh:193
SimbodyLink(EntityPtr _parent)
Constructor.
boost::shared_ptr< Connection > ConnectionPtr
Definition: CommonTypes.hh:134
GAZEBO_VISIBLE void Init(google::protobuf::Message &_message, const std::string &_id="")
Initialize a message.
virtual void Fini()
Finalize the body.
virtual void RestoreSimbodyState(SimTK::State &_state)
boost::shared_ptr< Entity > EntityPtr
Definition: PhysicsTypes.hh:85
virtual void SetLinearVel(const ignition::math::Vector3d &_vel)
Set the linear velocity of the body.
virtual ignition::math::Vector3d WorldCoGLinearVel() const
Get the linear velocity at the body's center of gravity in the world frame.
virtual void AddTorque(const ignition::math::Vector3d &_torque)
Add a torque to the body.
virtual bool GetGravityMode() const
Get the gravity mode.
boost::shared_ptr< SimbodyPhysics > SimbodyPhysicsPtr
Definition: SimbodyTypes.hh:40
virtual void SetAngularVel(const ignition::math::Vector3d &_vel)
Set the angular velocity of the body.
virtual void OnPoseChange()
This function is called when the entity's (or one of its parents) pose of the parent has changed.
virtual void AddLinkForce(const ignition::math::Vector3d &_force, const ignition::math::Vector3d &_offset=ignition::math::Vector3d::Zero)
Add a force expressed in the link frame.
virtual void SaveSimbodyState(const SimTK::State &_state)
virtual void AddForceAtWorldPosition(const ignition::math::Vector3d &_force, const ignition::math::Vector3d &_pos)
Add a force to the body using a global position.
virtual void AddForceAtRelativePosition(const ignition::math::Vector3d &_force, const ignition::math::Vector3d &_relpos)
Add a force (in world frame coordinates) to the body at a position relative to the center of mass whi...
virtual void SetForce(const ignition::math::Vector3d &_force)
Set the force applied to the body.
virtual ignition::math::Vector3d WorldForce() const
Get the force applied to the body in the world frame.
SimTK::MobilizedBody masterMobod
Definition: SimbodyLink.hh:186
virtual void SetAngularDamping(double _damping)
Set the angular damping factor.
virtual ignition::math::Vector3d WorldAngularVel() const
Get the angular velocity of the entity in the world frame.
Simbody wrapper forward declarations and typedefs.