Go to the documentation of this file.
17 #ifndef GAZEBO_PHYSICS_JOINT_HH_
18 #define GAZEBO_PHYSICS_JOINT_HH_
23 #include <boost/any.hpp>
24 #include <ignition/math/Pose3.hh>
25 #include <ignition/math/Vector3.hh>
29 #include "gazebo/msgs/MessageTypes.hh"
39 #define MAX_JOINT_AXIS 2
95 public:
virtual ~
Joint();
102 const ignition::math::Pose3d &_pose);
106 public:
virtual void Load(sdf::ElementPtr _sdf);
109 public:
virtual void Init();
112 public:
virtual void Fini();
115 public:
void Update();
119 public:
virtual void UpdateParameters(sdf::ElementPtr _sdf);
122 public:
virtual void Reset();
127 public:
void SetState(
const JointState &_state);
131 public:
void SetModel(
ModelPtr _model);
138 public:
virtual LinkPtr GetJointLink(
unsigned int _index)
const = 0;
144 public:
virtual bool AreConnected(
LinkPtr _one,
LinkPtr _two)
const = 0;
152 public:
virtual void Detach();
159 public:
virtual void SetAxis(
const unsigned int _index,
160 const ignition::math::Vector3d &_axis) = 0;
166 public:
virtual void SetDamping(
unsigned int _index,
double _damping) = 0;
172 public:
double GetDamping(
unsigned int _index);
177 public:
virtual void ApplyStiffnessDamping();
185 public:
virtual void SetStiffnessDamping(
unsigned int _index,
186 double _stiffness,
double _damping,
double _reference = 0) = 0;
193 public:
virtual void SetStiffness(
unsigned int _index,
194 const double _stiffness) = 0;
201 public:
double GetStiffness(
unsigned int _index);
207 public:
double GetSpringReferencePosition(
unsigned int _index)
const;
212 public:
template<
typename T>
214 {
return jointUpdate.Connect(_subscriber);}
219 public: ignition::math::Vector3d LocalAxis(
const unsigned int _index)
225 public:
virtual ignition::math::Vector3d GlobalAxis(
226 unsigned int _index)
const = 0;
231 public:
virtual void SetAnchor(
const unsigned int _index,
232 const ignition::math::Vector3d &_anchor) = 0;
237 public:
virtual ignition::math::Vector3d Anchor(
238 const unsigned int _index)
const = 0;
243 public:
virtual double GetEffortLimit(
unsigned int _index);
248 public:
virtual void SetEffortLimit(
unsigned int _index,
double _effort);
253 public:
virtual double GetVelocityLimit(
unsigned int _index);
258 public:
virtual void SetVelocityLimit(
unsigned int _index,
268 public:
virtual void SetVelocity(
unsigned int _index,
double _vel) = 0;
273 public:
virtual double GetVelocity(
unsigned int _index)
const = 0;
283 public:
virtual void SetForce(
unsigned int _index,
double _effort) = 0;
290 public:
double CheckAndTruncateForce(
unsigned int _index,
double _effort);
298 public:
virtual double GetForce(
unsigned int _index);
322 public:
virtual JointWrench GetForceTorque(
unsigned int _index) = 0;
340 public:
virtual double Position(
const unsigned int _index = 0) const
345 public: virtual
unsigned int DOF() const = 0;
363 public: virtual
bool SetPosition(
364 const
unsigned int _index, const
double _position,
365 const
bool _preserveWorldVelocity = false);
377 protected:
bool SetPositionMaximal(
378 const
unsigned int _index,
double _position,
379 const
bool _preserveWorldVelocity = false);
388 protected:
bool SetVelocityMaximal(
unsigned int _index,
double _velocity);
396 public: virtual
ignition::math::Vector3d LinkForce(
397 const
unsigned int _index) const = 0;
405 public: virtual
ignition::math::Vector3d LinkTorque(
406 const
unsigned int _index) const = 0;
417 public: virtual
bool SetParam(const std::
string &_key,
419 const
boost::any &_value) = 0;
425 public: virtual
double GetParam(const std::
string &_key,
426 unsigned int _index);
430 public:
LinkPtr GetChild() const;
434 public:
LinkPtr GetParent() const;
438 public: msgs::
Joint::Type GetMsgType() const;
442 public: virtual
void FillMsg(msgs::
Joint &_msg);
451 public:
double GetInertiaRatio(const
unsigned int _index) const;
462 public:
double InertiaRatio(const
ignition::math::Vector3d &_axis) const;
468 public: virtual
double LowerLimit(
unsigned int _index = 0) const;
481 public: virtual
double UpperLimit(const
unsigned int _index = 0) const;
490 public: virtual
void SetLowerLimit(const
unsigned int _index,
491 const
double _limit);
500 public: virtual
void SetUpperLimit(const
unsigned int _index,
501 const
double _limit);
505 public: virtual
void SetProvideFeedback(
bool _enable);
508 public: virtual
void CacheForceTorque();
513 public:
void SetStopStiffness(
unsigned int _index,
double _stiffness);
518 public:
void SetStopDissipation(
unsigned int _index,
double _dissipation);
523 public:
double GetStopStiffness(
unsigned int _index) const;
528 public:
double GetStopDissipation(
unsigned int _index) const;
533 public:
ignition::math::Pose3d InitialAnchorPose() const;
539 public:
ignition::math::Pose3d WorldPose() const;
546 public:
ignition::math::Pose3d ParentWorldPose() const;
553 public:
ignition::math::Pose3d AnchorErrorPose() const;
560 public:
ignition::math::Quaterniond AxisFrame(
561 const
unsigned int _index) const;
575 public:
ignition::math::Quaterniond AxisFrameOffset(
576 const
unsigned int _index) const;
582 public:
double GetWorldEnergyPotentialSpring(
unsigned int _index) const;
591 protected: virtual
double PositionImpl(const
unsigned int _index = 0)
604 protected:
bool FindAllConnectedLinks(const
LinkPtr &_originalParentLink,
612 protected:
ignition::math::Pose3d ChildLinkPose(
613 const
unsigned int _index, const
double _position);
616 protected: virtual
void RegisterIntrospectionItems();
620 private:
void RegisterIntrospectionPosition(const
unsigned int _index);
624 private:
void RegisterIntrospectionVelocity(const
unsigned int _index);
628 private:
void LoadImpl(const
ignition::math::Pose3d &_pose);
651 protected:
ignition::math::Pose3d parentAnchorPose;
691 private: static sdf::ElementPtr sdfJoint;
694 protected:
bool provideFeedback;
697 private: std::vector<std::
string>
sensors;
700 private: event::EventT<
void ()> jointUpdate;
703 private:
double staticPosition;
virtual bool AreConnected(LinkPtr _one, LinkPtr _two) const =0
Determines of the two bodies are connected by a joint.
double velocityLimit[2]
Store Joint velocity limit as specified in SDF.
Definition: Joint.hh:672
virtual void SetAnchor(const unsigned int _index, const ignition::math::Vector3d &_anchor)=0
Set the anchor point.
virtual bool SetParam(const std::string &_key, unsigned int _index, const boost::any &_value)=0
Set a non-generic parameter for the joint.
#define MAX_JOINT_AXIS
maximum number of axis per joint anticipated.
Definition: Joint.hh:39
double springReferencePosition[2]
joint spring reference (zero load) position
Definition: Joint.hh:663
virtual unsigned int DOF() const =0
Get the number of degrees of freedom for this joint.
virtual ignition::math::Vector3d GlobalAxis(unsigned int _index) const =0
Get the axis of rotation in global cooridnate frame.
virtual LinkPtr GetJointLink(unsigned int _index) const =0
Get the link to which the joint is attached according the _index.
msgs::Joint::Type GetMsgType() const
Get the joint type as msgs::Joint::Type.
@ HI_STOP
Upper joint limit.
Definition: Joint.hh:84
virtual bool SetPosition(const unsigned int _index, const double _position, const bool _preserveWorldVelocity=false)
The child links of this joint are updated based on desired position.
double GetStopDissipation(unsigned int _index) const
Get joint stop dissipation.
double GetSpringReferencePosition(unsigned int _index) const
Get joint spring reference position.
virtual void SetUpperLimit(const unsigned int _index, const double _limit)
Set the joint's upper limit.
virtual void CacheForceTorque()
Cache Joint Force Torque Values if necessary for physics engine.
virtual void FillMsg(msgs::Joint &_msg)
Fill a joint message.
Forward declarations for the common classes.
Definition: Animation.hh:26
virtual void SetDamping(unsigned int _index, double _damping)=0
Set the joint damping.
virtual void SetAxis(const unsigned int _index, const ignition::math::Vector3d &_axis)=0
Set the axis of rotation where axis is specified in local joint frame.
keeps track of state of a physics::Joint
Definition: JointState.hh:42
@ STOP_CFM
Stop limit constraint force mixing.
Definition: Joint.hh:69
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:93
ignition::math::Quaterniond AxisFrame(const unsigned int _index) const
Get orientation of reference frame for specified axis, relative to world frame.
LinkPtr GetParent() const
Get the parent link.
double InertiaRatio(const ignition::math::Vector3d &_axis) const
Computes moment of inertia (MOI) across an arbitrary axis specified in the world frame.
bool SetVelocityMaximal(unsigned int _index, double _velocity)
Helper function for maximal coordinate solver SetVelocity.
sensors
Definition: SensorManager.hh:35
bool SetPositionMaximal(const unsigned int _index, double _position, const bool _preserveWorldVelocity=false)
Helper function for maximal coordinate solver SetPosition.
bool FindAllConnectedLinks(const LinkPtr &_originalParentLink, Link_V &_connectedLinks)
internal helper to find all links connected to the child link branching out from the children of the ...
double lowerLimit[2]
Store Joint position lower limit as specified in SDF.
Definition: Joint.hh:675
bool provideFeedback
Provide Feedback data for contact forces.
Definition: Joint.hh:694
double CheckAndTruncateForce(unsigned int _index, double _effort)
check if the force against velocityLimit and effortLimit, truncate if necessary.
void Update()
Update the joint.
ignition::math::Quaterniond AxisFrameOffset(const unsigned int _index) const
Get orientation of joint axis reference frame relative to joint frame.
ignition::math::Vector3d LocalAxis(const unsigned int _index) const
Get the axis of rotation.
virtual void Detach()
Detach this joint from all links.
virtual double UpperLimit(const unsigned int _index=0) const
Get the joint's upper limit.
Definition: JointMaker.hh:44
virtual void SetStiffnessDamping(unsigned int _index, double _stiffness, double _damping, double _reference=0)=0
Set the joint spring stiffness.
virtual double Position(const unsigned int _index=0) const final
Get the position of an axis according to its index.
virtual void SetLowerLimit(const unsigned int _index, const double _limit)
Set the joint's lower limit.
virtual double PositionImpl(const unsigned int _index=0) const =0
Helper function to get the position of an axis.
void SetState(const JointState &_state)
Set the joint state.
virtual void SetVelocity(unsigned int _index, double _vel)=0
Set the velocity of an axis(index).
virtual void ApplyStiffnessDamping()
Callback to apply spring stiffness and viscous damping effects to joint.
double effortLimit[2]
Store Joint effort limit as specified in SDF.
Definition: Joint.hh:669
LinkPtr childLink
The first link this joint connects to.
Definition: Joint.hh:631
ignition::math::Pose3d ChildLinkPose(const unsigned int _index, const double _position)
internal function to help us compute child link pose if a joint position change is applied.
virtual void Reset()
Reset the object.
double GetDamping(unsigned int _index)
Returns the current joint damping coefficient.
@ SUSPENSION_ERP
Suspension error reduction parameter.
Definition: Joint.hh:60
virtual double GetVelocity(unsigned int _index) const =0
Get the rotation rate of an axis(index)
@ SUSPENSION_CFM
Suspension constraint force mixing.
Definition: Joint.hh:63
virtual double GetForce(unsigned int _index)
virtual void Reset()
Reset the joint.
JointWrench wrench
Cache Joint force torque values in case physics engine clears them at the end of update step.
Definition: Joint.hh:682
ignition::math::Pose3d parentAnchorPose
Anchor pose relative to parent link frame.
Definition: Joint.hh:651
@ LO_STOP
Lower joint limit.
Definition: Joint.hh:87
virtual double GetEffortLimit(unsigned int _index)
Get the effort limit on axis(index).
double GetStiffness(unsigned int _index)
Returns the current joint spring stiffness coefficient.
Base class for all joints.
Definition: Joint.hh:50
Base class for most physics classes.
Definition: Base.hh:77
LinkPtr anchorLink
Anchor link.
Definition: Joint.hh:654
@ FUDGE_FACTOR
Fudge factor.
Definition: Joint.hh:57
ignition::math::Pose3d WorldPose() const
Get pose of joint frame relative to world frame.
void SetStopDissipation(unsigned int _index, double _dissipation)
Set joint stop dissipation.
void SetModel(ModelPtr _model)
Set the model this joint belongs too.
LinkPtr GetChild() const
Get the child link.
double upperLimit[2]
Store Joint position upper limit as specified in SDF.
Definition: Joint.hh:678
void Load(LinkPtr _parent, LinkPtr _child, const ignition::math::Pose3d &_pose)
Set pose, parent and child links of a physics::Joint.
@ FMAX
Maximum force.
Definition: Joint.hh:78
ConnectionPtr Connect(const std::function< T > &_subscriber)
Connect a callback to this event.
Definition: Event.hh:558
double GetStopStiffness(unsigned int _index) const
Get joint stop stiffness.
virtual void SetProvideFeedback(bool _enable)
Set whether the joint should generate feedback.
ignition::math::Pose3d AnchorErrorPose() const
Get pose offset between anchor pose on child and parent, expressed in the parent link frame.
virtual void Init()
Initialize a joint.
virtual void UpdateParameters(sdf::ElementPtr _sdf)
Update the parameters using new sdf values.
ignition::math::Vector3d anchorPos
Anchor pose.
Definition: Joint.hh:641
double GetWorldEnergyPotentialSpring(unsigned int _index) const
Returns this joint's spring potential energy, based on the reference position of the spring.
@ CFM
Constraint force mixing.
Definition: Joint.hh:75
boost::shared_ptr< Base > BasePtr
Definition: PhysicsTypes.hh:77
virtual ~Joint()
Destructor.
double GetInertiaRatio(const unsigned int _index) const
Computes moment of inertia (MOI) across a specified joint axis.
boost::shared_ptr< Connection > ConnectionPtr
Definition: CommonTypes.hh:134
sdf::ElementPtr sdf
The SDF values for this object.
Definition: Base.hh:337
ignition::math::Pose3d anchorPose
Anchor pose specified in SDF <joint><pose> tag.
Definition: Joint.hh:648
GAZEBO_VISIBLE void Init(google::protobuf::Message &_message, const std::string &_id="")
Initialize a message.
void SetStopStiffness(unsigned int _index, double _stiffness)
Set joint stop stiffness.
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:109
Attribute
Joint attribute types.
Definition: Joint.hh:54
virtual double GetVelocityLimit(unsigned int _index)
Get the velocity limit on axis(index).
LinkPtr parentLink
The second link this joint connects to.
Definition: Joint.hh:634
virtual JointWrench GetForceTorque(unsigned int _index)=0
get internal force and torque values at a joint.
@ ERP
Error reduction parameter.
Definition: Joint.hh:72
virtual void Attach(LinkPtr _parent, LinkPtr _child)
Attach the two bodies with this joint.
virtual double GetParam(const std::string &_key, unsigned int _index)
Get a non-generic parameter for the joint.
Joint(BasePtr _parent)
Constructor.
event::ConnectionPtr ConnectJointUpdate(T _subscriber)
Connect a boost::slot the the joint update signal.
Definition: Joint.hh:213
double stiffnessCoefficient[2]
joint stiffnessCoefficient
Definition: Joint.hh:660
virtual void RegisterIntrospectionItems()
Register items in the introspection service.
std::vector< LinkPtr > Link_V
Definition: PhysicsTypes.hh:225
double dissipationCoefficient[2]
joint viscous damping coefficient
Definition: Joint.hh:657
virtual double LowerLimit(unsigned int _index=0) const
Get the joint's lower limit.
gazebo::event::ConnectionPtr applyDamping
apply damping for adding viscous damping forces on updates
Definition: Joint.hh:666
virtual void Fini()
Finialize the object.
ModelPtr model
Pointer to the parent model.
Definition: Joint.hh:637
virtual void SetForce(unsigned int _index, double _effort)=0
Set the force applied to this physics::Joint.
virtual void SetEffortLimit(unsigned int _index, double _effort)
Set the effort limit on a joint axis.
Wrench information from a joint. These are forces and torques on parent and child Links,...
Definition: JointWrench.hh:40
virtual ignition::math::Vector3d LinkForce(const unsigned int _index) const =0
Get the forces applied to the center of mass of a physics::Link due to the existence of this Joint.
virtual ignition::math::Vector3d LinkTorque(const unsigned int _index) const =0
Get the torque applied to the center of mass of a physics::Link due to the existence of this Joint.
ignition::math::Pose3d ParentWorldPose() const
Get anchor pose on parent link relative to world frame.
bool axisParentModelFrame[2]
Flags that are set to true if an axis value is expressed in the parent model frame.
Definition: Joint.hh:687
ignition::math::Pose3d InitialAnchorPose() const
Get initial Anchor Pose specified by model <joint><pose>...</pose></joint>
virtual void SetStiffness(unsigned int _index, const double _stiffness)=0
Set the joint spring stiffness.
@ STOP_ERP
Stop limit error reduction parameter.
Definition: Joint.hh:66
@ VEL
Velocity.
Definition: Joint.hh:81
virtual ignition::math::Vector3d Anchor(const unsigned int _index) const =0
Get the anchor point.
virtual void SetVelocityLimit(unsigned int _index, double _velocity)
Set the velocity limit on a joint axis.