Branch data Line data Source code
1 : : // Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
2 : : // Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
3 : : // RDL - Robot Dynamics Library
4 : : // Licensed under the zlib license. See LICENSE for more details.
5 : :
6 : : #ifndef __RDL_JOINT_H__
7 : : #define __RDL_JOINT_H__
8 : :
9 : : /**
10 : : * @file Joint.hpp
11 : : */
12 : :
13 : : #include <assert.h>
14 : : #include <iostream>
15 : : #include <rdl_dynamics/RdlExceptions.hpp>
16 : : #include <rdl_dynamics/rdl_eigenmath.hpp>
17 : : namespace RobotDynamics
18 : : {
19 : : struct Model;
20 : :
21 : : /** \page joint_description Joint Modeling
22 : : *
23 : : * \section joint_overview Overview
24 : : *
25 : : * The Robot Dynamics Library supports a multitude of joints:
26 : : * revolute, planar, fixed, singularity-free spherical joints and joints
27 : : * with multiple degrees of freedom in any combinations.
28 : : *
29 : : * Fixed joints do not cause any overhead in RDL as the bodies that are
30 : : * rigidly connected are merged into a single body. For details see \ref joint_models_fixed.
31 : : *
32 : : * Joints with multiple degrees of freedom are emulated by default which
33 : : * means that they are split up into multiple single degree of freedom
34 : : * joints which results in equivalent models. This has the benefit that it
35 : : * simplifies the required algebra and also code branching in RDL. A
36 : : * special case are joints with three degrees of freedom for which specific
37 : : * joints are available that should be used for performance reasons
38 : : * whenever possible. See \ref joint_three_dof for details.
39 : : *
40 : : * Joints are defined by their motion subspace. For each degree of freedom
41 : : * a one dimensional motion subspace is specified as a Math::SpatialVector.
42 : : * This vector follows the following convention: \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
43 : : *
44 : : * To specify a planar joint with three degrees of freedom for which the
45 : : * first two are translations in \f$x\f$ and \f$y\f$ direction and the last
46 : : * is a rotation around \f$z\f$, the following joint definition can be
47 : : * used:
48 : :
49 : : * \code Joint planar_joint = Joint (
50 : : * Math::SpatialVector (0., 0., 0., 1., 0., 0.),
51 : : * Math::SpatialVector (0., 0., 0., 0., 1., 0.),
52 : : * Math::SpatialVector (0., 0., 1., 0., 0., 0.)
53 : : * );
54 : : * \endcode
55 : :
56 : : * \note Please note that in the Robot Dynamics Library all angles
57 : : * are specified in radians.
58 : : *
59 : : * \section joint_models_fixed Fixed Joints
60 : : *
61 : : * Fixed joints do not add an additional degree of freedom to the model.
62 : : * When adding a body that via a fixed joint (i.e. when the type is
63 : : * JointTypeFixed) then the dynamical parameters mass and inertia are
64 : : * merged onto its moving parent. By doing so fixed bodies do not add
65 : : * computational costs when performing dynamics computations.
66 : :
67 : : * To ensure a consistent API for the Kinematics such fixed bodies have a
68 : : * different range of ids. Where as the ids start at 1 get incremented for
69 : : * each added body, fixed bodies start at Model::fixed_body_discriminator
70 : : * which has a default value of std::numeric_limits<unsigned int>::max() /
71 : : * 2. This means theoretical a maximum of each 2147483646 movable and fixed
72 : : * bodies are possible.
73 : :
74 : : * To check whether a body is connected by a fixed joint you can use the
75 : : * function Model::IsFixedBodyId().
76 : :
77 : : * \section joint_three_dof 3-DoF Joints
78 : : *
79 : : * RDL has highly efficient implementations for the following three degree
80 : : * of freedom joints:
81 : : * <ul>
82 : : * <li>\ref JointTypeTranslationXYZ which first translates along X, then
83 : : * Y, and finally Z.</li>
84 : : * <li>\ref JointTypeEulerZYX which first rotates around Z, then Y, and
85 : : * then X.</li>
86 : : * <li>\ref JointTypeEulerXYZ which first rotates around X, then Y, and
87 : : * then Z.</li>
88 : : * <li>\ref JointTypeEulerYXZ which first rotates around Y, then X, and
89 : : * then Z.</li>
90 : : * <li>\ref JointTypeSpherical which is a singularity free joint that
91 : : * uses a Quaternion and the bodies angular velocity (see \ref
92 : : * joint_singularities for details).</li>
93 : : * </ul>
94 : : *
95 : : * These joints can be created by providing the joint type as an argument
96 : : * to the Joint constructor, e.g.:
97 : : *
98 : : * \code Joint joint_rot_zyx = Joint ( JointTypeEulerZYX ); \endcode
99 : : *
100 : : * Using 3-Dof joints is always favourable over using their emulated
101 : : * counterparts as they are considerably faster and describe the same
102 : : * kinematics and dynamics.
103 : :
104 : : * \section joint_floatingbase Floating-Base Joint (a.k.a. Freeflyer Joint)
105 : : *
106 : : * RDL has a special joint type for floating-base systems that uses the
107 : : * enum JointTypeFloatingBase. The first three DoF are translations along
108 : : * X,Y, and Z. For the rotational part it uses a JointTypeSpherical joint.
109 : : * It is internally modeled by a JointTypeTranslationXYZ and a
110 : : * JointTypeSpherical joint. It is recommended to only use this joint for
111 : : * the very first body added to the model.
112 : : *
113 : : * Positional variables are translations along X, Y, and Z, and for
114 : : * rotations it uses Quaternions. To set/get the orientation use
115 : : * Model::SetQuaternion () / Model::GetQuaternion() with the body id
116 : : * returned when adding the floating base (i.e. the call to
117 : : * Model::addBody() or Model::appendBody()).
118 : :
119 : : * \section joint_singularities Joint Singularities
120 : :
121 : : * Singularities in the models arise when a joint has three rotational
122 : : * degrees of freedom and the rotations are described by Euler- or
123 : : * Cardan-angles. The singularities present in these rotation
124 : : * parametrizations (e.g. for ZYX Euler-angles for rotations where a
125 : : * +/- 90 degrees rotation around the Y-axis) may cause problems in
126 : : * dynamics calculations, such as a rank-deficit joint-space inertia matrix
127 : : * or exploding accelerations in the forward dynamics calculations.
128 : : *
129 : : * For this case RDL has the special joint type
130 : : * RobotDynamics::JointTypeSpherical. When using this joint type the
131 : : * model does not suffer from singularities, however this also results in
132 : : * a change of interpretation for the values \f$\mathbf{q}, \mathbf{\dot{q}}, \mathbf{\ddot{q}}\f$, and \f$\mathbf{\tau}\f$:
133 : : *
134 : : * <ul>
135 : : * <li> The values in \f$\mathbf{q}\f$ for the joint parameterizes the orientation of a joint using a
136 : : * Quaternion \f$q_i\f$ </li>
137 : : * <li> The values in \f$\mathbf{\dot{q}}\f$ for the joint describe the angular
138 : : * velocity \f$\omega\f$ of the joint in body coordinates</li>
139 : : * <li> The values in \f$\mathbf{\ddot{q}}\f$ for the joint describe the angular
140 : : * acceleration \f$\dot{\omega}\f$ of the joint in body coordinates</li>
141 : : * <li> The values in \f$\mathbf{\tau}\f$ for the joint describe the three couples
142 : : * acting on the body in body coordinates that are actuated by the joint.</li>
143 : : * </ul>
144 : :
145 : : * As a result, the dimension of the vector \f$\mathbf{q}\f$ is higher than
146 : : * of the vector of the velocity variables. Additionally, the values in
147 : : * \f$\mathbf{\dot{q}}\f$ are \b not the derivative of \f$q\f$ and are therefore
148 : : * denoted by \f$\mathbf{\bar{q}}\f$ (and similarly for the joint
149 : : * accelerations).
150 : :
151 : : * RDL stores the Quaternions in \f$\mathbf{q}\f$ such that the 4th component of
152 : : * the joint is appended to \f$\mathbf{q}\f$. E.g. for a model with the joints:
153 : : * TX, Spherical, TY, Spherical, the values of \f$\mathbf{q},\mathbf{\bar{q}},\mathbf{\bar{\bar{q}}},\mathbf{\tau}\f$ are:
154 : : *
155 : :
156 : : \f{eqnarray*}
157 : : \mathbf{q} &=& ( q_{tx}, q_{q1,x}, q_{q1,y}, q_{q1,z}, q_{ty}, q_{q2,x}, q_{q2,y}, q_{q2,z}, q_{q1,w}, q_{q2,w})^T
158 : : *\\
159 : : \mathbf{\bar{q}} &=& ( \dot{q}_{tx}, \omega_{1,x}, \omega_{1,y}, \omega_{1,z}, \dot{q}_{ty}, \omega_{2,x}, \omega_{2,y},
160 : : *\\*\omega_{2,z} )^T \\
161 : : \mathbf{\bar{\bar{q}}} &=& ( \ddot{q}_{tx}, \dot{\omega}_{1,x}, \dot{\omega}_{1,y}, \dot{\omega}_{1,z}, \ddot{q}_{ty},
162 : : *\\*\dot{\omega}_{2,x}, \dot{\omega}_{2,y}, \dot{\omega}_{2,z} )^T \\
163 : : \mathbf{\tau} &=& ( \tau_{tx}, \tau_{1,x}, \tau_{1,y}, \tau_{1,z}, \tau_{ty}, \tau_{2,x}, \tau_{2,y}, \tau_{2,z} )^T
164 : : \f}
165 : :
166 : : * \subsection spherical_integration Numerical Integration of Quaternions
167 : : *
168 : : * An additional consequence of this is, that special treatment is required
169 : : * when numerically integrating the angular velocities. One possibility is
170 : : * to interpret the angular velocity as an axis-angle pair scaled by the
171 : : * timestep and use it create a quaternion that is applied to the previous
172 : : * Quaternion. Another is to compute the quaternion rates from the angular
173 : : * velocity. For details see James Diebel "Representing Attitude: Euler
174 : : * Angles, Unit Quaternions, and Rotation Vectors", 2006,
175 : : * http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.110.5134.
176 : : *
177 : : */
178 : :
179 : : /** \brief General types of joints
180 : : */
181 : : enum JointType
182 : : {
183 : : JointTypeUndefined = 0,
184 : : JointTypeRevolute,
185 : : JointTypePrismatic,
186 : : JointTypeRevoluteX,
187 : : JointTypeRevoluteY,
188 : : JointTypeRevoluteZ,
189 : : JointTypeSpherical, ///< 3 DoF joint using Quaternions for joint positional variables and
190 : : // angular velocity for joint velocity variables.
191 : : JointTypeEulerZYX, ///< 3 DoF joint that uses Euler ZYX convention (faster than emulated
192 : : // multi DoF joints).
193 : : JointTypeEulerXYZ, ///< 3 DoF joint that uses Euler XYZ convention (faster than emulated
194 : : // multi DoF joints).
195 : : JointTypeEulerYXZ, ///< 3 DoF joint that uses Euler YXZ convention (faster than emulated
196 : : // multi DoF joints).
197 : : JointTypeTranslationXYZ,
198 : : JointTypeFloatingBase, ///< A 6-DoF joint for floating-base (or freeflyer) systems.
199 : : JointTypeFixed, ///< Fixed joint which causes the inertial properties to be merged with
200 : : // the parent body.
201 : : JointType1DoF,
202 : : JointType2DoF, ///< Emulated 2 DoF joint.
203 : : JointType3DoF, ///< Emulated 3 DoF joint.
204 : : JointType4DoF, ///< Emulated 4 DoF joint.
205 : : JointType5DoF, ///< Emulated 5 DoF joint.
206 : : JointType6DoF, ///< Emulated 6 DoF joint.
207 : : JointTypeCustom, ///< User defined joints of varying size
208 : : };
209 : :
210 : : /**
211 : : * @struct Joint
212 : : * \brief Describes a joint relative to the predecessor body.
213 : : *
214 : : * @note The construction of a Joint is NOT a realtime safe procedure, so be sure you are not
215 : : * creating temporary joints at runtime. For maximum performance, you should create all joints
216 : : * at config time and just reference those.
217 : : *
218 : : * This class contains all information required for one single joint. This
219 : : * contains the joint type and the axis of the joint. See \ref joint_description for detailed description.
220 : : *
221 : : */
222 : : struct Joint
223 : : {
224 : 2194 : Joint() : mJointAxes(nullptr), mJointType(JointTypeUndefined), mDoFCount(0), q_index(0), custom_joint_index(-1)
225 : : {
226 : 2194 : }
227 : :
228 : 666 : explicit Joint(JointType type) : mJointAxes(nullptr), mJointType(type), mDoFCount(0), q_index(0), custom_joint_index(-1)
229 : : {
230 : 666 : if (type == JointTypeRevoluteX)
231 : : {
232 : 133 : mDoFCount = 1;
233 : 266 : mJointAxes = new Math::SpatialVector[mDoFCount];
234 : 133 : mJointAxes[0] = Math::SpatialVector(1., 0., 0., 0., 0., 0.);
235 : : }
236 : 533 : else if (type == JointTypeRevoluteY)
237 : : {
238 : 35 : mDoFCount = 1;
239 : 70 : mJointAxes = new Math::SpatialVector[mDoFCount];
240 : 35 : mJointAxes[0] = Math::SpatialVector(0., 1., 0., 0., 0., 0.);
241 : : }
242 : 498 : else if (type == JointTypeRevoluteZ)
243 : : {
244 : 19 : mDoFCount = 1;
245 : 38 : mJointAxes = new Math::SpatialVector[mDoFCount];
246 : 19 : mJointAxes[0] = Math::SpatialVector(0., 0., 1., 0., 0., 0.);
247 : : }
248 : 479 : else if (type == JointTypeSpherical)
249 : : {
250 : 43 : mDoFCount = 3;
251 : :
252 : 172 : mJointAxes = new Math::SpatialVector[mDoFCount];
253 : :
254 : 43 : mJointAxes[0] = Math::SpatialVector(0., 0., 1., 0., 0., 0.);
255 : 43 : mJointAxes[1] = Math::SpatialVector(0., 1., 0., 0., 0., 0.);
256 : 43 : mJointAxes[2] = Math::SpatialVector(1., 0., 0., 0., 0., 0.);
257 : : }
258 : 436 : else if (type == JointTypeEulerZYX)
259 : : {
260 : 120 : mDoFCount = 3;
261 : :
262 : 480 : mJointAxes = new Math::SpatialVector[mDoFCount];
263 : :
264 : 120 : mJointAxes[0] = Math::SpatialVector(0., 0., 1., 0., 0., 0.);
265 : 120 : mJointAxes[1] = Math::SpatialVector(0., 1., 0., 0., 0., 0.);
266 : 120 : mJointAxes[2] = Math::SpatialVector(1., 0., 0., 0., 0., 0.);
267 : : }
268 : 316 : else if (type == JointTypeEulerXYZ)
269 : : {
270 : 7 : mDoFCount = 3;
271 : :
272 : 28 : mJointAxes = new Math::SpatialVector[mDoFCount];
273 : :
274 : 7 : mJointAxes[0] = Math::SpatialVector(1., 0., 0., 0., 0., 0.);
275 : 7 : mJointAxes[1] = Math::SpatialVector(0., 1., 0., 0., 0., 0.);
276 : 7 : mJointAxes[2] = Math::SpatialVector(0., 0., 1., 0., 0., 0.);
277 : : }
278 : 309 : else if (type == JointTypeEulerYXZ)
279 : : {
280 : 65 : mDoFCount = 3;
281 : :
282 : 260 : mJointAxes = new Math::SpatialVector[mDoFCount];
283 : :
284 : 65 : mJointAxes[0] = Math::SpatialVector(0., 1., 0., 0., 0., 0.);
285 : 65 : mJointAxes[1] = Math::SpatialVector(1., 0., 0., 0., 0., 0.);
286 : 65 : mJointAxes[2] = Math::SpatialVector(0., 0., 1., 0., 0., 0.);
287 : : }
288 : 244 : else if (type == JointTypeTranslationXYZ)
289 : : {
290 : 80 : mDoFCount = 3;
291 : :
292 : 320 : mJointAxes = new Math::SpatialVector[mDoFCount];
293 : :
294 : 80 : mJointAxes[0] = Math::SpatialVector(0., 0., 0., 1., 0., 0.);
295 : 80 : mJointAxes[1] = Math::SpatialVector(0., 0., 0., 0., 1., 0.);
296 : 80 : mJointAxes[2] = Math::SpatialVector(0., 0., 0., 0., 0., 1.);
297 : : }
298 : 164 : else if ((type >= JointType1DoF) && (type <= JointType6DoF))
299 : : {
300 : : // create a joint and allocate memory for it.
301 : : // Warning: the memory does not get initialized by this function!
302 : 3 : mDoFCount = type - JointType1DoF + 1;
303 : 15 : mJointAxes = new Math::SpatialVector[mDoFCount];
304 : : }
305 : 161 : else if (type == JointTypeCustom)
306 : : {
307 : : // This constructor cannot be used for a JointTypeCustom because
308 : : // we have no idea what mDoFCount is.
309 : 1 : throw RdlException("Error: Invalid use of Joint constructor Joint(JointType type). Only allowed when type != JointTypeCustom");
310 : : }
311 : 160 : else if ((type != JointTypeFixed) && (type != JointTypeFloatingBase))
312 : : {
313 : 1 : throw RdlException("Error: Invalid use of Joint constructor Joint(JointType type).");
314 : : }
315 : 664 : }
316 : :
317 : 69 : Joint(JointType type, int degreesOfFreedom) : mJointAxes(nullptr), mJointType(type), mDoFCount(0), q_index(0), custom_joint_index(-1)
318 : : {
319 : 69 : if (type == JointTypeCustom)
320 : : {
321 : 68 : mDoFCount = degreesOfFreedom;
322 : 192 : mJointAxes = new Math::SpatialVector[mDoFCount];
323 : :
324 : 192 : for (unsigned int i = 0; i < mDoFCount; ++i)
325 : : {
326 : 124 : mJointAxes[i] = Math::SpatialVector(0., 0., 0., 0., 0., 0.);
327 : : }
328 : : }
329 : : else
330 : : {
331 : 1 : throw RdlException("Error: Invalid use of Joint constructor Joint(JointType type, int degreesOfFreedom). Only allowed when type == JointTypeCustom.");
332 : : }
333 : 68 : }
334 : :
335 : 14189 : Joint(const Joint& joint) : mJointType(joint.mJointType), mDoFCount(joint.mDoFCount), q_index(joint.q_index), custom_joint_index(joint.custom_joint_index)
336 : : {
337 : 29373 : mJointAxes = new Math::SpatialVector[mDoFCount];
338 : :
339 : 29373 : for (unsigned int i = 0; i < mDoFCount; i++)
340 : : {
341 : 15184 : mJointAxes[i] = joint.mJointAxes[i];
342 : : }
343 : 14189 : }
344 : :
345 : 3782 : Joint& operator=(const Joint& joint)
346 : : {
347 : 3782 : if (this != &joint)
348 : : {
349 : 3782 : if (mDoFCount > 0)
350 : : {
351 : 2260 : assert(mJointAxes);
352 : 2260 : delete[] mJointAxes;
353 : : }
354 : 3782 : mJointType = joint.mJointType;
355 : 3782 : mDoFCount = joint.mDoFCount;
356 : 3782 : custom_joint_index = joint.custom_joint_index;
357 : 7762 : mJointAxes = new Math::SpatialVector[mDoFCount];
358 : :
359 : 7762 : for (unsigned int i = 0; i < mDoFCount; i++)
360 : : {
361 : 3980 : mJointAxes[i] = joint.mJointAxes[i];
362 : : }
363 : :
364 : 3782 : q_index = joint.q_index;
365 : : }
366 : 3782 : return *this;
367 : : }
368 : :
369 : 20868 : ~Joint()
370 : : {
371 : 20868 : if (mJointAxes)
372 : : {
373 : 20037 : assert(mJointAxes);
374 : 20037 : delete[] mJointAxes;
375 : 20037 : mJointAxes = nullptr;
376 : 20037 : mDoFCount = 0;
377 : 20037 : custom_joint_index = -1;
378 : : }
379 : 20868 : }
380 : :
381 : : /** \brief Constructs a joint from the given cartesian parameters.
382 : : *
383 : : * This constructor creates all the required spatial values for the given
384 : : * cartesian parameters.
385 : : *
386 : : * \param joint_type whether the joint is revolute or prismatic
387 : : * \param joint_axis the axis of rotation or translation
388 : : */
389 : 370 : Joint(const JointType joint_type, const Math::Vector3d& joint_axis) : mDoFCount(1), q_index(0), custom_joint_index(-1)
390 : : {
391 : 740 : mJointAxes = new Math::SpatialVector[mDoFCount];
392 : :
393 : : // Some assertions, as we concentrate on simple cases
394 : :
395 : : // Only rotation around the Z-axis
396 : 370 : assert(joint_type == JointTypeRevolute || joint_type == JointTypePrismatic);
397 : :
398 : 370 : mJointType = joint_type;
399 : :
400 : 370 : if (joint_type == JointTypeRevolute)
401 : : {
402 : : // make sure we have a unit axis
403 : 13 : mJointAxes[0].set(joint_axis[0], joint_axis[1], joint_axis[2], 0., 0., 0.);
404 : : }
405 : 357 : else if (joint_type == JointTypePrismatic)
406 : : {
407 : : // make sure we have a unit axis
408 : 357 : assert(joint_axis.squaredNorm() == 1.);
409 : :
410 : 357 : mJointAxes[0].set(0., 0., 0., joint_axis[0], joint_axis[1], joint_axis[2]);
411 : : }
412 : 370 : }
413 : :
414 : : /** \brief Constructs a 1 DoF joint with the given motion subspaces.
415 : : *
416 : : * The motion subspaces are of the format:
417 : : * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
418 : : *
419 : : * \note So far only pure rotations or pure translations are supported.
420 : : *
421 : : * \param axis_0 Motion subspace for axis 0
422 : : */
423 : 3498 : explicit Joint(const Math::SpatialVector& axis_0) : mDoFCount(1), q_index(0), custom_joint_index(-1)
424 : : {
425 : 6996 : mJointAxes = new Math::SpatialVector[mDoFCount];
426 : 3498 : mJointAxes[0] = Math::SpatialVector(axis_0);
427 : :
428 : 3498 : if (axis_0 == Math::SpatialVector(1., 0., 0., 0., 0., 0.))
429 : : {
430 : 650 : mJointType = JointTypeRevoluteX;
431 : : }
432 : 2848 : else if (axis_0 == Math::SpatialVector(0., 1., 0., 0., 0., 0.))
433 : : {
434 : 1304 : mJointType = JointTypeRevoluteY;
435 : : }
436 : 1544 : else if (axis_0 == Math::SpatialVector(0., 0., 1., 0., 0., 0.))
437 : : {
438 : 1223 : mJointType = JointTypeRevoluteZ;
439 : : }
440 : : else
441 : : {
442 : 321 : mJointType = JointType1DoF;
443 : : }
444 : 3498 : validate_spatial_axis(mJointAxes[0]);
445 : 3498 : }
446 : :
447 : : /** \brief Constructs a 2 DoF joint with the given motion subspaces.
448 : : *
449 : : * The motion subspaces are of the format:
450 : : * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
451 : : *
452 : : * \note So far only pure rotations or pure translations are supported.
453 : : *
454 : : * \param axis_0 Motion subspace for axis 0
455 : : * \param axis_1 Motion subspace for axis 1
456 : : */
457 : 63 : Joint(const Math::SpatialVector& axis_0, const Math::SpatialVector& axis_1) : mJointType(JointType2DoF), mDoFCount(2), q_index(0), custom_joint_index(-1)
458 : : {
459 : 189 : mJointAxes = new Math::SpatialVector[mDoFCount];
460 : 63 : mJointAxes[0] = axis_0;
461 : 63 : mJointAxes[1] = axis_1;
462 : :
463 : 63 : validate_spatial_axis(mJointAxes[0]);
464 : 63 : validate_spatial_axis(mJointAxes[1]);
465 : 63 : }
466 : :
467 : : /** \brief Constructs a 3 DoF joint with the given motion subspaces.
468 : : *
469 : : * The motion subspaces are of the format:
470 : : * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
471 : : *
472 : : * \note So far only pure rotations or pure translations are supported.
473 : : *
474 : : * \param axis_0 Motion subspace for axis 0
475 : : * \param axis_1 Motion subspace for axis 1
476 : : * \param axis_2 Motion subspace for axis 2
477 : : */
478 : 128 : Joint(const Math::SpatialVector& axis_0, const Math::SpatialVector& axis_1, const Math::SpatialVector& axis_2)
479 : 128 : : mJointType(JointType3DoF), mDoFCount(3), q_index(0), custom_joint_index(-1)
480 : : {
481 : 512 : mJointAxes = new Math::SpatialVector[mDoFCount];
482 : :
483 : 128 : mJointAxes[0] = axis_0;
484 : 128 : mJointAxes[1] = axis_1;
485 : 128 : mJointAxes[2] = axis_2;
486 : :
487 : 128 : validate_spatial_axis(mJointAxes[0]);
488 : 128 : validate_spatial_axis(mJointAxes[1]);
489 : 128 : validate_spatial_axis(mJointAxes[2]);
490 : 128 : }
491 : :
492 : : /** \brief Constructs a 4 DoF joint with the given motion subspaces.
493 : : *
494 : : * The motion subspaces are of the format:
495 : : * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
496 : : *
497 : : * \note So far only pure rotations or pure translations are supported.
498 : : *
499 : : * \param axis_0 Motion subspace for axis 0
500 : : * \param axis_1 Motion subspace for axis 1
501 : : * \param axis_2 Motion subspace for axis 2
502 : : * \param axis_3 Motion subspace for axis 3
503 : : */
504 : 1 : Joint(const Math::SpatialVector& axis_0, const Math::SpatialVector& axis_1, const Math::SpatialVector& axis_2, const Math::SpatialVector& axis_3)
505 : 1 : : mJointType(JointType4DoF), mDoFCount(4), q_index(0), custom_joint_index(-1)
506 : : {
507 : 5 : mJointAxes = new Math::SpatialVector[mDoFCount];
508 : :
509 : 1 : mJointAxes[0] = axis_0;
510 : 1 : mJointAxes[1] = axis_1;
511 : 1 : mJointAxes[2] = axis_2;
512 : 1 : mJointAxes[3] = axis_3;
513 : :
514 : 1 : validate_spatial_axis(mJointAxes[0]);
515 : 1 : validate_spatial_axis(mJointAxes[1]);
516 : 1 : validate_spatial_axis(mJointAxes[2]);
517 : 1 : validate_spatial_axis(mJointAxes[3]);
518 : 1 : }
519 : :
520 : : /** \brief Constructs a 5 DoF joint with the given motion subspaces.
521 : : *
522 : : * The motion subspaces are of the format:
523 : : * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
524 : : *
525 : : * \note So far only pure rotations or pure translations are supported.
526 : : *
527 : : * \param axis_0 Motion subspace for axis 0
528 : : * \param axis_1 Motion subspace for axis 1
529 : : * \param axis_2 Motion subspace for axis 2
530 : : * \param axis_3 Motion subspace for axis 3
531 : : * \param axis_4 Motion subspace for axis 4
532 : : */
533 : 1 : Joint(const Math::SpatialVector& axis_0, const Math::SpatialVector& axis_1, const Math::SpatialVector& axis_2, const Math::SpatialVector& axis_3,
534 : : const Math::SpatialVector& axis_4)
535 : 1 : : mJointType(JointType5DoF), mDoFCount(5), q_index(0), custom_joint_index(-1)
536 : : {
537 : 6 : mJointAxes = new Math::SpatialVector[mDoFCount];
538 : :
539 : 1 : mJointAxes[0] = axis_0;
540 : 1 : mJointAxes[1] = axis_1;
541 : 1 : mJointAxes[2] = axis_2;
542 : 1 : mJointAxes[3] = axis_3;
543 : 1 : mJointAxes[4] = axis_4;
544 : :
545 : 1 : validate_spatial_axis(mJointAxes[0]);
546 : 1 : validate_spatial_axis(mJointAxes[1]);
547 : 1 : validate_spatial_axis(mJointAxes[2]);
548 : 1 : validate_spatial_axis(mJointAxes[3]);
549 : 1 : validate_spatial_axis(mJointAxes[4]);
550 : 1 : }
551 : :
552 : : /** \brief Constructs a 6 DoF joint with the given motion subspaces.
553 : : *
554 : : * The motion subspaces are of the format:
555 : : * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
556 : : *
557 : : * \note So far only pure rotations or pure translations are supported.
558 : : *
559 : : * \param axis_0 Motion subspace for axis 0
560 : : * \param axis_1 Motion subspace for axis 1
561 : : * \param axis_2 Motion subspace for axis 2
562 : : * \param axis_3 Motion subspace for axis 3
563 : : * \param axis_4 Motion subspace for axis 4
564 : : * \param axis_5 Motion subspace for axis 5
565 : : */
566 : 92 : Joint(const Math::SpatialVector& axis_0, const Math::SpatialVector& axis_1, const Math::SpatialVector& axis_2, const Math::SpatialVector& axis_3,
567 : : const Math::SpatialVector& axis_4, const Math::SpatialVector& axis_5)
568 : 92 : : mJointType(JointType6DoF), mDoFCount(6), q_index(0), custom_joint_index(-1)
569 : : {
570 : 644 : mJointAxes = new Math::SpatialVector[mDoFCount];
571 : :
572 : 92 : mJointAxes[0] = axis_0;
573 : 92 : mJointAxes[1] = axis_1;
574 : 92 : mJointAxes[2] = axis_2;
575 : 92 : mJointAxes[3] = axis_3;
576 : 92 : mJointAxes[4] = axis_4;
577 : 92 : mJointAxes[5] = axis_5;
578 : :
579 : 92 : validate_spatial_axis(mJointAxes[0]);
580 : 92 : validate_spatial_axis(mJointAxes[1]);
581 : 92 : validate_spatial_axis(mJointAxes[2]);
582 : 92 : validate_spatial_axis(mJointAxes[3]);
583 : 92 : validate_spatial_axis(mJointAxes[4]);
584 : 92 : validate_spatial_axis(mJointAxes[5]);
585 : 92 : }
586 : :
587 : : /** \brief Checks whether we have pure rotational or translational axis.
588 : : *
589 : : * This function is mainly used to print out warnings when specifying an
590 : : * axis that might not be intended.
591 : : */
592 : 4570 : bool validate_spatial_axis(Math::SpatialVector& axis)
593 : : {
594 : 4570 : if (fabs(axis.norm() - 1.0) > 1.0e-8)
595 : : {
596 : 1 : std::cerr << "Warning: joint axis is not unit!" << std::endl;
597 : : }
598 : :
599 : 4570 : bool axis_rotational = false;
600 : 4570 : bool axis_translational = false;
601 : :
602 : 4570 : Math::Vector3d rotation(axis[0], axis[1], axis[2]);
603 : 4570 : Math::Vector3d translation(axis[3], axis[4], axis[5]);
604 : :
605 : 4570 : if (fabs(translation.norm()) < 1.0e-8)
606 : : {
607 : 3958 : axis_rotational = true;
608 : : }
609 : :
610 : 4570 : if (fabs(rotation.norm()) < 1.0e-8)
611 : : {
612 : 611 : axis_translational = true;
613 : : }
614 : :
615 : 4570 : return axis_rotational || axis_translational;
616 : : }
617 : :
618 : : /// \brief The spatial axes of the joint
619 : : Math::SpatialVector* mJointAxes;
620 : :
621 : : /// \brief Type of joint
622 : : JointType mJointType;
623 : :
624 : : /// \brief Number of degrees of freedom of the joint. Note: CustomJoints
625 : : // have here a value of 0 and their actual numbers of degrees of freedom
626 : : // can be obtained using the CustomJoint structure.
627 : : unsigned int mDoFCount;
628 : : unsigned int q_index;
629 : : unsigned int custom_joint_index;
630 : : };
631 : :
632 : : /** \brief Computes all variables for a joint model and updates the body frame as well as the body's
633 : : * center of mass frame
634 : : *
635 : : * By appropriate modification of this function all types of joints can be
636 : : * modeled. See RobotDynamics::CustomJoint
637 : : *
638 : : * \param model the rigid body model
639 : : * \param joint_id the id of the joint we are interested in. This will be used to determine the type of joint and also the
640 : : **entries of \f[ q, \dot{q} \f].
641 : : * \param q joint state variables
642 : : * \param qdot joint velocity variables
643 : : */
644 : : void jcalc(Model& model, unsigned int joint_id, const Math::VectorNd& q, const Math::VectorNd& qdot);
645 : :
646 : : Math::SpatialTransform jcalc_XJ(Model& model, unsigned int joint_id, const Math::VectorNd& q);
647 : :
648 : : void jcalc_X_lambda_S(Model& model, unsigned int joint_id, const Math::VectorNd& q);
649 : :
650 : : /**
651 : : * @struct CustomJoint
652 : : * @brief CustomJoint is a struct used to create a joint with user defined parameters. This is
653 : : * accomplished by overriding the RobotDynamics::Joint::jcalc methods that calculate each joints
654 : : * kinematic parameters
655 : : */
656 : : struct CustomJoint
657 : : {
658 : : /**
659 : : * @brief Constructor
660 : : */
661 : 95 : CustomJoint() : mDoFCount(0)
662 : : {
663 : 95 : }
664 : :
665 : : /**
666 : : * @brief Destructor
667 : : */
668 : 95 : virtual ~CustomJoint()
669 : 95 : {
670 : 95 : }
671 : :
672 : : /**
673 : : * @param model
674 : : * @param joint_id
675 : : * @param q
676 : : * @param qdot
677 : : */
678 : : virtual void jcalc(Model& model, unsigned int joint_id, const Math::VectorNd& q, const Math::VectorNd& qdot) = 0;
679 : :
680 : : virtual void jcalc_X_lambda_S(Model& model, unsigned int joint_id, const Math::VectorNd& q) = 0;
681 : :
682 : : unsigned int mDoFCount; /**< Degrees of freedom of the joint */
683 : : Math::SpatialTransform XJ; /**< Joint state transform. Transform from parent */
684 : : Math::MatrixNd S; /**< Motion subspace map */
685 : : Math::MatrixNd S_o; /**< Motion subspace map */
686 : : Math::MatrixNd U;
687 : : Math::MatrixNd Dinv;
688 : : Math::VectorNd u;
689 : : Math::VectorNd d_u;
690 : : Math::VectorNd ndof0_vec; /**< mDoFCount x 1 vector of zeros */
691 : : };
692 : : } // namespace RobotDynamics
693 : :
694 : : /* RDL_JOINT_H */
695 : : #endif // ifndef __RDL_JOINT_H__
|
