17#ifndef GAZEBO_PHYSICS_LINK_HH_
18#define GAZEBO_PHYSICS_LINK_HH_
29#include <ignition/math/Matrix3.hh>
69 public:
virtual void Load(sdf::ElementPtr _sdf);
72 public:
virtual void Init();
95 public:
void SetScale(
const ignition::math::Vector3d &_scale);
158 const ignition::math::Vector3d &_vel) = 0;
163 const ignition::math::Vector3d &_vel) = 0;
168 const ignition::math::Vector3d &_force) = 0;
173 const ignition::math::Vector3d &_torque) = 0;
177 public:
virtual void AddForce(
const ignition::math::Vector3d &_force) = 0;
183 const ignition::math::Vector3d &_force) = 0;
189 const ignition::math::Vector3d &_force,
190 const ignition::math::Vector3d &_pos) = 0;
201 const ignition::math::Vector3d &_force,
202 const ignition::math::Vector3d &_relPos) = 0;
211 const ignition::math::Vector3d &_force,
212 const ignition::math::Vector3d &_offset =
213 ignition::math::Vector3d::Zero) = 0;
218 const ignition::math::Vector3d &_torque) = 0;
224 const ignition::math::Vector3d &_torque) = 0;
245 const ignition::math::Vector3d &_offset)
const = 0;
255 const ignition::math::Vector3d &_offset,
256 const ignition::math::Quaterniond &_q)
const = 0;
306 public:
virtual ignition::math::Vector3d
WorldForce()
const = 0;
346 public:
CollisionPtr GetCollisionById(
unsigned int _id)
const;
419 std::function<
void (
bool)> _subscriber);
447 using Base::RemoveChild;
453 const ignition::math::Pose3d &_offset);
530 using Entity::SetStatic;
543 const ignition::math::Pose3d &_worldReferenceFrameSrc,
544 const ignition::math::Pose3d &_worldReferenceFrameDst,
545 const bool _preserveWorldVelocity =
false);
562 const LinkPtr &_originalParentLink,
563 Link_V &_connectedLinks,
bool _fistLink =
false);
600 public:
bool VisualId(
const std::string &_visName, uint32_t &_visualId)
608 ignition::math::Pose3d &_pose)
const;
615 const ignition::math::Pose3d &_pose);
626 private:
void PublishData();
630 private:
void LoadCollision(sdf::ElementPtr _sdf);
634 private:
void LoadLight(sdf::ElementPtr _sdf);
638 private:
void SetInertialFromCollisions();
642 private:
void OnCollision(ConstContactsPtr &_msg);
645 private:
void ParseVisuals();
651 private:
bool ContainsLink(
const Link_V &_vector,
const LinkPtr &_value);
655 private:
void UpdateVisualGeomSDF(
const ignition::math::Vector3d &_scale);
658 private:
void UpdateVisualMsg();
663 private:
void OnWrenchMsg(ConstWrenchPtr &_msg);
667 private:
void ProcessWrenchMsg(
const msgs::Wrench &_msg);
671 private:
void LoadBattery(
const sdf::ElementPtr _sdf);
686 protected:
bool initialized =
false;
689 private: std::unique_ptr<LinkPrivate> dataPtr;
Information for use in an update event.
Definition: UpdateInfo.hh:31
virtual void RemoveChild(unsigned int _id)
Remove a child from this entity.
virtual void Update()
Update the object.
Definition: Base.hh:171
Base class for all physics objects in Gazebo.
Definition: Entity.hh:53
virtual void Reset()
Reset the entity.
void SetStatic(const bool &_static)
Set whether this entity is static: immovable.
Store state information of a physics::Link object.
Definition: LinkState.hh:48
Link class defines a rigid body entity, containing information on inertia, visual and collision prope...
Definition: Link.hh:59
void Fini()
Finalize the body.
const ignition::math::Vector3d WorldWindLinearVel() const
Returns this link's wind velocity in the world coordinate frame.
ModelPtr GetModel() const
Get the model that this body belongs to.
InertialPtr inertial
Inertial properties.
Definition: Link.hh:677
msgs::Visual GetVisualMessage(const std::string &_name) const
Returns the visual message specified by its name.
ignition::math::Vector3d WorldLinearAccel() const
Get the linear acceleration of the body in the world frame.
common::BatteryPtr Battery(const std::string &_name) const
Get a battery by name.
virtual void SetGravityMode(bool _mode)=0
Set whether gravity affects this body.
virtual void AddForceAtRelativePosition(const ignition::math::Vector3d &_force, const ignition::math::Vector3d &_relPos)=0
Add a force (in world frame coordinates) to the body at a position relative to the center of mass whi...
virtual void SetEnabled(bool _enable) const =0
Set whether this body is enabled.
common::BatteryPtr Battery(const size_t _index) const
Get a battery based on an index.
ignition::math::Vector3d RelativeAngularVel() const
Get the angular velocity of the body.
const ignition::math::Vector3d RelativeWindLinearVel() const
Returns this link's wind velocity.
virtual bool SetSelected(bool _set)
Set whether this entity has been selected by the user through the gui.
virtual void SetLinearDamping(double _damping)=0
Set the linear damping factor.
virtual ignition::math::Vector3d WorldCoGLinearVel() const =0
Get the linear velocity at the body's center of gravity in the world frame.
virtual ignition::math::Vector3d WorldForce() const =0
Get the force applied to the body in the world frame.
std::vector< ignition::math::Pose3d > attachedModelsOffset
Offsets for the attached models.
Definition: Link.hh:683
void Update(const common::UpdateInfo &_info)
Update the collision.
virtual ignition::math::Vector3d WorldTorque() const =0
Get the torque applied to the body about the center of mass in world frame coordinates.
Link_V GetParentJointsLinks() const
Returns a vector of parent Links connected by joints.
ignition::math::Vector3d RelativeLinearVel() const
Get the linear velocity of the body.
virtual void SetWindMode(const bool _mode)
Set whether wind affects this body.
std::map< uint32_t, msgs::Visual > Visuals_M
Definition: Link.hh:619
virtual void UpdateMass()
Update the mass matrix.
Definition: Link.hh:471
virtual void SetKinematic(const bool &_kinematic)
\TODO Implement this function.
virtual void AddLinkForce(const ignition::math::Vector3d &_force, const ignition::math::Vector3d &_offset=ignition::math::Vector3d::Zero)=0
Add a force expressed in the link frame.
std::string GetSensorName(unsigned int _index) const
Get sensor name.
ignition::math::Pose3d WorldCoGPose() const
Get the pose of the body's center of gravity in the world coordinate frame.
bool VisualId(const std::string &_visName, uint32_t &_visualId) const
Get the unique ID of a visual.
void SetInertial(const InertialPtr &_inertial)
Set the mass of the link.
ignition::math::Vector3d RelativeAngularAccel() const
Get the angular acceleration of the body.
void AddParentJoint(JointPtr _joint)
Joints that have this Link as a child Link.
bool SetVisualPose(const uint32_t _id, const ignition::math::Pose3d &_pose)
Set the pose of a visual.
void Reset()
Reset the link.
virtual void Init()
Initialize the body.
virtual void RemoveChild(EntityPtr _child)
ignition::math::Vector3d RelativeTorque() const
Get the torque applied to the body.
Collision_V GetCollisions() const
Get all the child collisions.
const Visuals_M & Visuals() const
Return the link visual elements.
void ResetPhysicsStates()
Reset the velocity, acceleration, force and torque of link.
void SetWindEnabled(const bool _enable)
Enable/disable wind for this link.
virtual bool GetKinematic() const
\TODO Implement this function.
Definition: Link.hh:392
void AddChildJoint(JointPtr _joint)
Joints that have this Link as a parent Link.
virtual void SetAutoDisable(bool _disable)=0
Allow the link to auto disable.
virtual void UpdateSurface()
Update surface parameters.
Definition: Link.hh:474
Joint_V GetParentJoints() const
Get the parent joints.
void SetState(const LinkState &_state)
Set the current link state.
event::ConnectionPtr ConnectEnabled(std::function< void(bool)> _subscriber)
Connect to the add entity signal.
void ProcessMsg(const msgs::Link &_msg)
Update parameters from a message.
ignition::math::Vector3d WorldAngularMomentum() const
Get the angular momentum of the body CoG in the world frame, which is computed as (I * w),...
InertialPtr GetInertial() const
Get the inertia of the link.
Definition: Link.hh:323
virtual void RegisterIntrospectionItems()
Register items in the introspection service.
virtual void SetLinkStatic(bool _static)=0
Freeze link to ground (inertial frame).
virtual ignition::math::Box BoundingBox() const
Get the bounding box for the link and all the child elements.
void FillMsg(msgs::Link &_msg)
Fill a link message.
unsigned int GetSensorCount() const
Get sensor count.
virtual void AddForce(const ignition::math::Vector3d &_force)=0
Add a force to the body.
double GetWorldEnergyKinetic() const
Returns this link's kinetic energy computed using link's CoG velocity in the inertial (world) frame.
virtual bool GetGravityMode() const =0
Get the gravity mode.
void UpdateWind(const common::UpdateInfo &_info)
Update the wind.
ignition::math::Vector3d RelativeForce() const
Get the force applied to the body.
virtual ignition::math::Vector3d WorldLinearVel(const ignition::math::Vector3d &_offset, const ignition::math::Quaterniond &_q) const =0
Get the linear velocity of a point on the body in the world frame, using an offset expressed in an ar...
virtual void AddRelativeForce(const ignition::math::Vector3d &_force)=0
Add a force to the body, components are relative to the body's own frame of reference.
ignition::math::Vector3d RelativeLinearAccel() const
Get the linear acceleration of the body.
bool FindAllConnectedLinksHelper(const LinkPtr &_originalParentLink, Link_V &_connectedLinks, bool _fistLink=false)
Helper function to find all connected links of a link based on parent/child relations of joints.
double GetLinearDamping() const
Get the linear damping factor.
ignition::math::Matrix3d WorldInertiaMatrix() const
Get the inertia matrix in the world frame.
void RemoveCollision(const std::string &_name)
Remove a collision from the link.
virtual void SetAngularVel(const ignition::math::Vector3d &_vel)=0
Set the angular velocity of the body.
bool GetSelfCollide() const
Get Self-Collision Flag.
virtual bool WindMode() const
Get the wind mode.
virtual ignition::math::Vector3d WorldLinearVel(const ignition::math::Vector3d &_offset) const =0
Get the linear velocity of a point on the body in the world frame, using an offset expressed in a bod...
virtual void SetForce(const ignition::math::Vector3d &_force)=0
Set the force applied to the body.
double GetWorldEnergy() const
Returns this link's total energy, or sum of Link::GetWorldEnergyPotential() and Link::GetWorldEnergyK...
void DetachAllStaticModels()
Detach all static models from this link.
void RemoveChildJoint(const std::string &_jointName)
Remove Joints that have this Link as a parent Link.
bool VisualPose(const uint32_t _id, ignition::math::Pose3d &_pose) const
Get the pose of a visual.
virtual void SetStatic(const bool &_static)
virtual void SetAngularDamping(double _damping)=0
Set the angular damping factor.
double GetWorldEnergyPotential() const
Returns this link's potential energy, based on position in world frame and gravity.
ignition::math::Pose3d WorldInertialPose() const
Get the world pose of the link inertia (cog position and Moment of Inertia frame).
Joint_V GetChildJoints() const
Get the child joints.
virtual void AddForceAtWorldPosition(const ignition::math::Vector3d &_force, const ignition::math::Vector3d &_pos)=0
Add a force to the body using a global position.
virtual void AddRelativeTorque(const ignition::math::Vector3d &_torque)=0
Add a torque to the body, components are relative to the body's own frame of reference.
size_t BatteryCount() const
Get the number of batteries in this link.
Link(EntityPtr _parent)
Constructor.
virtual void Load(sdf::ElementPtr _sdf)
Load the body based on an SDF element.
void SetPublishData(bool _enable)
Enable/Disable link data publishing.
void SetLaserRetro(float _retro)
Set the laser retro reflectiveness.
void AttachStaticModel(ModelPtr &_model, const ignition::math::Pose3d &_offset)
Attach a static model to this link.
virtual void UpdateParameters(sdf::ElementPtr _sdf)
Update the parameters using new sdf values.
ignition::math::Vector3d WorldAngularAccel() const
Get the angular acceleration of the body in the world frame, which is computed as (I^-1 * (T - w x L)...
void DetachStaticModel(const std::string &_modelName)
Detach a static model from this link.
virtual void SetTorque(const ignition::math::Vector3d &_torque)=0
Set the torque applied to the body.
double GetAngularDamping() const
Get the angular damping factor.
virtual ~Link()
Destructor.
virtual void OnPoseChange()
This function is called when the entity's (or one of its parents) pose of the parent has changed.
virtual bool GetEnabled() const =0
Get whether this body is enabled in the physics engine.
CollisionPtr GetCollision(const std::string &_name)
Get a child collision by name.
virtual void AddTorque(const ignition::math::Vector3d &_torque)=0
Add a torque to the body.
void SetCollideMode(const std::string &_mode)
Set the collide mode of the body.
CollisionPtr GetCollision(unsigned int _index) const
Get a child collision by index.
virtual void SetLinearVel(const ignition::math::Vector3d &_vel)=0
Set the linear velocity of the body.
bool initialized
This flag is set to true when the link is initialized.
Definition: Link.hh:686
void MoveFrame(const ignition::math::Pose3d &_worldReferenceFrameSrc, const ignition::math::Pose3d &_worldReferenceFrameDst, const bool _preserveWorldVelocity=false)
Move Link given source and target frames specified in world coordinates.
void RemoveParentJoint(const std::string &_jointName)
Remove Joints that have this Link as a child Link.
virtual ignition::math::Vector3d WorldLinearVel() const
Get the linear velocity of the origin of the link frame, expressed in the world frame.
Visuals_M visuals
Link visual elements.
Definition: Link.hh:680
virtual void SetSelfCollide(bool _collide)=0
Set whether this body will collide with others in the model.
Link_V GetChildJointsLinks() const
Returns a vector of children Links connected by joints.
void SetScale(const ignition::math::Vector3d &_scale)
Set the scale of the link.
std::shared_ptr< Battery > BatteryPtr
Definition: CommonTypes.hh:125
boost::shared_ptr< Connection > ConnectionPtr
Definition: CommonTypes.hh:134
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:109
boost::shared_ptr< Inertial > InertialPtr
Definition: PhysicsTypes.hh:157
boost::shared_ptr< Collision > CollisionPtr
Definition: PhysicsTypes.hh:113
std::vector< JointPtr > Joint_V
Definition: PhysicsTypes.hh:213
boost::shared_ptr< Entity > EntityPtr
Definition: PhysicsTypes.hh:85
std::vector< LinkPtr > Link_V
Definition: PhysicsTypes.hh:225
std::vector< CollisionPtr > Collision_V
Definition: PhysicsTypes.hh:229
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:93
boost::shared_ptr< Joint > JointPtr
Definition: PhysicsTypes.hh:117
Forward declarations for the common classes.
Definition: Animation.hh:27