gazebo / gazebo / physics / Joint.hh

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
/*
 * Copyright 2012 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
/* Desc: The base joint class
 * Author: Nate Koenig, Andrew Howard
 * Date: 21 May 2003
 */

#ifndef _JOINT_HH_
#define _JOINT_HH_

#include <string>

#include <boost/any.hpp>

#include "gazebo/common/Event.hh"
#include "gazebo/common/Events.hh"
#include "gazebo/math/Angle.hh"
#include "gazebo/math/Vector3.hh"
#include "gazebo/msgs/MessageTypes.hh"

#include "gazebo/physics/JointState.hh"
#include "gazebo/physics/Base.hh"
#include "gazebo/physics/JointWrench.hh"

/// \brief maximum number of axis per joint anticipated.
/// Currently, this is 2 as 3-axis joints (e.g. ball)
/// actuation, control is not there yet.
#define MAX_JOINT_AXIS 2

namespace gazebo
{
  namespace physics
  {
    /// \addtogroup gazebo_physics
    /// \{

    /// \class Joint Joint.hh physics/physics.hh
    /// \brief Base class for all joints
    class Joint : public Base
    {
      /// \enum Attribute
      /// \brief Joint attribute types.
      public: enum Attribute
              {
                /// \brief Fudge factor.
                FUDGE_FACTOR,

                /// \brief Suspension error reduction parameter.
                SUSPENSION_ERP,

                /// \brief Suspension constraint force mixing.
                SUSPENSION_CFM,

                /// \brief Stop limit error reduction parameter.
                STOP_ERP,

                /// \brief Stop limit constraint force mixing.
                STOP_CFM,

                /// \brief Error reduction parameter.
                ERP,

                /// \brief Constraint force mixing.
                CFM,

                /// \brief Maximum force.
                FMAX,

                /// \brief Velocity.
                VEL,

                /// \brief High stop angle.
                HI_STOP,

                /// \brief Low stop angle.
                LO_STOP
              };

      /// \brief Constructor
      /// \param[in] Joint parent
      public: explicit Joint(BasePtr _parent);

      /// \brief Destructor
      public: virtual ~Joint();

      /// \brief Set pose, parent and child links of a physics::Joint
      /// \param[in] _parent Parent link.
      /// \param[in] _child Child link.
      /// \param[in] _pose Pose containing Joint Anchor offset from child link.
      public: void Load(LinkPtr _parent, LinkPtr _child,
                        const math::Pose &_pose);

      /// \brief Set parent and child links of a physics::Joint and its
      /// anchor offset position.
      /// This funciton is deprecated, use
      /// Load(LinkPtr _parent, LinkPtr _child, const math::Pose &_pose)
      /// \param[in] _parent Parent link.
      /// \param[in] _child Child link.
      /// \param[in] _pos Joint Anchor offset from child link.
      public: void Load(LinkPtr _parent, LinkPtr _child,
                        const math::Vector3 &_pos) GAZEBO_DEPRECATED(1.5);

      /// \brief Load physics::Joint from a SDF sdf::Element.
      /// \param[in] _sdf SDF values to load from.
      public: virtual void Load(sdf::ElementPtr _sdf);

      /// \brief Initialize a joint.
      public: virtual void Init();

      /// \brief Update the joint.
      public: void Update();

      /// \brief Update the parameters using new sdf values.
      /// \param[in] _sdf SDF values to update from.
      public: virtual void UpdateParameters(sdf::ElementPtr _sdf);

      /// \brief Reset the joint.
      public: virtual void Reset();

      /// \brief Set the joint state.
      /// \param[in] _state Joint state
      public: void SetState(const JointState &_state);

      /// \brief Set the model this joint belongs too.
      /// \param[in] _model Pointer to a model.
      public: void SetModel(ModelPtr _model);

      /// \brief Get the link to which the joint is attached according
      /// the _index.
      /// \param[in] _index Index of the link to retreive.
      /// \return Pointer to the request link. NULL if the index was
      /// invalid.
      public: virtual LinkPtr GetJointLink(int _index) const = 0;

      /// \brief Determines of the two bodies are connected by a joint.
      /// \param[in] _one First link.
      /// \param[in] _two Second link.
      /// \return True if the two links are connected by a joint.
      public: virtual bool AreConnected(LinkPtr _one, LinkPtr _two) const = 0;

      /// \brief Attach the two bodies with this joint.
      /// \param[in] _parent Parent link.
      /// \param[in] _child Child link.
      public: virtual void Attach(LinkPtr _parent, LinkPtr _child);

      /// \brief Detach this joint from all links.
      public: virtual void Detach();

      /// \brief Set the axis of rotation.
      /// \param[in] _index Index of the axis to set.
      /// \param[in] _axis Vector in world frame of axis direction
      ///                  (must have length greater than zero).
      public: virtual void SetAxis(int _index, const math::Vector3 &_axis) = 0;

