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
25#include "gazebo/util/system.hh"
26
27namespace 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
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 private: dJointFeedback *feedback;
279
281 private: double stopCFM;
282
284 private: double stopERP;
285
291 private: double forceApplied[MAX_JOINT_AXIS];
292
295 private: common::Time forceAppliedTime;
296 };
298 }
299}
300#endif
#define MAX_JOINT_AXIS
maximum number of axis per joint anticipated.
Definition Joint.hh:39
A Time class, can be used to hold wall- or sim-time.
Definition Time.hh:48
Wrench information from a joint.
Definition JointWrench.hh:41
Base class for all joints.
Definition Joint.hh:51
ODE joint interface.
Definition ODEJoint.hh:36
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.
dJointID jointId
This is our ODE ID.
Definition ODEJoint.hh:275
double GetERP()
Get the ERP of this joint.
virtual void SetDamping(unsigned int _index, double _damping) override
Set the joint damping.
virtual void Attach(LinkPtr _parent, LinkPtr _child) override
Attach the two bodies with this joint.
void SetCFM(double _cfm)
Set the CFM of this joint.
void ApplyExplicitStiffnessDamping()
simulating a joint spring and damper explicitly.
double GetCFM()
Get the CFM of this joint.
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.
virtual void ApplyStiffnessDamping() override
Callback to apply spring stiffness and viscous damping effects to joint.
virtual void Detach() override
Detach this joint from all links.
void SetERP(double _erp)
Set the ERP of this joint.
bool UsesImplicitSpringDamper()
Get flag indicating whether implicit spring damper is enabled.
ODEJoint(BasePtr _parent)
Constructor.
virtual bool AreConnected(LinkPtr _one, LinkPtr _two) const override
Determines of the two bodies are connected by a joint.
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.
dJointFeedback * GetFeedback()
Get the feedback data structure for this joint, if set.
void ApplyImplicitStiffnessDamping()
simulate implicit spring and damper with CFM/ERP and meddling with Joint limits.
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.
virtual JointWrench GetForceTorque(unsigned int _index) override
get internal force and torque values at a joint.
virtual double GetForce(unsigned int _index) override
virtual void SetProvideFeedback(bool _enable) override
Set whether the joint should generate feedback.
virtual ~ODEJoint()
Destructor.
CFMMode
internal variables used for implicit damping
Definition ODEJoint.hh:39
@ JOINT_LIMIT
implicit damping not active, enforcing joints limits
Definition ODEJoint.hh:45
@ NONE
implicit damping not active
Definition ODEJoint.hh:41
@ DAMPING_ACTIVE
implicit damping active, joints within limits
Definition ODEJoint.hh:43
virtual double GetParam(const std::string &_key, unsigned int _index) override
Get a non-generic parameter for the joint.
virtual void SetLowerLimit(const unsigned int _index, const double _limit) override
Set the joint's lower limit.
virtual void SetForce(unsigned int _index, double _force) override
Set the force applied to this physics::Joint.
void UseImplicitSpringDamper(const bool _implicit)
Set flag indicating whether implicit spring damper is enabled.
virtual void SetUpperLimit(const unsigned int _index, const double _limit) override
Set the joint's upper limit.
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 GetStopERP()
Get access to stopERP.
Definition ODEJoint.hh:159
virtual void SetParam(unsigned int _parameter, double _value)
Set an ODE joint paramter.
virtual void CacheForceTorque() override
Cache Joint Force Torque Values if necessary for physics engine.
double GetStopCFM()
Get access to stopCFM.
Definition ODEJoint.hh:152
virtual void Reset() override
Reset the joint.
virtual void SetForceImpl(unsigned int _index, double _force)=0
Set the force applied to this physics::Joint.
virtual void SetStiffness(unsigned int _index, const double _stiffness) override
Set the joint spring stiffness.
virtual void Load(sdf::ElementPtr _sdf) override
Load physics::Joint from a SDF sdf::Element.
virtual bool SetParam(const std::string &_key, unsigned int _index, const boost::any &_value) override
Set a non-generic parameter for the joint.
virtual LinkPtr GetJointLink(unsigned int _index) const override
Get the link to which the joint is attached according the _index.
virtual double GetParam(unsigned int _parameter) const
Get an ODE joint parameter.
boost::shared_ptr< Link > LinkPtr
Definition PhysicsTypes.hh:109
boost::shared_ptr< Base > BasePtr
Definition PhysicsTypes.hh:77
Forward declarations for the common classes.
Definition Animation.hh:27