Joint.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef GAZEBO_PHYSICS_JOINT_HH_
18 #define GAZEBO_PHYSICS_JOINT_HH_
19 
20 #include <string>
21 #include <vector>
22 
23 #include <boost/any.hpp>
24 #include <ignition/math/Pose3.hh>
25 #include <ignition/math/Vector3.hh>
26 
27 #include "gazebo/common/Event.hh"
28 #include "gazebo/common/Events.hh"
29 #include "gazebo/msgs/MessageTypes.hh"
30 
32 #include "gazebo/physics/Base.hh"
34 #include "gazebo/util/system.hh"
35 
39 #define MAX_JOINT_AXIS 2
40 
41 namespace gazebo
42 {
43  namespace physics
44  {
47 
50  class GZ_PHYSICS_VISIBLE Joint : public Base
51  {
54  public: enum Attribute
55  {
58 
61 
64 
67 
70 
72  ERP,
73 
75  CFM,
76 
79 
81  VEL,
82 
85 
87  LO_STOP
88  };
89 
92  public: explicit Joint(BasePtr _parent);
93 
95  public: virtual ~Joint();
96 
101  public: void Load(LinkPtr _parent, LinkPtr _child,
102  const ignition::math::Pose3d &_pose);
103 
106  public: virtual void Load(sdf::ElementPtr _sdf) override;
107 
109  public: virtual void Init() override;
110 
112  public: virtual void Fini() override;
113 
115  public: void Update() override;
116 
119  public: virtual void UpdateParameters(sdf::ElementPtr _sdf) override;
120 
122  public: virtual void Reset() override;
123  using Base::Reset;
124 
127  public: void SetState(const JointState &_state);
128 
131  public: void SetModel(ModelPtr _model);
132 
138  public: virtual LinkPtr GetJointLink(unsigned int _index) const = 0;
139 
144  public: virtual bool AreConnected(LinkPtr _one, LinkPtr _two) const = 0;
145 
149  public: virtual void Attach(LinkPtr _parent, LinkPtr _child);
150 
152  public: virtual void Detach();
153 
159  public: virtual void SetAxis(const unsigned int _index,
160  const ignition::math::Vector3d &_axis) = 0;
161 
166  public: virtual void SetDamping(unsigned int _index, double _damping) = 0;
167 
172  public: double GetDamping(unsigned int _index);
173 
177  public: virtual void ApplyStiffnessDamping();
178 
185  public: virtual void SetStiffnessDamping(unsigned int _index,
186  double _stiffness, double _damping, double _reference = 0) = 0;
187 
193  public: virtual void SetStiffness(unsigned int _index,
194  const double _stiffness) = 0;
195 
201  public: double GetStiffness(unsigned int _index);
202 
207  public: double GetSpringReferencePosition(unsigned int _index) const;
208 
212  public: template<typename T>
214  {return jointUpdate.Connect(_subscriber);}
215 
219  public: ignition::math::Vector3d LocalAxis(const unsigned int _index)
220  const;
221 
225  public: virtual ignition::math::Vector3d GlobalAxis(
226  unsigned int _index) const = 0;
227 
231  public: virtual void SetAnchor(const unsigned int _index,
232  const ignition::math::Vector3d &_anchor) = 0;
233 
237  public: virtual ignition::math::Vector3d Anchor(
238  const unsigned int _index) const = 0;
239 
243  public: virtual double GetEffortLimit(unsigned int _index);
244 
248  public: virtual void SetEffortLimit(unsigned int _index, double _effort);
249 
253  public: virtual double GetVelocityLimit(unsigned int _index);
254 
258  public: virtual void SetVelocityLimit(unsigned int _index,
259  double _velocity);
260 
268  public: virtual void SetVelocity(unsigned int _index, double _vel) = 0;
269 
273  public: virtual double GetVelocity(unsigned int _index) const = 0;
274 
283  public: virtual void SetForce(unsigned int _index, double _effort) = 0;
284 
290  public: double CheckAndTruncateForce(unsigned int _index, double _effort);
291 
298  public: virtual double GetForce(unsigned int _index);
299 
322  public: virtual JointWrench GetForceTorque(unsigned int _index) = 0;
323 
340  public: virtual double Position(const unsigned int _index = 0) const
341  final;
342 
345  public: virtual unsigned int DOF() const = 0;
346 
363  public: virtual bool SetPosition(
364  const unsigned int _index, const double _position,
365  const bool _preserveWorldVelocity = false);
366 
377  protected: bool SetPositionMaximal(
378  const unsigned int _index, double _position,
379  const bool _preserveWorldVelocity = false);
380 
388  protected: bool SetVelocityMaximal(unsigned int _index, double _velocity);
389 
396  public: virtual ignition::math::Vector3d LinkForce(
397  const unsigned int _index) const = 0;
398 
405  public: virtual ignition::math::Vector3d LinkTorque(
406  const unsigned int _index) const = 0;
407 
417  public: virtual bool SetParam(const std::string &_key,
418  unsigned int _index,
419  const boost::any &_value) = 0;
420 
425  public: virtual double GetParam(const std::string &_key,
426  unsigned int _index);
427 
430  public: LinkPtr GetChild() const;
431 
434  public: LinkPtr GetParent() const;
435 
438  public: msgs::Joint::Type GetMsgType() const;
439 
442  public: virtual void FillMsg(msgs::Joint &_msg);
443 
451  public: double GetInertiaRatio(const unsigned int _index) const;
452 
462  public: double InertiaRatio(const ignition::math::Vector3d &_axis) const;
463 
468  public: virtual double LowerLimit(unsigned int _index = 0) const;
469 
481  public: virtual double UpperLimit(const unsigned int _index = 0) const;
482 
490  public: virtual void SetLowerLimit(const unsigned int _index,
491  const double _limit);
492 
500  public: virtual void SetUpperLimit(const unsigned int _index,
501  const double _limit);
502 
505  public: virtual void SetProvideFeedback(bool _enable);
506 
508  public: virtual void CacheForceTorque();
509 
513  public: void SetStopStiffness(unsigned int _index, double _stiffness);
514 
518  public: void SetStopDissipation(unsigned int _index, double _dissipation);
519 
523  public: double GetStopStiffness(unsigned int _index) const;
524 
528  public: double GetStopDissipation(unsigned int _index) const;
529 
533  public: ignition::math::Pose3d InitialAnchorPose() const;
534 
539  public: ignition::math::Pose3d WorldPose() const;
540 
546  public: ignition::math::Pose3d ParentWorldPose() const;
547 
553  public: ignition::math::Pose3d AnchorErrorPose() const;
554 
560  public: ignition::math::Vector3d ResolveAxisXyz(
561  const unsigned int _index,
562  const std::string &_resolveTo = "") const;
563 
569  public: ignition::math::Quaterniond AxisFrame(
570  const unsigned int _index) const;
571 
584  public: ignition::math::Quaterniond AxisFrameOffset(
585  const unsigned int _index) const;
586 
591  public: double GetWorldEnergyPotentialSpring(unsigned int _index) const;
592 
593  // Documentation inherited
594  public: virtual std::optional<sdf::SemanticPose> SDFSemanticPose()
595  const override;
596 
599  public: const sdf::Joint *GetSDFDom() const;
600 
608  protected: virtual double PositionImpl(const unsigned int _index = 0)
609  const = 0;
610 
621  protected: bool FindAllConnectedLinks(const LinkPtr &_originalParentLink,
622  Link_V &_connectedLinks);
623 
629  protected: ignition::math::Pose3d ChildLinkPose(
630  const unsigned int _index, const double _position);
631 
633  protected: virtual void RegisterIntrospectionItems() override;
634 
637  private: void RegisterIntrospectionPosition(const unsigned int _index);
638 
641  private: void RegisterIntrospectionVelocity(const unsigned int _index);
642 
645  private: void LoadImpl(const ignition::math::Pose3d &_pose);
646 
648  protected: LinkPtr childLink;
649 
651  protected: LinkPtr parentLink;
652 
654  protected: ModelPtr model;
655 
658  protected: ignition::math::Vector3d anchorPos;
659 
665  protected: ignition::math::Pose3d anchorPose;
666 
668  protected: ignition::math::Pose3d parentAnchorPose;
669 
671  protected: LinkPtr anchorLink;
672 
674  protected: double dissipationCoefficient[MAX_JOINT_AXIS];
675 
677  protected: double stiffnessCoefficient[MAX_JOINT_AXIS];
678 
680  protected: double springReferencePosition[MAX_JOINT_AXIS];
681 
683  protected: gazebo::event::ConnectionPtr applyDamping;
684 
686  protected: double effortLimit[MAX_JOINT_AXIS];
687 
689  protected: double velocityLimit[MAX_JOINT_AXIS];
690 
692  protected: double lowerLimit[MAX_JOINT_AXIS];
693 
695  protected: double upperLimit[MAX_JOINT_AXIS];
696 
699  protected: JointWrench wrench;
700 
703  protected: std::string axisExpressedIn[MAX_JOINT_AXIS];
704 
707  private: static sdf::ElementPtr sdfJoint;
708 
710  protected: bool provideFeedback;
711 
713  private: std::vector<std::string> sensors;
714 
716  private: event::EventT<void ()> jointUpdate;
717 
719  private: double staticPosition;
720 
722  private: double stopStiffness[MAX_JOINT_AXIS];
723 
725  private: double stopDissipation[MAX_JOINT_AXIS];
726 
728  private: const sdf::Joint *jointSDFDom = nullptr;
729  };
731  }
732 }
733 #endif
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:689
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.
virtual void Reset() override
Reset 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:680
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.
virtual std::optional< sdf::SemanticPose > SDFSemanticPose() const override
Get the SDF SemanticPose object associated with the pose of this object.
Definition: Model.hh:40
@ 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.
const sdf::Joint * GetSDFDom() const
Get the SDF DOM object of this joint.
Forward declarations for the common classes.
Definition: Animation.hh:26
virtual void Init() override
Initialize a joint.
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:36
@ STOP_CFM
Stop limit constraint force mixing.
Definition: Joint.hh:69
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:94
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:36
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:692
bool provideFeedback
Provide Feedback data for contact forces.
Definition: Joint.hh:710
double CheckAndTruncateForce(unsigned int _index, double _effort)
check if the force against velocityLimit and effortLimit, truncate if necessary.
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:686
virtual void Fini() override
Finialize the object.
LinkPtr childLink
The first link this joint connects to.
Definition: Joint.hh:648
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.
virtual void RegisterIntrospectionItems() override
Register items in the introspection service.
@ 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)
JointWrench wrench
Cache Joint force torque values in case physics engine clears them at the end of update step.
Definition: Joint.hh:699
ignition::math::Pose3d parentAnchorPose
Anchor pose relative to parent link frame.
Definition: Joint.hh:668
virtual void UpdateParameters(sdf::ElementPtr _sdf) override
Update the parameters using new sdf values.
@ 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:72
LinkPtr anchorLink
Anchor link.
Definition: Joint.hh:671
@ 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:695
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.
ignition::math::Vector3d anchorPos
Anchor pose.
Definition: Joint.hh:658
void Update() override
Update the joint.
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:78
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:345
ignition::math::Pose3d anchorPose
Anchor pose specified in SDF <joint><pose> tag.
Definition: Joint.hh:665
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:110
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:651
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:677
std::vector< LinkPtr > Link_V
Definition: PhysicsTypes.hh:226
double dissipationCoefficient[2]
joint viscous damping coefficient
Definition: Joint.hh:674
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:683
ModelPtr model
Pointer to the parent model.
Definition: Joint.hh:654
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.
std::string axisExpressedIn[2]
Expressed-in values for each axis that are checked if the DOM has errors.
Definition: Joint.hh:703
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.
ignition::math::Vector3d ResolveAxisXyz(const unsigned int _index, const std::string &_resolveTo="") const
Resolve axis xyz value to the named frame, defaulting to the joint frame if no frame name is supplied...
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.