      /// \brief Set the joint damping.
      /// \param[in] _index Index of the axis to set.
      /// \param[in] _damping Damping value for the axis.
      public: virtual void SetDamping(int _index, double _damping) = 0;

      /// \brief Callback to apply damping force to joint.
      public: virtual void ApplyDamping();

      /// \brief Connect a boost::slot the the joint update signal.
      /// \param[in] _subscriber Callback for the connection.
      /// \return Connection pointer, which must be kept in scope.
      public: template<typename T>
              event::ConnectionPtr ConnectJointUpdate(T _subscriber)
              {return jointUpdate.Connect(_subscriber);}

      /// \brief Disconnect a boost::slot the the joint update signal.
      /// \param[in] _conn Connection to disconnect.
      public: void DisconnectJointUpdate(event::ConnectionPtr &_conn)
              {jointUpdate.Disconnect(_conn);}

      /// \brief Get the axis of rotation.
      /// \param[in] _index Index of the axis to get.
      /// \return Axis value for the provided index.
      public: math::Vector3 GetLocalAxis(int _index) const;

      /// \brief Get the axis of rotation in global cooridnate frame.
      /// \param[in] _index Index of the axis to get.
      /// \return Axis value for the provided index.
      public: virtual math::Vector3 GetGlobalAxis(int _index) const = 0;

      /// \brief Set the anchor point.
      /// \param[in] _index Indx of the axis.
      /// \param[in] _anchor Anchor value.
      public: virtual void SetAnchor(int _index,
                                     const math::Vector3 &_anchor) = 0;

      /// \brief Get the anchor point.
      /// \param[in] _index Index of the axis.
      /// \return Anchor value for the axis.
      public: virtual math::Vector3 GetAnchor(int _index) const = 0;

      /// \brief Set the high stop of an axis(index).
      /// \param[in] _index Index of the axis.
      /// \param[in] _angle High stop angle.
      public: virtual void SetHighStop(int _index,
                                       const math::Angle &_angle);

      /// \brief Set the low stop of an axis(index).
      /// \param[in] _index Index of the axis.
      /// \param[in] _angle Low stop angle.
      public: virtual void SetLowStop(int _index,
                                      const math::Angle &_angle);

      /// \brief Get the high stop of an axis(index).
      /// This function is replaced by GetUpperLimit(unsigned int).
      /// If you are interested in getting the value of dParamHiStop*,
      /// use GetAttribute(hi_stop, _index)
      /// \param[in] _index Index of the axis.
      /// \return Angle of the high stop value.
      public: virtual math::Angle GetHighStop(int _index) = 0;

      /// \brief Get the low stop of an axis(index).
      /// This function is replaced by GetLowerLimit(unsigned int).
      /// If you are interested in getting the value of dParamHiStop*,
      /// use GetAttribute(hi_stop, _index)
      /// \param[in] _index Index of the axis.
      /// \return Angle of the low stop value.
      public: virtual math::Angle GetLowStop(int _index) = 0;

      /// \brief Get the effort limit on axis(index).
      /// \param[in] _index Index of axis, where 0=first axis and 1=second axis
      /// \return Effort limit specified in SDF
      public: virtual double GetEffortLimit(int _index);

      /// \brief Get the velocity limit on axis(index).
      /// \param[in] _index Index of axis, where 0=first axis and 1=second axis
      /// \return Velocity limit specified in SDF
      public: virtual double GetVelocityLimit(int _index);

      /// \brief Set the velocity of an axis(index).
      /// \param[in] _index Index of the axis.
      /// \param[in] _vel Velocity.
      public: virtual void SetVelocity(int _index, double _vel) = 0;

      /// \brief Get the rotation rate of an axis(index)
      /// \param[in] _index Index of the axis.
      /// \return The rotaional velocity of the joint axis.
      public: virtual double GetVelocity(int _index) const = 0;

      /// \brief Set the force applied to this physics::Joint.
      /// Note that the unit of force should be consistent with the rest
      /// of the simulation scales.  E.g.  if you are using
      /// metric units, the unit for force is Newtons.  If using
      /// imperial units (sorry), then unit of force is lb-force
      /// not (lb-mass), etc.
      /// \param[in] _index Index of the axis.
      /// \param[in] _force Force value.
      public: virtual void SetForce(int _index, double _force);

      /// \brief @todo: not yet implemented.
      /// Get the forces applied at this Joint.
      /// Note that the unit of force should be consistent with the rest
      /// of the simulation scales.  E.g.  if you are using
      /// metric units, the unit for force is Newtons.  If using
      /// imperial units (sorry), then unit of force is lb-force
      /// not (lb-mass), etc.
      /// \param[in] _index Index of the axis.
      /// \return The force applied to an axis.
      public: virtual double GetForce(int _index) GAZEBO_DEPRECATED(1.5);

