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
29#include "gazebo/msgs/MessageTypes.hh"
30
34#include "gazebo/util/system.hh"
35
39#define MAX_JOINT_AXIS 2
40
41namespace 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
73
76
79
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:72
virtual void Reset()
Reset the object.
sdf::ElementPtr sdf
The SDF values for this object.
Definition Base.hh:331
keeps track of state of a physics::Joint
Definition JointState.hh:37
Wrench information from a joint.
Definition JointWrench.hh:41
Base class for all joints.
Definition Joint.hh:51
ignition::math::Pose3d ParentWorldPose() const
Get anchor pose on parent link relative to world frame.
void SetStopDissipation(unsigned int _index, double _dissipation)
Set joint stop dissipation.
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
@ LO_STOP
Lower joint limit.
Definition Joint.hh:87
@ CFM
Constraint force mixing.
Definition Joint.hh:75
virtual double GetVelocity(unsigned int _index) const =0
Get the rotation rate of an axis(index)
double upperLimit[2]
Store Joint position upper limit as specified in SDF.
Definition Joint.hh:678
double InertiaRatio(const ignition::math::Vector3d &_axis) const
Computes moment of inertia (MOI) across an arbitrary axis specified in the world frame.
LinkPtr parentLink
The second link this joint connects to.
Definition Joint.hh:634
virtual double Position(const unsigned int _index=0) const final
Get the position of an axis according to its index.
gazebo::event::ConnectionPtr applyDamping
apply damping for adding viscous damping forces on updates
Definition Joint.hh:666
virtual void ApplyStiffnessDamping()
Callback to apply spring stiffness and viscous damping effects to joint.
double lowerLimit[2]
Store Joint position lower limit as specified in SDF.
Definition Joint.hh:675
double GetStopDissipation(unsigned int _index) const
Get joint stop dissipation.
double GetStopStiffness(unsigned int _index) const
Get joint stop stiffness.
msgs::Joint::Type GetMsgType() const
Get the joint type as msgs::Joint::Type.
virtual void Fini()
Finialize the object.
virtual void SetUpperLimit(const unsigned int _index, const double _limit)
Set the joint's upper limit.
virtual void Init()
Initialize a joint.
double springReferencePosition[2]
joint spring reference (zero load) position
Definition Joint.hh:663
virtual void SetStiffness(unsigned int _index, const double _stiffness)=0
Set the joint spring stiffness.
virtual void CacheForceTorque()
Cache Joint Force Torque Values if necessary for physics engine.
bool provideFeedback
Provide Feedback data for contact forces.
Definition Joint.hh:694
double effortLimit[2]
Store Joint effort limit as specified in SDF.
Definition Joint.hh:669
virtual void SetProvideFeedback(bool _enable)
Set whether the joint should generate feedback.
virtual double UpperLimit(const unsigned int _index=0) const
Get the joint's upper limit.
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 SetVelocityLimit(unsigned int _index, double _velocity)
Set the velocity limit on a joint axis.
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 ...
virtual void Reset()
Reset the joint.
virtual unsigned int DOF() const =0
Get the number of degrees of freedom for this joint.
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.
ModelPtr model
Pointer to the parent model.
Definition Joint.hh:637
double GetDamping(unsigned int _index)
Returns the current joint damping coefficient.
bool SetVelocityMaximal(unsigned int _index, double _velocity)
Helper function for maximal coordinate solver SetVelocity.
bool axisParentModelFrame[2]
Flags that are set to true if an axis value is expressed in the parent model frame.
Definition Joint.hh:687
LinkPtr GetChild() const
Get the child link.
double dissipationCoefficient[2]
joint viscous damping coefficient
Definition Joint.hh:657
LinkPtr childLink
The first link this joint connects to.
Definition Joint.hh:631
ignition::math::Pose3d parentAnchorPose
Anchor pose relative to parent link frame.
Definition Joint.hh:651
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 void RegisterIntrospectionItems()
Register items in the introspection service.
virtual void SetLowerLimit(const unsigned int _index, const double _limit)
Set the joint's lower limit.
bool SetPositionMaximal(const unsigned int _index, double _position, const bool _preserveWorldVelocity=false)
Helper function for maximal coordinate solver SetPosition.
double stiffnessCoefficient[2]
joint stiffnessCoefficient
Definition Joint.hh:660
virtual JointWrench GetForceTorque(unsigned int _index)=0
get internal force and torque values at a joint.
LinkPtr GetParent() const
Get the parent link.
virtual double LowerLimit(unsigned int _index=0) const
Get the joint's lower limit.
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 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.
double GetWorldEnergyPotentialSpring(unsigned int _index) const
Returns this joint's spring potential energy, based on the reference position of the spring.
virtual ~Joint()
Destructor.
void SetStopStiffness(unsigned int _index, double _stiffness)
Set joint stop stiffness.
virtual void SetAnchor(const unsigned int _index, const ignition::math::Vector3d &_anchor)=0
Set the anchor point.
ignition::math::Pose3d AnchorErrorPose() const
Get pose offset between anchor pose on child and parent, expressed in the parent link frame.
void SetModel(ModelPtr _model)
Set the model this joint belongs too.
virtual double GetParam(const std::string &_key, unsigned int _index)
Get a non-generic parameter for the joint.
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::Quaterniond AxisFrame(const unsigned int _index) const
Get orientation of reference frame for specified axis, relative to world frame.
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).
ignition::math::Pose3d anchorPose
Anchor pose specified in SDF <joint><pose> tag.
Definition Joint.hh:648
ignition::math::Vector3d anchorPos
Anchor pose.
Definition Joint.hh:641
virtual void SetForce(unsigned int _index, double _effort)=0
Set the force applied to this physics::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 WorldPose() const
Get pose of joint frame relative to world frame.
virtual ignition::math::Vector3d GlobalAxis(unsigned int _index) const =0
Get the axis of rotation in global cooridnate frame.
double GetInertiaRatio(const unsigned int _index) const
Computes moment of inertia (MOI) across a specified joint axis.
ignition::math::Pose3d InitialAnchorPose() const
Get initial Anchor Pose specified by model <joint><pose>...</pose></joint>
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 double PositionImpl(const unsigned int _index=0) const =0
Helper function to get the position of an axis.
virtual void Load(sdf::ElementPtr _sdf)
Load physics::Joint from a SDF sdf::Element.
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.
virtual void FillMsg(msgs::Joint &_msg)
Fill a joint message.
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.
virtual void UpdateParameters(sdf::ElementPtr _sdf)
Update the parameters using new sdf values.
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.
LinkPtr anchorLink
Anchor link.
Definition Joint.hh:654
double velocityLimit[2]
Store Joint velocity limit as specified in SDF.
Definition Joint.hh:672
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.
ConnectionPtr Connect(const std::function< T > &_subscriber)
Connect a callback to this event.
Definition Event.hh:558
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
STL namespace.