ODEJoint.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 _ODEJOINT_HH_
18 #define _ODEJOINT_HH_
19 
20 #include <boost/any.hpp>
21 #include <string>
22 
24 #include "gazebo/physics/Joint.hh"
25 #include "gazebo/util/system.hh"
26 
27 namespace gazebo
28 {
29  namespace physics
30  {
33 
35  class GZ_PHYSICS_VISIBLE ODEJoint : public Joint
36  {
38  public: enum CFMMode
39  {
41  NONE = 0x00000000,
43  DAMPING_ACTIVE = 0x00000001,
45  JOINT_LIMIT = 0x00000002
46  };
47 
50  public: explicit ODEJoint(BasePtr _parent);
51 
53  public: virtual ~ODEJoint();
54 
55  // Documentation inherited.
56  public: virtual void Load(sdf::ElementPtr _sdf) override;
57 
58  // Documentation inherited.
59  public: virtual void Fini() override;
60 
61  // Documentation inherited.
62  public: virtual void Reset() override;
63 
64  // Documentation inherited.
65  public: virtual LinkPtr GetJointLink(unsigned int _index) const override;
66 
67  // Documentation inherited.
68  public: virtual bool AreConnected(LinkPtr _one, LinkPtr _two) const
69  override;
70 
71  // Documentation inherited.
72  public: virtual void CacheForceTorque() override;
73 
80  public: virtual double GetParam(unsigned int _parameter) const;
81 
88  public: virtual void SetParam(unsigned int _parameter, double _value);
89 
90  // Documentation inherited
91  public: virtual void SetDamping(unsigned int _index, double _damping)
92  override;
93 
94  // Documentation inherited.
95  public: virtual bool SetPosition(
96  const unsigned int _index,
97  const double _position,
98  const bool _preserveWorldVelocity = false)
99  override;
100 
101  // Documentation inherited.
102  public: virtual void SetStiffness(unsigned int _index,
103  const double _stiffness) override;
104 
105  // Documentation inherited.
106  public: virtual void SetStiffnessDamping(unsigned int _index,
107  double _stiffness, double _damping, double _reference = 0) override;
108 
109  // Documentation inherited.
110  public: virtual void Attach(LinkPtr _parent, LinkPtr _child) override;
111 
112  // Documentation inherited.
113  public: virtual void Detach() override;
114 
117  public: void SetERP(double _erp);
118 
121  public: double GetERP();
122 
125  public: void SetCFM(double _cfm);
126 
129  public: double GetCFM();
130 
133  public: dJointFeedback *GetFeedback();
134 
137  public: bool UsesImplicitSpringDamper();
138 
141  public: void UseImplicitSpringDamper(const bool _implicit);
142 
146 
149 
152  public: double GetStopCFM()
153  {
154  return this->stopCFM;
155  }
156 
159  public: double GetStopERP()
160  {
161  return this->stopERP;
162  }
163 
173  private: double ApplyAdaptiveDamping(unsigned int _index,
174  const double _damping);
175 
182  private: void KpKdToCFMERP(const double _dt,
183  const double _kp, const double _kd,
184  double &_cfm, double &_erp);
185 
192  private: void CFMERPToKpKd(const double _dt,
193  const double _cfm, const double _erp,
194  double &_kp, double &_kd);
195 
197  private: int implicitDampingState[MAX_JOINT_AXIS];
198 
200  private: double currentKd[MAX_JOINT_AXIS];
201 
203  private: double currentKp[MAX_JOINT_AXIS];
204 
207  private: bool stiffnessDampingInitialized;
208 
210  private: bool useImplicitSpringDamper;
211 
212  // Documentation inherited.
213  public: virtual void SetUpperLimit(const unsigned int _index,
214  const double _limit) override;
215 
216  // Documentation inherited.
217  public: virtual void SetLowerLimit(const unsigned int _index,
218  const double _limit) override;
219 
220  // Documentation inherited.
221  public: virtual ignition::math::Vector3d LinkForce(
222  const unsigned int _index) const override;
223 
224  // Documentation inherited.
225  public: virtual ignition::math::Vector3d LinkTorque(
226  const unsigned int _index) const override;
227 
228  // Documentation inherited.
229  public: virtual void SetAxis(const unsigned int _index,
230  const ignition::math::Vector3d &_axis) override;
231 
232  // Documentation inherited.
233  public: virtual bool SetParam(const std::string &_key,
234  unsigned int _index,
235  const boost::any &_value) override;
236 
237  // Documentation inherited.
238  public: virtual double GetParam(const std::string &_key,
239  unsigned int _index) override;
240 
241  // Documentation inherited.
242  public: virtual void SetProvideFeedback(bool _enable) override;
243 
244  // Documentation inherited.
245  public: virtual JointWrench GetForceTorque(unsigned int _index) override;
246 
247  // Documentation inherited.
248  public: virtual void SetForce(unsigned int _index, double _force)
249  override;
250 
251  // Documentation inherited.
252  public: virtual double GetForce(unsigned int _index) override;
253 
254  // Documentation inherited.
255  public: virtual void ApplyStiffnessDamping() override;
256 
257  // Documentation inherited.
266  protected: virtual void SetForceImpl(
267  unsigned int _index, double _force) = 0;
268 
272  private: void SaveForce(unsigned int _index, double _force);
273 
275  protected: dJointID jointId;
276 
278  protected: double angleOffset[MAX_JOINT_AXIS];
279 
281  private: dJointFeedback *feedback;
282 
284  private: double stopCFM;
285 
287  private: double stopERP;
288 
294  private: double forceApplied[MAX_JOINT_AXIS];
295 
298  private: common::Time forceAppliedTime;
299  };
301  }
302 }
303 #endif
ODEJoint(BasePtr _parent)
Constructor.
virtual void SetDamping(unsigned int _index, double _damping) override
Set the joint damping.
virtual void SetLowerLimit(const unsigned int _index, const double _limit) override
Set the joint's lower limit.
#define MAX_JOINT_AXIS
maximum number of axis per joint anticipated.
Definition: Joint.hh:39
dJointFeedback * GetFeedback()
Get the feedback data structure for this joint, if set.
ODE joint interface.
Definition: ODEJoint.hh:36
virtual void Load(sdf::ElementPtr _sdf) override
Load physics::Joint from a SDF sdf::Element.
virtual ignition::math::Vector3d LinkTorque(const unsigned int _index) const override
Get the torque applied to the center of mass of a physics::Link due to the existence of this Joint.
virtual LinkPtr GetJointLink(unsigned int _index) const override
Get the link to which the joint is attached according the _index.
CFMMode
internal variables used for implicit damping
Definition: ODEJoint.hh:39
Forward declarations for the common classes.
Definition: Animation.hh:27
A Time class, can be used to hold wall- or sim-time. stored as sec and nano-sec.
Definition: Time.hh:48
virtual void SetAxis(const unsigned int _index, const ignition::math::Vector3d &_axis) override
Set the axis of rotation where axis is specified in local joint frame.
virtual void CacheForceTorque() override
Cache Joint Force Torque Values if necessary for physics engine.
virtual void Reset() override
Reset the joint.
void SetERP(double _erp)
Set the ERP of this joint.
bool UsesImplicitSpringDamper()
Get flag indicating whether implicit spring damper is enabled.
void ApplyExplicitStiffnessDamping()
simulating a joint spring and damper explicitly.
@ DAMPING_ACTIVE
implicit damping active, joints within limits
Definition: ODEJoint.hh:43
void UseImplicitSpringDamper(const bool _implicit)
Set flag indicating whether implicit spring damper is enabled.
virtual ~ODEJoint()
Destructor.
dJointID jointId
This is our ODE ID.
Definition: ODEJoint.hh:275
virtual bool SetPosition(const unsigned int _index, const double _position, const bool _preserveWorldVelocity=false) override
The child links of this joint are updated based on desired position.
double GetCFM()
Get the CFM of this joint.
virtual double GetForce(unsigned int _index) override
Base class for all joints.
Definition: Joint.hh:51
virtual bool AreConnected(LinkPtr _one, LinkPtr _two) const override
Determines of the two bodies are connected by a joint.
virtual void SetStiffnessDamping(unsigned int _index, double _stiffness, double _damping, double _reference=0) override
Set the joint spring stiffness.
virtual void Fini() override
Finialize the object.
double angleOffset[MAX_JOINT_AXIS]
Joint angle(s) when ODE joint has angle 0.
Definition: ODEJoint.hh:278
virtual double GetParam(unsigned int _parameter) const
Get an ODE joint parameter.
virtual JointWrench GetForceTorque(unsigned int _index) override
get internal force and torque values at a joint.
virtual void SetParam(unsigned int _parameter, double _value)
Set an ODE joint paramter.
virtual void SetStiffness(unsigned int _index, const double _stiffness) override
Set the joint spring stiffness.
boost::shared_ptr< Base > BasePtr
Definition: PhysicsTypes.hh:78
virtual ignition::math::Vector3d LinkForce(const unsigned int _index) const override
Get the forces applied to the center of mass of a physics::Link due to the existence of this Joint.
void SetCFM(double _cfm)
Set the CFM of this joint.
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:110
double GetStopCFM()
Get access to stopCFM.
Definition: ODEJoint.hh:152
virtual void Detach() override
Detach this joint from all links.
virtual void Attach(LinkPtr _parent, LinkPtr _child) override
Attach the two bodies with this joint.
double GetStopERP()
Get access to stopERP.
Definition: ODEJoint.hh:159
virtual void SetUpperLimit(const unsigned int _index, const double _limit) override
Set the joint's upper limit.
Wrench information from a joint. These are forces and torques on parent and child Links,...
Definition: JointWrench.hh:41
@ NONE
implicit damping not active
Definition: ODEJoint.hh:41
virtual void ApplyStiffnessDamping() override
Callback to apply spring stiffness and viscous damping effects to joint.
virtual void SetForceImpl(unsigned int _index, double _force)=0
Set the force applied to this physics::Joint.
@ JOINT_LIMIT
implicit damping not active, enforcing joints limits
Definition: ODEJoint.hh:45
virtual bool SetParam(const std::string &_key, unsigned int _index, const boost::any &_value) override
Set a non-generic parameter for the joint.
virtual void SetForce(unsigned int _index, double _force) override
Set the force applied to this physics::Joint.
virtual void SetProvideFeedback(bool _enable) override
Set whether the joint should generate feedback.
virtual double GetParam(const std::string &_key, unsigned int _index) override
Get a non-generic parameter for the joint.
double GetERP()
Get the ERP of this joint.
void ApplyImplicitStiffnessDamping()
simulate implicit spring and damper with CFM/ERP and meddling with Joint limits.