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);
107 
109  public: virtual void Init();
110 
112  public: virtual void Fini();
113 
115  public: void Update();
116 
119  public: virtual void UpdateParameters(sdf::ElementPtr _sdf);
120 
122  public: virtual void Reset();
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::Quaterniond AxisFrame(
561  const unsigned int _index) const;
562 
575  public: ignition::math::Quaterniond AxisFrameOffset(
576  const unsigned int _index) const;
577 
582  public: double GetWorldEnergyPotentialSpring(unsigned int _index) const;
583 
591  protected: virtual double PositionImpl(const unsigned int _index = 0)
592  const = 0;
593 
604  protected: bool FindAllConnectedLinks(const LinkPtr &_originalParentLink,
605  Link_V &_connectedLinks);
606 
612  protected: ignition::math::Pose3d ChildLinkPose(
613  const unsigned int _index, const double _position);
614 
616  protected: virtual void RegisterIntrospectionItems();
617 
620  private: void RegisterIntrospectionPosition(const unsigned int _index);
621 
624  private: void RegisterIntrospectionVelocity(const unsigned int _index);
625 
628  private: void LoadImpl(const ignition::math::Pose3d &_pose);
629 
631  protected: LinkPtr childLink;
632 
634  protected: LinkPtr parentLink;
635 
637  protected: ModelPtr model;
638 
641  protected: ignition::math::Vector3d anchorPos;
642 
648  protected: ignition::math::Pose3d anchorPose;
649 
651  protected: ignition::math::Pose3d parentAnchorPose;
652 
654  protected: LinkPtr anchorLink;
655 
657  protected: double dissipationCoefficient[MAX_JOINT_AXIS];
658 
660  protected: double stiffnessCoefficient[MAX_JOINT_AXIS];
661 
663  protected: double springReferencePosition[MAX_JOINT_AXIS];
664 
666  protected: gazebo::event::ConnectionPtr applyDamping;
667 
669  protected: double effortLimit[MAX_JOINT_AXIS];
670 
672  protected: double velocityLimit[MAX_JOINT_AXIS];
673 
675  protected: double lowerLimit[MAX_JOINT_AXIS];
676 
678  protected: double upperLimit[MAX_JOINT_AXIS];
679 
682  protected: JointWrench wrench;
683 
687  protected: bool axisParentModelFrame[MAX_JOINT_AXIS];
688 
691  private: static sdf::ElementPtr sdfJoint;
692 
694  protected: bool provideFeedback;
695 
697  private: std::vector<std::string> sensors;
698 
700  private: event::EventT<void ()> jointUpdate;
701 
703  private: double staticPosition;
704 
706  private: double stopStiffness[MAX_JOINT_AXIS];
707 
709  private: double stopDissipation[MAX_JOINT_AXIS];
710  };
712  }
713 }
714 #endif
#define MAX_JOINT_AXIS
maximum number of axis per joint anticipated.
Definition: Joint.hh:39
sensors
Definition: SensorManager.hh:35
Base class for most physics classes.
Definition: Base.hh:78
virtual void Reset()
Reset the object.
keeps track of state of a physics::Joint
Definition: JointState.hh:43
Wrench information from a joint.
Definition: JointWrench.hh:41
Base class for all joints.
Definition: Joint.hh:51
virtual void SetStiffnessDamping(unsigned int _index, double _stiffness, double _damping, double _reference=0)=0
Set the joint spring stiffness.
Attribute
Joint attribute types.
Definition: Joint.hh:55
@ SUSPENSION_ERP
Suspension error reduction parameter.
Definition: Joint.hh:60
@ FMAX
Maximum force.
Definition: Joint.hh:78
@ SUSPENSION_CFM
Suspension constraint force mixing.
Definition: Joint.hh:63
@ FUDGE_FACTOR
Fudge factor.
Definition: Joint.hh:57
@ ERP
Error reduction parameter.
Definition: Joint.hh:72
@ HI_STOP
Upper joint limit.
Definition: Joint.hh:84
@ VEL
Velocity.
Definition: Joint.hh:81
@ STOP_CFM
Stop limit constraint force mixing.
Definition: Joint.hh:69
@ STOP_ERP
Stop limit error reduction parameter.
Definition: Joint.hh:66
@ CFM
Constraint force mixing.
Definition: Joint.hh:75
virtual double GetVelocity(unsigned int _index) const =0
Get the rotation rate of an axis(index)
virtual double Position(const unsigned int _index=0) const final
Get the position of an axis according to its index.
virtual void ApplyStiffnessDamping()
Callback to apply spring stiffness and viscous damping effects to joint.
virtual void Fini()
Finialize the object.
virtual void Init()
Initialize a joint.
virtual void SetStiffness(unsigned int _index, const double _stiffness)=0
Set the joint spring stiffness.
virtual void SetVelocityLimit(unsigned int _index, double _velocity)
Set the velocity limit on a joint axis.
virtual void Reset()
Reset the joint.
double CheckAndTruncateForce(unsigned int _index, double _effort)
check if the force against velocityLimit and effortLimit, truncate if necessary.
double GetDamping(unsigned int _index)
Returns the current joint damping coefficient.
double GetStiffness(unsigned int _index)
Returns the current joint spring stiffness coefficient.
virtual LinkPtr GetJointLink(unsigned int _index) const =0
Get the link to which the joint is attached according the _index.
event::ConnectionPtr ConnectJointUpdate(T _subscriber)
Connect a boost::slot the the joint update signal.
Definition: Joint.hh:213
virtual ignition::math::Vector3d Anchor(const unsigned int _index) const =0
Get the anchor point.
virtual JointWrench GetForceTorque(unsigned int _index)=0
get internal force and torque values at a joint.
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.
virtual void Attach(LinkPtr _parent, LinkPtr _child)
Attach the two bodies with this joint.
virtual ~Joint()
Destructor.
virtual void SetAnchor(const unsigned int _index, const ignition::math::Vector3d &_anchor)=0
Set the anchor point.
void SetModel(ModelPtr _model)
Set the model this joint belongs too.
virtual void Detach()
Detach this joint from all links.
void Load(LinkPtr _parent, LinkPtr _child, const ignition::math::Pose3d &_pose)
Set pose, parent and child links of a physics::Joint.
ignition::math::Vector3d LocalAxis(const unsigned int _index) const
Get the axis of rotation.
virtual double GetEffortLimit(unsigned int _index)
Get the effort limit on axis(index).
virtual void SetForce(unsigned int _index, double _effort)=0
Set the force applied to this physics::Joint.
virtual ignition::math::Vector3d GlobalAxis(unsigned int _index) const =0
Get the axis of rotation in global cooridnate frame.
virtual void SetDamping(unsigned int _index, double _damping)=0
Set the joint damping.
void SetState(const JointState &_state)
Set the joint state.
double GetSpringReferencePosition(unsigned int _index) const
Get joint spring reference position.
virtual void Load(sdf::ElementPtr _sdf)
Load physics::Joint from a SDF sdf::Element.
virtual void UpdateParameters(sdf::ElementPtr _sdf)
Update the parameters using new sdf values.
virtual void SetVelocity(unsigned int _index, double _vel)=0
Set the velocity of an axis(index).
Joint(BasePtr _parent)
Constructor.
virtual double GetVelocityLimit(unsigned int _index)
Get the velocity limit on axis(index).
virtual double GetForce(unsigned int _index)
void Update()
Update the joint.
virtual bool AreConnected(LinkPtr _one, LinkPtr _two) const =0
Determines of the two bodies are connected by a joint.
virtual void SetEffortLimit(unsigned int _index, double _effort)
Set the effort limit on a joint axis.
Definition: JointMaker.hh:45
boost::shared_ptr< Connection > ConnectionPtr
Definition: CommonTypes.hh:134
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:109
boost::shared_ptr< Base > BasePtr
Definition: PhysicsTypes.hh:77
std::vector< LinkPtr > Link_V
Definition: PhysicsTypes.hh:225
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:93
Forward declarations for the common classes.
Definition: Animation.hh:27
Definition: Model.hh:41