      /// \brief @todo: not yet implemented.
      /// Get the forces applied at this Joint.
      /// Note that the unit of force should be consistent with the rest
      /// of the simulation scales.  E.g.  if you are using
      /// metric units, the unit for force is Newtons.  If using
      /// imperial units (sorry), then unit of force is lb-force
      /// not (lb-mass), etc.
      /// \param[in] _index Index of the axis.
      /// \return The force applied to an axis.
      public: virtual double GetForce(unsigned int _index);

      /// \brief get internal force and torque values at a joint
      /// Note that you must set
      ///   <provide_feedback>true<provide_feedback>
      /// in the joint sdf to use this.
      /// \param[in] _index Force and torque on child link if _index = 0
      /// and on parent link of _index = 1
      /// \return The force and torque at the joint
      public: virtual JointWrench GetForceTorque(int _index)
        GAZEBO_DEPRECATED(1.5) = 0;

      /// \brief get internal force and torque values at a joint
      /// Note that you must set
      ///   <provide_feedback>true<provide_feedback>
      /// in the joint sdf to use this.
      /// \param[in] _index Force and torque on child link if _index = 0
      /// and on parent link of _index = 1
      /// \return The force and torque at the joint
      public: virtual JointWrench GetForceTorque(unsigned int _index) = 0;

      /// \brief Set the max allowed force of an axis(index).
      /// Note that the unit of force should be consistent with the rest
      /// of the simulation scales.  E.g.  if you are using
      /// metric units, the unit for force is Newtons.  If using
      /// imperial units (sorry), then unit of force is lb-force
      /// not (lb-mass), etc.
      /// \param[in] _index Index of the axis.
      /// \param[in] _force Maximum force that can be applied to the axis.
      public: virtual void SetMaxForce(int _index, double _force) = 0;

      /// \brief Get the max allowed force of an axis(index).
      /// Note that the unit of force should be consistent with the rest
      /// of the simulation scales.  E.g.  if you are using
      /// metric units, the unit for force is Newtons.  If using
      /// imperial units (sorry), then unit of force is lb-force
      /// not (lb-mass), etc.
      /// \param[in] _index Index of the axis.
      /// \return The maximum force.
      public: virtual double GetMaxForce(int _index) = 0;

      /// \brief Get the angle of rotation of an axis(index)
      /// \param[in] _index Index of the axis.
      /// \return Angle of the axis.
      public: math::Angle GetAngle(int _index) const;

      /// \brief Get the angle count.
      /// \return The number of DOF for the joint.
      public: virtual unsigned int GetAngleCount() const = 0;

      /// \brief If the Joint is static, Gazebo stores the state of
      /// this Joint as a scalar inside the Joint class, so
      /// this call will NOT move the joint dynamically for a static Model.
      /// But if this Model is not static, then it is updated dynamically,
      /// all the conencted children Link's are moved as a result of the
      /// Joint angle setting.  Dynamic Joint angle update is accomplished
      /// by calling JointController::SetJointPosition.
      /// \param[in] _index Index of the axis.
      /// \param[in] _angle Angle to set the joint to.
      public: void SetAngle(int _index, math::Angle _angle);

      /// \brief Get the forces applied to the center of mass of a physics::Link
      /// due to the existence of this Joint.
      /// Note that the unit of force should be consistent with the rest
      /// of the simulation scales.  E.g.  if you are using
      /// metric units, the unit for force is Newtons.  If using
      /// imperial units (sorry), then unit of force is lb-force
      /// not (lb-mass), etc.
      /// \param[in] index The index of the link(0 or 1).
      /// \return Force applied to the link.
      public: virtual math::Vector3 GetLinkForce(unsigned int _index) const = 0;

      /// \brief Get the torque applied to the center of mass of a physics::Link
      /// due to the existence of this Joint.
      /// Note that the unit of torque should be consistent with the rest
      /// of the simulation scales.  E.g.  if you are using
      /// metric units, the unit for force is Newtons-Meters.  If using
      /// imperial units (sorry), then unit of force is lb-force-inches
      /// not (lb-mass-inches), etc.
      /// \param[in] index The index of the link(0 or 1)
      /// \return Torque applied to the link.
      public: virtual math::Vector3 GetLinkTorque(
                  unsigned int _index) const = 0;

      /// \brief Set a non-generic parameter for the joint.
      /// replaces SetAttribute(Attribute, int, double)
      /// \param[in] _key String key.
      /// \param[in] _index Index of the axis.
      /// \param[in] _value Value of the attribute.
      public: virtual void SetAttribute(const std::string &_key, int _index,
                                        const boost::any &_value) = 0;

