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 
145  public: void ApplyImplicitStiffnessDamping();
146 
148  public: void ApplyExplicitStiffnessDamping();
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
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:110
Forward declarations for the common classes.
Definition: Animation.hh:26
double GetStopERP()
Get access to stopERP.
Definition: ODEJoint.hh:159
dJointID jointId
This is our ODE ID.
Definition: ODEJoint.hh:275
double GetStopCFM()
Get access to stopCFM.
Definition: ODEJoint.hh:152
CFMMode
internal variables used for implicit damping
Definition: ODEJoint.hh:38
#define MAX_JOINT_AXIS
maximum number of axis per joint anticipated.
Definition: Joint.hh:39
Base class for all joints.
Definition: Joint.hh:50
Wrench information from a joint.
Definition: JointWrench.hh:40
ODE joint interface.
Definition: ODEJoint.hh:35
boost::shared_ptr< Base > BasePtr
Definition: PhysicsTypes.hh:78
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:47