17 #ifndef GAZEBO_PHYSICS_LINK_HH_
18 #define GAZEBO_PHYSICS_LINK_HH_
23 #include <ignition/math/Matrix3.hh>
63 public:
virtual void Load(sdf::ElementPtr _sdf)
override;
66 public:
virtual void Init()
override;
69 public:
void Fini()
override;
89 public:
void SetScale(
const ignition::math::Vector3d &_scale);
152 const ignition::math::Vector3d &_vel) = 0;
157 const ignition::math::Vector3d &_vel) = 0;
162 const ignition::math::Vector3d &_force) = 0;
167 const ignition::math::Vector3d &_torque) = 0;
171 public:
virtual void AddForce(
const ignition::math::Vector3d &_force) = 0;
177 const ignition::math::Vector3d &_force) = 0;
183 const ignition::math::Vector3d &_force,
184 const ignition::math::Vector3d &_pos) = 0;
195 const ignition::math::Vector3d &_force,
196 const ignition::math::Vector3d &_relPos) = 0;
205 const ignition::math::Vector3d &_force,
206 const ignition::math::Vector3d &_offset =
207 ignition::math::Vector3d::Zero) = 0;
212 const ignition::math::Vector3d &_torque) = 0;
218 const ignition::math::Vector3d &_torque) = 0;
239 const ignition::math::Vector3d &_offset)
const = 0;
249 const ignition::math::Vector3d &_offset,
250 const ignition::math::Quaterniond &_q)
const = 0;
300 public:
virtual ignition::math::Vector3d
WorldForce()
const = 0;
340 public:
CollisionPtr GetCollisionById(
unsigned int _id)
const;
360 public:
virtual ignition::math::AxisAlignedBox
BoundingBox()
const
414 std::function<
void (
bool)> _subscriber);
448 const ignition::math::Pose3d &_offset);
538 const ignition::math::Pose3d &_worldReferenceFrameSrc,
539 const ignition::math::Pose3d &_worldReferenceFrameDst,
540 const bool _preserveWorldVelocity =
false);
557 const LinkPtr &_originalParentLink,
558 Link_V &_connectedLinks,
bool _fistLink =
false);
595 public:
bool VisualId(
const std::string &_visName, uint32_t &_visualId)
603 ignition::math::Pose3d &_pose)
const;
610 const ignition::math::Pose3d &_pose);
628 private:
void PublishData();
632 private:
void LoadCollision(sdf::ElementPtr _sdf);
636 private:
void LoadLight(sdf::ElementPtr _sdf);
640 private:
void SetInertialFromCollisions();
644 private:
void OnCollision(ConstContactsPtr &_msg);
647 private:
void ParseVisuals();
653 private:
bool ContainsLink(
const Link_V &_vector,
const LinkPtr &_value);
657 private:
void UpdateVisualGeomSDF(
const ignition::math::Vector3d &_scale);
660 private:
void UpdateVisualMsg();
665 private:
void OnWrenchMsg(ConstWrenchPtr &_msg);
669 private:
void ProcessWrenchMsg(
const msgs::Wrench &_msg);
673 private:
void LoadBattery(
const sdf::ElementPtr _sdf);
688 protected:
bool initialized =
false;
691 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:166
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:53
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:679
msgs::Visual GetVisualMessage(const std::string &_name) const
Returns the visual message specified by its name.
const sdf::Link * GetSDFDom() const
Get the SDF DOM object of this link.
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.
const ignition::math::Vector3d RelativeWindLinearVel() const
Returns this link's wind velocity.
ignition::math::Vector3d RelativeAngularAccel() const override
Get the angular acceleration of the body.
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:685
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.
virtual void SetWindMode(const bool _mode)
Set whether wind affects this body.
std::map< uint32_t, msgs::Visual > Visuals_M
Definition: Link.hh:621
virtual void UpdateMass()
Update the mass matrix.
Definition: Link.hh:466
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.
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.
virtual void OnPoseChange() override
This function is called when the entity's (or one of its parents) pose of the parent has changed.
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.
void Fini() override
Finalize the body.
void ResetPhysicsStates()
Reset the velocity, acceleration, force and torque of link.
void SetWindEnabled(const bool _enable)
Enable/disable wind for this link.
ignition::math::Vector3d WorldAngularAccel() const override
Get the angular acceleration of the body in the world frame, which is computed as (I^-1 * (T - w x L)...
ignition::math::Vector3d WorldLinearAccel() const override
Get the linear acceleration of the body in the world frame.
virtual bool GetKinematic() const
\TODO Implement this function.
Definition: Link.hh:387
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:469
virtual ignition::math::AxisAlignedBox BoundingBox() const override
Get the bounding box for the link and all the child elements.
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.
virtual bool SetSelected(bool _set) override
Set whether this entity has been selected by the user through the gui.
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:317
virtual void SetLinkStatic(bool _static)=0
Freeze link to ground (inertial frame).
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.
virtual ignition::math::Vector3d WorldLinearVel() const override
Get the linear velocity of the origin of the link frame, expressed in the world frame.
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.
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.
const Visuals_M & Visuals() const
Return the link visual elements.
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...
virtual void RegisterIntrospectionItems() override
Register items in the introspection service.
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.
std::optional< sdf::SemanticPose > SDFSemanticPose() const override
Get the SDF SemanticPose object associated with the pose of this object.
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 UpdateParameters(sdf::ElementPtr _sdf) override
Update the parameters using new sdf values.
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.
ignition::math::Vector3d RelativeAngularVel() const override
Get the angular velocity of the body.
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.
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 Init() override
Initialize the body.
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.
ignition::math::Vector3d RelativeLinearAccel() const override
Get the linear acceleration of the body.
virtual void Load(sdf::ElementPtr _sdf) override
Load the body based on an SDF element.
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.
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.
Visuals_M visuals
Link visual elements.
Definition: Link.hh:682
void Reset() override
Reset the link.
virtual void SetSelfCollide(bool _collide)=0
Set whether this body will collide with others in the model.
ignition::math::Vector3d RelativeLinearVel() const override
Get the linear velocity of the body.
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:110
boost::shared_ptr< Inertial > InertialPtr
Definition: PhysicsTypes.hh:158
boost::shared_ptr< Collision > CollisionPtr
Definition: PhysicsTypes.hh:114
std::vector< JointPtr > Joint_V
Definition: PhysicsTypes.hh:214
boost::shared_ptr< Entity > EntityPtr
Definition: PhysicsTypes.hh:86
std::vector< LinkPtr > Link_V
Definition: PhysicsTypes.hh:226
std::vector< CollisionPtr > Collision_V
Definition: PhysicsTypes.hh:230
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:94
boost::shared_ptr< Joint > JointPtr
Definition: PhysicsTypes.hh:118
Forward declarations for the common classes.
Definition: Animation.hh:27