      /// \brief Get a non-generic parameter for the joint.
      /// \param[in] _key String key.
      /// \param[in] _index Index of the axis.
      /// \param[in] _value Value of the attribute.
      public: virtual double GetAttribute(const std::string &_key,
                                                unsigned int _index) = 0;

      /// \brief Get the child link
      /// \return Pointer to the child link.
      public: LinkPtr GetChild() const;

      /// \brief Get the parent link.
      /// \return Pointer to the parent link.
      public: LinkPtr GetParent() const;

      /// \brief Fill a joint message.
      /// \param[out] _msg Message to fill with this joint's properties.
      public: void FillMsg(msgs::Joint &_msg);

      /// \brief Accessor to inertia ratio across this joint.
      /// \param[in] _index Joint axis index.
      /// \return returns the inertia ratio across specified joint axis.
      public: double GetInertiaRatio(unsigned int _index) const;

      /// \brief:  get the joint upper limit
      /// (replaces GetLowStop and GetHighStop)
      /// \param[in] _index Index of the axis.
      /// \return Upper limit of the axis.
      public: math::Angle GetLowerLimit(unsigned int _index) const
        {
          if (_index < this->GetAngleCount())
            return this->lowerLimit[_index];

          gzwarn << "requesting lower limit of joint index out of bound\n";
          return math::Angle();
        }

      /// \brief:  get the joint lower limit
      /// (replacee GetLowStop and GetHighStop)
      /// \param[in] _index Index of the axis.
      /// \return Upper limit of the axis.
      public: math::Angle GetUpperLimit(unsigned int _index) const
        {
          if (_index < this->GetAngleCount())
            return this->upperLimit[_index];

          gzwarn << "requesting upper limit of joint index out of bound\n";
          return math::Angle();
        }

      /// \brief Get the angle of an axis helper function.
      /// \param[in] _index Index of the axis.
      /// \return Angle of the axis.
      protected: virtual math::Angle GetAngleImpl(int _index) const = 0;

      /// \brief Helper function to load a joint.
      /// \param[in] _pose Pose of the anchor.
      private: void LoadImpl(const math::Pose &_pose);

      /// \brief Helper function to load a joint.
      /// This function is deprecated, use LoadImpl(math::Pose &)
      /// \param[in] _pos Position of the anchor.
      private: void LoadImpl(const math::Vector3 &_pos) GAZEBO_DEPRECATED(1.5);

      /// \brief Computes inertiaRatio for this joint during Joint::Init
      /// The inertia ratio for each joint between [1, +inf] gives a sense
      /// of how well this model will perform in iterative LCP methods.
      private: void ComputeInertiaRatio();

      /// \brief The first link this joint connects to
      protected: LinkPtr childLink;

      /// \brief The second link this joint connects to
      protected: LinkPtr parentLink;

      /// \brief Pointer to the parent model.
      protected: ModelPtr model;

      /// \brief Anchor pose.  This is the xyz offset of the joint frame from
      /// child frame specified in the parent link frame
      protected: math::Vector3 anchorPos;

      /// \brief Anchor pose specified in SDF <joint><pose> tag.
      /// AnchorPose is the transform from child link frame to joint frame
      /// specified in the child link frame.
      /// AnchorPos is more relevant in normal usage, but sometimes,
      /// we do need this (e.g. GetForceTorque and joint visualization).
      protected: math::Pose anchorPose;

      /// \brief Anchor link.
      protected: LinkPtr anchorLink;

      /// \brief Joint update event.
      private: event::EventT<void ()> jointUpdate;

      /// \brief joint dampingCoefficient
      protected: double dampingCoefficient;

      /// \brief Angle used when the joint is paret of a static model.
      private: math::Angle staticAngle;

      /// \brief apply damping for adding viscous damping forces on updates
      protected: gazebo::event::ConnectionPtr applyDamping;

      /// \brief Save force applied by user
      /// This plus the joint feedback (joint contstraint forces) is the
      /// equivalent of simulated force torque sensor reading
      /// Allocate a 2 vector in case hinge2 joint is used.
      protected: double forceApplied[MAX_JOINT_AXIS];

      /// \brief Store Joint effort limit as specified in SDF
      protected: double effortLimit[MAX_JOINT_AXIS];

      /// \brief Store Joint velocity limit as specified in SDF
      protected: double velocityLimit[MAX_JOINT_AXIS];

      /// \brief Store Joint inertia ratio.  This is a measure of how well
      /// this model behaves using interative LCP solvers.
      protected: double inertiaRatio[MAX_JOINT_AXIS];

      /// \brief Store Joint position lower limit as specified in SDF
      protected: math::Angle lowerLimit[MAX_JOINT_AXIS];

      /// \brief Store Joint position upper limit as specified in SDF
      protected: math::Angle upperLimit[MAX_JOINT_AXIS];

      /// \brief option to use CFM damping
      protected: bool useCFMDamping;
    };
    /// \}
  }
}
#endif
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.