-
Notifications
You must be signed in to change notification settings - Fork 40
/
Joint.hh
390 lines (342 loc) · 15.9 KB
/
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
/*
* Copyright (C) 2018 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.
*
*/
#ifndef IGNITION_PHYSICS_JOINT_HH_
#define IGNITION_PHYSICS_JOINT_HH_
#include <ignition/physics/FeatureList.hh>
#include <ignition/physics/Geometry.hh>
namespace ignition
{
namespace physics
{
/////////////////////////////////////////////////
/// \brief This feature retrieves the generalized joint states such as
/// position, velocity, acceleration of the joint, applied force to the
/// joint and the transformation matrix from the joint's parent link to this
/// joint's child link based on its current position.
class IGNITION_PHYSICS_VISIBLE GetBasicJointState : public virtual Feature
{
/// \brief The Joint API for getting basic joint state
public: template <typename PolicyT, typename FeaturesT>
class Joint : public virtual Feature::Joint<PolicyT, FeaturesT>
{
public: using Scalar = typename PolicyT::Scalar;
public: using Pose = typename FromPolicy<PolicyT>::template Use<Pose>;
/// \brief Get the generalized position of a specific generalized
/// coordinate within this joint.
/// \param[in] _dof
/// The desired generalized coordinate within this joint. Values start
/// from 0 and stop before Joint::GetDegreesOfFreedom().
/// \return the generalized position of _dof
public: Scalar GetPosition(const std::size_t _dof) const;
/// \brief Get the generalized velocity of a specific generalized
/// coordinate within this joint.
/// \param[in] _dof
/// The desired generalized coordinate within this joint. Values start
/// from 0 and stop before Joint::GetDegreesOfFreedom().
/// \return the generalized velocity of _dof
public: Scalar GetVelocity(const std::size_t _dof) const;
/// \brief Get the generalized acceleration of a specific generalized
/// coordinate within this joint.
/// \param[in] _dof
/// The desired generalized coordinate within this joint. Values start
/// from 0 and stop before Joint::GetDegreesOfFreedom().
/// \return the generalized acceleration of _dof
public: Scalar GetAcceleration(const std::size_t _dof) const;
/// \brief Get the generalized force of a specific generalized
/// coordinate within this joint.
/// \param[in] _dof
/// The desired generalized coordinate within this joint. Values start
/// from 0 and stop before Joint::GetDegreesOfFreedom().
/// \return the generalized force of _dof
public: Scalar GetForce(const std::size_t _dof) const;
/// \brief Get the transformation from the joint's parent to the joint's
/// child produced by the current generalized positions and joint
/// properties.
/// \return Transformation matrix of this Joint
public: Pose GetTransform() const;
};
/// \private The implementation API for getting basic joint state
public: template <typename PolicyT>
class Implementation : public virtual Feature::Implementation<PolicyT>
{
public: using Scalar = typename PolicyT::Scalar;
public: using Pose = typename FromPolicy<PolicyT>::template Use<Pose>;
// see Joint::GetPosition above
public: virtual Scalar GetJointPosition(
const Identity &_id, std::size_t _dof) const = 0;
// see Joint::GetVelocity above
public: virtual Scalar GetJointVelocity(
const Identity &_id, std::size_t _dof) const = 0;
// see Joint::GetAcceleration above
public: virtual Scalar GetJointAcceleration(
const Identity &_id, std::size_t _dof) const = 0;
// see Joint::GetForce above
public: virtual Scalar GetJointForce(
const Identity &_id, std::size_t _dof) const = 0;
// see Joint::GetTransform above
public: virtual Pose GetJointTransform(const Identity &_id) const = 0;
};
};
/////////////////////////////////////////////////
/// \brief This feature sets the generalized joint states such as
/// position, velocity, acceleration of the joint and the applied force to
/// the joint.
class IGNITION_PHYSICS_VISIBLE SetBasicJointState : public virtual Feature
{
/// \brief The Joint API for setting basic joint state
public: template <typename PolicyT, typename FeaturesT>
class Joint : public virtual Feature::Joint<PolicyT, FeaturesT>
{
public: using Scalar = typename PolicyT::Scalar;
/// \brief Set the generalized position of a specific generalized
/// coordinate within this joint.
/// \param[in] _dof
/// The desired generalized coordinate within this joint. Values start
/// from 0 and stop before Joint::GetDegreesOfFreedom().
/// \param[in] _value
/// The desired generalized position. Units depend on the underlying
/// joint type.
public: void SetPosition(const std::size_t _dof, const Scalar _value);
/// \brief Set the generalized velocity of a specific generalized
/// coordinate within this joint.
/// \param[in] _dof
/// The desired generalized coordinate within this joint. Values start
/// from 0 and stop before Joint::GetDegreesOfFreedom().
/// \param[in] _value
/// The desired generalized velocity. Units depend on the underlying
/// joint type.
public: void SetVelocity(const std::size_t _dof, const Scalar _value);
/// \brief Set the generalized acceleration of a specific generalized
/// coordinate within this joint.
/// \param[in] _dof
/// The desired generalized coordinate within this joint. Values start
/// from 0 and stop before Joint::GetDegreesOfFreedom().
/// \param[in] _value
/// The desired generalized acceleration. Units depend on the
/// underlying joint type.
public: void SetAcceleration(
const std::size_t _dof, const Scalar _value);
/// \brief Set the generalized force of a specific generalized
/// coordinate within this joint.
/// \param[in] _dof
/// The desired generalized coordinate within this joint. Values start
/// from 0 and stop before Joint::GetDegreesOfFreedom().
/// \param[in] _value
/// The desired generalized force. Units depend on the underlying
/// joint type.
public: void SetForce(const std::size_t _dof, const Scalar _value);
};
/// \private The implementation API for setting basic joint state
public: template <typename PolicyT>
class Implementation : public virtual Feature::Implementation<PolicyT>
{
public: using Scalar = typename PolicyT::Scalar;
// see Joint::SetPosition above
public: virtual void SetJointPosition(
const Identity &_id, std::size_t _dof, Scalar _value) = 0;
// See Joint::SetVelocity above
public: virtual void SetJointVelocity(
const Identity &_id, std::size_t _dof, Scalar _value) = 0;
// See Joint::SetAcceleration above
public: virtual void SetJointAcceleration(
const Identity &_id, std::size_t _dof, Scalar _value) = 0;
// See Joint::SetForce above
public: virtual void SetJointForce(
const Identity &_id, std::size_t _dof, Scalar _value) = 0;
};
};
/////////////////////////////////////////////////
/// \brief This feature retrieves the generalized joint properties such as
/// Degrees Of Freedom (DoF), the transformation matrix from the joint's
/// parent link to this joint and the transformation matrix from this joint
/// to its child link.
class IGNITION_PHYSICS_VISIBLE GetBasicJointProperties
: public virtual Feature
{
/// \brief The Joint API for getting basic joint properties
public: template <typename PolicyT, typename FeaturesT>
class Joint : public virtual Feature::Joint<PolicyT, FeaturesT>
{
public: using Pose = typename FromPolicy<PolicyT>::template Use<Pose>;
/// \brief Get the number of degrees of freedom (a.k.a. number of
/// generalized coordinates) within this joint.
/// \return Number of degrees of freedom
public: std::size_t GetDegreesOfFreedom() const;
/// \brief Get the transformation matrix from the parent link (or world)
/// to the base of the joint transform. The final result of
/// Joint::GetTransform() will be
///
/// \code
/// GetTransformFromParent() * T(q) * GetTransformToChild()
/// \endcode
///
/// where `T(q)` is the transform produced by the current generalized
/// positions of the joint.
///
/// \return Transform from parent
public: Pose GetTransformFromParent() const;
/// \brief Get the transformation matrix from the tip of the joint
/// transform to the child link. The final result of
/// Joint::GetTransform() will be
///
/// \code
/// GetTransformFromParent() * T(q) * GetTransformToChild()
/// \endcode
///
/// where `T(q)` is the transform produced by the current generalized
/// positions of the joint.
///
/// \return Transform to child
public: Pose GetTransformToChild() const;
};
/// \private The implementation API for getting basic joint properties
public: template <typename PolicyT>
class Implementation : public virtual Feature::Implementation<PolicyT>
{
public: using Pose = typename FromPolicy<PolicyT>::template Use<Pose>;
public: virtual std::size_t GetJointDegreesOfFreedom(
const Identity &_id) const = 0;
public: virtual Pose GetJointTransformFromParent(
const Identity &_id) const = 0;
public: virtual Pose GetJointTransformToChild(
const Identity &_id) const = 0;
};
};
/////////////////////////////////////////////////
class IGNITION_PHYSICS_VISIBLE SetJointTransformFromParentFeature
: public virtual Feature
{
/// \brief The Joint API for setting the transform from the joint's parent
public: template <typename PolicyT, typename FeaturesT>
class Joint : public virtual Feature::Joint<PolicyT, FeaturesT>
{
public: using Pose = typename FromPolicy<PolicyT>::template Use<Pose>;
/// \brief Set the transformation matrix from the parent link (or world)
/// to the base of the joint transform. The final result of
/// Joint::GetTransform() will be
///
/// \code
/// GetTransformFromParent() * T(q) * GetTransformToChild()
/// \endcode
///
/// where `T(q)` is the transform produced by the current generalized
/// positions of the joint.
///
/// \param[in] _pose
/// The desired transformation matrix
public: void SetTransformFromParent(const Pose &_pose);
};
/// \private The implementation API for setting the transform from the
/// parent
public: template <typename PolicyT>
class Implementation : public virtual Feature::Implementation<PolicyT>
{
public: using Pose = typename FromPolicy<PolicyT>::template Use<Pose>;
// see Joint::SetTransformFromParent above
public: virtual void SetJointTransformFromParent(
const Identity &_id, const Pose &_pose) = 0;
};
};
/////////////////////////////////////////////////
class IGNITION_PHYSICS_VISIBLE SetJointTransformToChildFeature
: public virtual Feature
{
/// \brief The Joint API for setting the transform to the joint's child
public: template <typename PolicyT, typename FeaturesT>
class Joint : public virtual Feature::Joint<PolicyT, FeaturesT>
{
public: using Pose = typename FromPolicy<PolicyT>::template Use<Pose>;
/// \brief Set the transformation matrix from the tip of the joint
/// transform to the child link. The final result of
/// Joint::GetTransform() will be
///
/// \code
/// GetTransformFromParent() * T(q) * GetTransformToChild()
/// \endcode
///
/// where `T(q)` is the transform produced by the current generalized
/// positions of the joint.
///
/// \param[in] _pose
/// The desired transformation matrix
public: void SetTransformToChild(const Pose &_pose);
};
/// \private The implementation API for setting the transform to the child
public: template <typename PolicyT>
class Implementation : public virtual Feature::Implementation<PolicyT>
{
public: using Pose = typename FromPolicy<PolicyT>::template Use<Pose>;
// see Joint::SetTransformToChild above
public: virtual void SetJointTransformToChild(
const Identity &_id, const Pose &_pose) = 0;
};
};
/////////////////////////////////////////////////
/// \brief This feature sets the commanded value of generalized velocity of
/// this Joint.
class IGNITION_PHYSICS_VISIBLE SetJointVelocityCommandFeature
: public virtual Feature
{
/// \brief The Joint API for setting velocity commands (target velocity).
/// This is different from SetVelocity in that this does not modify the
/// internal state of the joint. Instead, the physics engine is expected
/// to compute the necessary joint torque for the commanded velocity and
/// apply it in the next simulation step.
public: template <typename PolicyT, typename FeaturesT>
class Joint : public virtual Feature::Joint<PolicyT, FeaturesT>
{
public: using Scalar = typename PolicyT::Scalar;
/// \brief Set the commanded value of generalized velocity of a specific
/// generalized coordinate within this joint.
/// \param[in] _dof
/// The desired generalized coordinate within this joint. Values start
/// from 0 and stop before Joint::GetDegreesOfFreedom().
/// \param[in] _value
/// The desired generalized velocity. Units depend on the underlying
/// joint type.
public: void SetVelocityCommand(
const std::size_t _dof, const Scalar _value);
};
/// \private The implementation API for setting joint velocity commands
public: template <typename PolicyT>
class Implementation : public virtual Feature::Implementation<PolicyT>
{
public: using Scalar = typename PolicyT::Scalar;
// See Joint::SetVelocityCommand above
public: virtual void SetJointVelocityCommand(
const Identity &_id, std::size_t _dof, Scalar _value) = 0;
};
};
class IGNITION_PHYSICS_VISIBLE DetachJointFeature
: public virtual Feature
{
public: template <typename PolicyT, typename FeaturesT>
class Joint : public virtual Feature::Joint<PolicyT, FeaturesT>
{
/// \brief Detach this joint.
public: void Detach();
};
public: template <typename PolicyT>
class Implementation : public virtual Feature::Implementation<PolicyT>
{
public: virtual void DetachJoint(const Identity &_jointID) = 0;
};
};
}
}
#include <ignition/physics/detail/Joint.hh>
#endif