ODEJoint.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2016 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  public: enum CFMMode
36  {
38  NONE = 0x00000000,
40  DAMPING_ACTIVE = 0x00000001,
42  JOINT_LIMIT = 0x00000002
43  };
44 
47  public: ODEJoint(BasePtr _parent);
48 
50  public: virtual ~ODEJoint();
51 
52  // Documentation inherited.
53  public: virtual void Load(sdf::ElementPtr _sdf);
54 
55  // Documentation inherited.
56  public: virtual void Reset();
57 
58  // Documentation inherited.
59  public: virtual LinkPtr GetJointLink(unsigned int _index) const;
60 
61  // Documentation inherited.
62  public: virtual bool AreConnected(LinkPtr _one, LinkPtr _two) const;
63 
64  // Documentation inherited.
65  public: virtual void CacheForceTorque();
66 
73  public: virtual double GetParam(unsigned int _parameter) const;
74 
81  public: virtual void SetParam(unsigned int _parameter, double _value);
82 
83  // Documentation inherited
84  public: virtual void SetDamping(unsigned int _index, double _damping);
85 
86  // Documentation inherited.
87  public: virtual bool SetPosition(unsigned int _index, double _position);
88 
89  // Documentation inherited.
90  public: virtual void SetStiffness(unsigned int _index,
91  const double _stiffness);
92 
93  // Documentation inherited.
94  public: virtual void SetStiffnessDamping(unsigned int _index,
95  double _stiffness, double _damping, double _reference = 0);
96 
97  // Documentation inherited.
98  public: virtual void Attach(LinkPtr _parent, LinkPtr _child);
99 
100  // Documentation inherited.
101  public: virtual void Detach();
102 
105  public: void SetERP(double _erp);
106 
109  public: double GetERP();
110 
113  public: void SetCFM(double _cfm);
114 
117  public: double GetCFM();
118 
121  public: dJointFeedback *GetFeedback();
122 
125  public: bool UsesImplicitSpringDamper();
126 
129  public: void UseImplicitSpringDamper(const bool _implicit);
130 
133  public: void ApplyImplicitStiffnessDamping();
134 
136  public: void ApplyExplicitStiffnessDamping();
137 
140  public: double GetStopCFM()
141  {
142  return this->stopCFM;
143  }
144 
147  public: double GetStopERP()
148  {
149  return this->stopERP;
150  }
151 
161  private: double ApplyAdaptiveDamping(unsigned int _index,
162  const double _damping);
163 
170  private: void KpKdToCFMERP(const double _dt,
171  const double _kp, const double _kd,
172  double &_cfm, double &_erp);
173 
180  private: void CFMERPToKpKd(const double _dt,
181  const double _cfm, const double _erp,
182  double &_kp, double &_kd);
183 
185  private: int implicitDampingState[MAX_JOINT_AXIS];
186 
188  private: double currentKd[MAX_JOINT_AXIS];
189 
191  private: double currentKp[MAX_JOINT_AXIS];
192 
195  private: bool stiffnessDampingInitialized;
196 
198  private: bool useImplicitSpringDamper;
199 
200  // Documentation inherited.
201  public: virtual bool SetHighStop(unsigned int _index,
202  const math::Angle &_angle);
203 
204  // Documentation inherited.
205  public: virtual bool SetLowStop(unsigned int _index,
206  const math::Angle &_angle);
207 
208  // Documentation inherited.
209  public: virtual math::Angle GetHighStop(unsigned int _index);
210 
211  // Documentation inherited.
212  public: virtual math::Angle GetLowStop(unsigned int _index);
213 
214  // Documentation inherited.
215  public: virtual math::Vector3 GetLinkForce(unsigned int _index) const;
216 
217  // Documentation inherited.
218  public: virtual math::Vector3 GetLinkTorque(unsigned int _index) const;
219 
220  // Documentation inherited.
221  public: virtual void SetAxis(unsigned int _index,
222  const math::Vector3 &_axis);
223 
224  // Documentation inherited.
225  public: virtual bool SetParam(const std::string &_key,
226  unsigned int _index,
227  const boost::any &_value);
228 
229  // Documentation inherited.
230  public: virtual double GetParam(const std::string &_key,
231  unsigned int _index);
232 
233  // Documentation inherited.
234  public: virtual void SetProvideFeedback(bool _enable);
235 
236  // Documentation inherited.
237  public: virtual JointWrench GetForceTorque(unsigned int _index);
238 
239  // Documentation inherited.
240  public: virtual void SetForce(unsigned int _index, double _force);
241 
242  // Documentation inherited.
243  public: virtual double GetForce(unsigned int _index);
244 
245  // Documentation inherited.
246  public: virtual void ApplyStiffnessDamping();
247 
248  // Documentation inherited.
257  protected: virtual void SetForceImpl(
258  unsigned int _index, double _force) = 0;
259 
263  private: void SaveForce(unsigned int _index, double _force);
264 
266  protected: dJointID jointId;
267 
269  private: dJointFeedback *feedback;
270 
272  private: double stopCFM;
273 
275  private: double stopERP;
276 
282  private: double forceApplied[MAX_JOINT_AXIS];
283 
286  private: common::Time forceAppliedTime;
287  };
288  }
289 }
290 #endif
boost::shared_ptr< Base > BasePtr
Definition: PhysicsTypes.hh:72
double GetStopCFM()
Get access to stopCFM.
Definition: ODEJoint.hh:140
#define GZ_PHYSICS_VISIBLE
Definition: system.hh:259
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
dJointID jointId
This is our ODE ID.
Definition: ODEJoint.hh:266
CFMMode
internal variables used for implicit damping
Definition: ODEJoint.hh:35
#define MAX_JOINT_AXIS
maximum number of axis per joint anticipated.
Definition: Joint.hh:39
double GetStopERP()
Get access to stopERP.
Definition: ODEJoint.hh:147
Base class for all joints.
Definition: Joint.hh:50
Wrench information from a joint.
Definition: JointWrench.hh:39
ODE joint interface.
Definition: ODEJoint.hh:32
An angle and related functions.
Definition: Angle.hh:53
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:100
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:44