Rigid Body Dynamics Library
Joint.cc
Go to the documentation of this file.
1 /*
2  * RBDL - Rigid Body Dynamics Library
3  * Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
4  *
5  * Licensed under the zlib license. See LICENSE for more details.
6  */
7 
8 #include <iostream>
9 #include <limits>
10 #include <assert.h>
11 
12 #include "rbdl/Logging.h"
13 
14 #include "rbdl/Model.h"
15 #include "rbdl/Joint.h"
16 
17 namespace RigidBodyDynamics {
18 
19 using namespace Math;
20 
21 RBDL_DLLAPI void jcalc (
22  Model &model,
23  unsigned int joint_id,
24  const VectorNd &q,
25  const VectorNd &qdot
26  ) {
27  // exception if we calculate it for the root body
28  assert (joint_id > 0);
29 
30  if (model.mJoints[joint_id].mJointType == JointTypeRevoluteX) {
31  model.X_J[joint_id] = Xrotx (q[model.mJoints[joint_id].q_index]);
32  model.v_J[joint_id][0] = qdot[model.mJoints[joint_id].q_index];
33  } else if (model.mJoints[joint_id].mJointType == JointTypeRevoluteY) {
34  model.X_J[joint_id] = Xroty (q[model.mJoints[joint_id].q_index]);
35  model.v_J[joint_id][1] = qdot[model.mJoints[joint_id].q_index];
36  } else if (model.mJoints[joint_id].mJointType == JointTypeRevoluteZ) {
37  model.X_J[joint_id] = Xrotz (q[model.mJoints[joint_id].q_index]);
38  model.v_J[joint_id][2] = qdot[model.mJoints[joint_id].q_index];
39  } else if (model.mJoints[joint_id].mJointType == JointTypeHelical) {
40  model.X_J[joint_id] = jcalc_XJ (model, joint_id, q);
41  jcalc_X_lambda_S(model, joint_id, q);
42  double Jqd = qdot[model.mJoints[joint_id].q_index];
43  model.v_J[joint_id] = model.S[joint_id] * Jqd;
44 
45  Vector3d St = model.S[joint_id].block(0,0,3,1);
46  Vector3d c = model.X_J[joint_id].E * model.mJoints[joint_id].mJointAxes[0].block(3,0,3,1);
47  c = St.cross(c);
48  c *= -Jqd * Jqd;
49  model.c_J[joint_id] = SpatialVector(0,0,0,c[0],c[1],c[2]);
50  } else if (model.mJoints[joint_id].mDoFCount == 1 &&
51  model.mJoints[joint_id].mJointType != JointTypeCustom) {
52  model.X_J[joint_id] = jcalc_XJ (model, joint_id, q);
53  model.v_J[joint_id] =
54  model.S[joint_id] * qdot[model.mJoints[joint_id].q_index];
55  } else if (model.mJoints[joint_id].mJointType == JointTypeSpherical) {
56  model.X_J[joint_id] =
57  SpatialTransform (model.GetQuaternion (joint_id, q).toMatrix(),
58  Vector3d (0., 0., 0.));
59 
60  model.multdof3_S[joint_id](0,0) = 1.;
61  model.multdof3_S[joint_id](1,1) = 1.;
62  model.multdof3_S[joint_id](2,2) = 1.;
63 
64  Vector3d omega (qdot[model.mJoints[joint_id].q_index],
65  qdot[model.mJoints[joint_id].q_index+1],
66  qdot[model.mJoints[joint_id].q_index+2]);
67 
68  model.v_J[joint_id] = SpatialVector (
69  omega[0], omega[1], omega[2],
70  0., 0., 0.);
71  } else if (model.mJoints[joint_id].mJointType == JointTypeEulerZYX) {
72  double q0 = q[model.mJoints[joint_id].q_index];
73  double q1 = q[model.mJoints[joint_id].q_index + 1];
74  double q2 = q[model.mJoints[joint_id].q_index + 2];
75 
76  double s0 = sin (q0);
77  double c0 = cos (q0);
78  double s1 = sin (q1);
79  double c1 = cos (q1);
80  double s2 = sin (q2);
81  double c2 = cos (q2);
82 
83  model.X_J[joint_id].E = Matrix3d(
84  c0 * c1, s0 * c1, -s1,
85  c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2,
86  c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2
87  );
88 
89  model.multdof3_S[joint_id](0,0) = -s1;
90  model.multdof3_S[joint_id](0,2) = 1.;
91 
92  model.multdof3_S[joint_id](1,0) = c1 * s2;
93  model.multdof3_S[joint_id](1,1) = c2;
94 
95  model.multdof3_S[joint_id](2,0) = c1 * c2;
96  model.multdof3_S[joint_id](2,1) = - s2;
97 
98  double qdot0 = qdot[model.mJoints[joint_id].q_index];
99  double qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
100  double qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
101 
102  model.v_J[joint_id] =
103  model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2);
104 
105  model.c_J[joint_id].set(
106  -c1*qdot0*qdot1,
107  -s1*s2*qdot0*qdot1 + c1*c2*qdot0*qdot2 - s2*qdot1*qdot2,
108  -s1*c2*qdot0*qdot1 - c1*s2*qdot0*qdot2 - c2*qdot1*qdot2,
109  0.,0., 0.);
110  } else if (model.mJoints[joint_id].mJointType == JointTypeEulerXYZ) {
111  double q0 = q[model.mJoints[joint_id].q_index];
112  double q1 = q[model.mJoints[joint_id].q_index + 1];
113  double q2 = q[model.mJoints[joint_id].q_index + 2];
114 
115  double s0 = sin (q0);
116  double c0 = cos (q0);
117  double s1 = sin (q1);
118  double c1 = cos (q1);
119  double s2 = sin (q2);
120  double c2 = cos (q2);
121 
122  model.X_J[joint_id].E = Matrix3d(
123  c2 * c1, s2 * c0 + c2 * s1 * s0, s2 * s0 - c2 * s1 * c0,
124  -s2 * c1, c2 * c0 - s2 * s1 * s0, c2 * s0 + s2 * s1 * c0,
125  s1, -c1 * s0, c1 * c0
126  );
127 
128  model.multdof3_S[joint_id](0,0) = c2 * c1;
129  model.multdof3_S[joint_id](0,1) = s2;
130 
131  model.multdof3_S[joint_id](1,0) = -s2 * c1;
132  model.multdof3_S[joint_id](1,1) = c2;
133 
134  model.multdof3_S[joint_id](2,0) = s1;
135  model.multdof3_S[joint_id](2,2) = 1.;
136 
137  double qdot0 = qdot[model.mJoints[joint_id].q_index];
138  double qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
139  double qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
140 
141  model.v_J[joint_id] =
142  model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2);
143 
144  model.c_J[joint_id].set(
145  -s2*c1*qdot2*qdot0 - c2*s1*qdot1*qdot0 + c2*qdot2*qdot1,
146  -c2*c1*qdot2*qdot0 + s2*s1*qdot1*qdot0 - s2*qdot2*qdot1,
147  c1*qdot1*qdot0,
148  0., 0., 0.
149  );
150  } else if (model.mJoints[joint_id].mJointType == JointTypeEulerYXZ) {
151  double q0 = q[model.mJoints[joint_id].q_index];
152  double q1 = q[model.mJoints[joint_id].q_index + 1];
153  double q2 = q[model.mJoints[joint_id].q_index + 2];
154 
155  double s0 = sin (q0);
156  double c0 = cos (q0);
157  double s1 = sin (q1);
158  double c1 = cos (q1);
159  double s2 = sin (q2);
160  double c2 = cos (q2);
161 
162  model.X_J[joint_id].E = Matrix3d(
163  c2 * c0 + s2 * s1 * s0, s2 * c1, -c2 * s0 + s2 * s1 * c0,
164  -s2 * c0 + c2 * s1 * s0, c2 * c1, s2 * s0 + c2 * s1 * c0,
165  c1 * s0, - s1, c1 * c0);
166 
167  model.multdof3_S[joint_id](0,0) = s2 * c1;
168  model.multdof3_S[joint_id](0,1) = c2;
169 
170  model.multdof3_S[joint_id](1,0) = c2 * c1;
171  model.multdof3_S[joint_id](1,1) = -s2;
172 
173  model.multdof3_S[joint_id](2,0) = -s1;
174  model.multdof3_S[joint_id](2,2) = 1.;
175 
176  double qdot0 = qdot[model.mJoints[joint_id].q_index];
177  double qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
178  double qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
179 
180  model.v_J[joint_id] =
181  model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2);
182 
183  model.c_J[joint_id].set(
184  c2*c1*qdot2*qdot0 - s2*s1*qdot1*qdot0 - s2*qdot2*qdot1,
185  -s2*c1*qdot2*qdot0 - c2*s1*qdot1*qdot0 - c2*qdot2*qdot1,
186  -c1*qdot1*qdot0,
187  0., 0., 0.
188  );
189  } else if(model.mJoints[joint_id].mJointType == JointTypeTranslationXYZ){
190  double q0 = q[model.mJoints[joint_id].q_index];
191  double q1 = q[model.mJoints[joint_id].q_index + 1];
192  double q2 = q[model.mJoints[joint_id].q_index + 2];
193 
194  model.X_J[joint_id].E = Matrix3d::Identity();
195  model.X_J[joint_id].r = Vector3d (q0, q1, q2);
196 
197  model.multdof3_S[joint_id](3,0) = 1.;
198  model.multdof3_S[joint_id](4,1) = 1.;
199  model.multdof3_S[joint_id](5,2) = 1.;
200 
201  double qdot0 = qdot[model.mJoints[joint_id].q_index];
202  double qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
203  double qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
204 
205  model.v_J[joint_id] =
206  model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2);
207 
208  model.c_J[joint_id].set(0., 0., 0., 0., 0., 0.);
209  } else if (model.mJoints[joint_id].mJointType == JointTypeCustom) {
210  const Joint &joint = model.mJoints[joint_id];
211  CustomJoint *custom_joint =
212  model.mCustomJoints[joint.custom_joint_index];
213  custom_joint->jcalc (model, joint_id, q, qdot);
214  } else {
215  std::cerr << "Error: invalid joint type " << model.mJoints[joint_id].mJointType << " at id " << joint_id << std::endl;
216  abort();
217  }
218 
219  model.X_lambda[joint_id] = model.X_J[joint_id] * model.X_T[joint_id];
220 }
221 
223  Model &model,
224  unsigned int joint_id,
225  const Math::VectorNd &q) {
226  // exception if we calculate it for the root body
227  assert (joint_id > 0);
228 
229  if (model.mJoints[joint_id].mDoFCount == 1
230  && model.mJoints[joint_id].mJointType != JointTypeCustom) {
231  if (model.mJoints[joint_id].mJointType == JointTypeRevolute) {
232  return Xrot (q[model.mJoints[joint_id].q_index], Vector3d (
233  model.mJoints[joint_id].mJointAxes[0][0],
234  model.mJoints[joint_id].mJointAxes[0][1],
235  model.mJoints[joint_id].mJointAxes[0][2]
236  ));
237  } else if (model.mJoints[joint_id].mJointType == JointTypePrismatic) {
238  return Xtrans ( Vector3d (
239  model.mJoints[joint_id].mJointAxes[0][3]
240  * q[model.mJoints[joint_id].q_index],
241  model.mJoints[joint_id].mJointAxes[0][4]
242  * q[model.mJoints[joint_id].q_index],
243  model.mJoints[joint_id].mJointAxes[0][5]
244  * q[model.mJoints[joint_id].q_index]
245  )
246  );
247  } else if (model.mJoints[joint_id].mJointType == JointTypeHelical) {
248  SpatialTransform rot = Xrot(
249  q[model.mJoints[joint_id].q_index], Vector3d (
250  model.mJoints[joint_id].mJointAxes[0][0],
251  model.mJoints[joint_id].mJointAxes[0][1],
252  model.mJoints[joint_id].mJointAxes[0][2]
253  ));
254  SpatialTransform trans = Xtrans ( Vector3d (
255  model.mJoints[joint_id].mJointAxes[0][3]
256  * q[model.mJoints[joint_id].q_index],
257  model.mJoints[joint_id].mJointAxes[0][4]
258  * q[model.mJoints[joint_id].q_index],
259  model.mJoints[joint_id].mJointAxes[0][5]
260  * q[model.mJoints[joint_id].q_index]
261  )
262  );
263  return rot * trans;
264  }
265  }
266  std::cerr << "Error: invalid joint type: " << model.mJoints[joint_id].mJointType << std::endl;
267  abort();
268  return SpatialTransform();
269 }
270 
271 RBDL_DLLAPI void jcalc_X_lambda_S (
272  Model &model,
273  unsigned int joint_id,
274  const VectorNd &q
275  ) {
276  // exception if we calculate it for the root body
277  assert (joint_id > 0);
278 
279  if (model.mJoints[joint_id].mJointType == JointTypeRevoluteX) {
280  model.X_lambda[joint_id] =
281  Xrotx (q[model.mJoints[joint_id].q_index]) * model.X_T[joint_id];
282  model.S[joint_id] = model.mJoints[joint_id].mJointAxes[0];
283  } else if (model.mJoints[joint_id].mJointType == JointTypeRevoluteY) {
284  model.X_lambda[joint_id] =
285  Xroty (q[model.mJoints[joint_id].q_index]) * model.X_T[joint_id];
286  model.S[joint_id] = model.mJoints[joint_id].mJointAxes[0];
287  } else if (model.mJoints[joint_id].mJointType == JointTypeRevoluteZ) {
288  model.X_lambda[joint_id] =
289  Xrotz (q[model.mJoints[joint_id].q_index]) * model.X_T[joint_id];
290  model.S[joint_id] = model.mJoints[joint_id].mJointAxes[0];
291  } else if (model.mJoints[joint_id].mJointType == JointTypeHelical){
292  SpatialTransform XJ = jcalc_XJ (model, joint_id, q);
293  model.X_lambda[joint_id] = XJ * model.X_T[joint_id];
294  // Set the joint axis
295  Vector3d trans = XJ.E * model.mJoints[joint_id].mJointAxes[0].block(3,0,3,1);
296 
297  model.S[joint_id] = SpatialVector(model.mJoints[joint_id].mJointAxes[0][0],
298  model.mJoints[joint_id].mJointAxes[0][1],
299  model.mJoints[joint_id].mJointAxes[0][2],
300  trans[0], trans[1], trans[2]);
301  } else if (model.mJoints[joint_id].mDoFCount == 1
302  && model.mJoints[joint_id].mJointType != JointTypeCustom){
303  model.X_lambda[joint_id] =
304  jcalc_XJ (model, joint_id, q) * model.X_T[joint_id];
305  model.S[joint_id] = model.mJoints[joint_id].mJointAxes[0];
306  } else if (model.mJoints[joint_id].mJointType == JointTypeSpherical) {
307  model.X_lambda[joint_id] = SpatialTransform (
308  model.GetQuaternion (joint_id, q).toMatrix(),
309  Vector3d (0., 0., 0.))
310  * model.X_T[joint_id];
311 
312  model.multdof3_S[joint_id].setZero();
313 
314  model.multdof3_S[joint_id](0,0) = 1.;
315  model.multdof3_S[joint_id](1,1) = 1.;
316  model.multdof3_S[joint_id](2,2) = 1.;
317  } else if (model.mJoints[joint_id].mJointType == JointTypeEulerZYX) {
318  double q0 = q[model.mJoints[joint_id].q_index];
319  double q1 = q[model.mJoints[joint_id].q_index + 1];
320  double q2 = q[model.mJoints[joint_id].q_index + 2];
321 
322  double s0 = sin (q0);
323  double c0 = cos (q0);
324  double s1 = sin (q1);
325  double c1 = cos (q1);
326  double s2 = sin (q2);
327  double c2 = cos (q2);
328 
329  model.X_lambda[joint_id] = SpatialTransform (
330  Matrix3d(
331  c0 * c1, s0 * c1, -s1,
332  c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2,
333  c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2
334  ),
335  Vector3d (0., 0., 0.))
336  * model.X_T[joint_id];
337 
338  model.multdof3_S[joint_id].setZero();
339 
340  model.multdof3_S[joint_id](0,0) = -s1;
341  model.multdof3_S[joint_id](0,2) = 1.;
342 
343  model.multdof3_S[joint_id](1,0) = c1 * s2;
344  model.multdof3_S[joint_id](1,1) = c2;
345 
346  model.multdof3_S[joint_id](2,0) = c1 * c2;
347  model.multdof3_S[joint_id](2,1) = - s2;
348  } else if (model.mJoints[joint_id].mJointType == JointTypeEulerXYZ) {
349  double q0 = q[model.mJoints[joint_id].q_index];
350  double q1 = q[model.mJoints[joint_id].q_index + 1];
351  double q2 = q[model.mJoints[joint_id].q_index + 2];
352 
353  double s0 = sin (q0);
354  double c0 = cos (q0);
355  double s1 = sin (q1);
356  double c1 = cos (q1);
357  double s2 = sin (q2);
358  double c2 = cos (q2);
359 
360  model.X_lambda[joint_id] = SpatialTransform (
361  Matrix3d(
362  c2 * c1, s2 * c0 + c2 * s1 * s0, s2 * s0 - c2 * s1 * c0,
363  -s2 * c1, c2 * c0 - s2 * s1 * s0, c2 * s0 + s2 * s1 * c0,
364  s1, -c1 * s0, c1 * c0
365  ),
366  Vector3d (0., 0., 0.))
367  * model.X_T[joint_id];
368 
369  model.multdof3_S[joint_id].setZero();
370 
371  model.multdof3_S[joint_id](0,0) = c2 * c1;
372  model.multdof3_S[joint_id](0,1) = s2;
373 
374  model.multdof3_S[joint_id](1,0) = -s2 * c1;
375  model.multdof3_S[joint_id](1,1) = c2;
376 
377  model.multdof3_S[joint_id](2,0) = s1;
378  model.multdof3_S[joint_id](2,2) = 1.;
379  } else if (model.mJoints[joint_id].mJointType == JointTypeEulerYXZ ) {
380  double q0 = q[model.mJoints[joint_id].q_index];
381  double q1 = q[model.mJoints[joint_id].q_index + 1];
382  double q2 = q[model.mJoints[joint_id].q_index + 2];
383 
384  double s0 = sin (q0);
385  double c0 = cos (q0);
386  double s1 = sin (q1);
387  double c1 = cos (q1);
388  double s2 = sin (q2);
389  double c2 = cos (q2);
390 
391  model.X_lambda[joint_id] = SpatialTransform (
392  Matrix3d(
393  c2 * c0 + s2 * s1 * s0, s2 * c1, -c2 * s0 + s2 * s1 * c0,
394  -s2 * c0 + c2 * s1 * s0, c2 * c1, s2 * s0 + c2 * s1 * c0,
395  c1 * s0, - s1, c1 * c0
396  ),
397  Vector3d (0., 0., 0.))
398  * model.X_T[joint_id];
399 
400  model.multdof3_S[joint_id].setZero();
401 
402  model.multdof3_S[joint_id](0,0) = s2 * c1;
403  model.multdof3_S[joint_id](0,1) = c2;
404 
405  model.multdof3_S[joint_id](1,0) = c2 * c1;
406  model.multdof3_S[joint_id](1,1) = -s2;
407 
408  model.multdof3_S[joint_id](2,0) = -s1;
409  model.multdof3_S[joint_id](2,2) = 1.;
410  } else if (model.mJoints[joint_id].mJointType == JointTypeTranslationXYZ) {
411  double q0 = q[model.mJoints[joint_id].q_index];
412  double q1 = q[model.mJoints[joint_id].q_index + 1];
413  double q2 = q[model.mJoints[joint_id].q_index + 2];
414 
415  model.X_lambda[joint_id] = SpatialTransform (
416  Matrix3d::Identity (3,3),
417  Vector3d (q0, q1, q2))
418  * model.X_T[joint_id];
419 
420  model.multdof3_S[joint_id].setZero();
421 
422  model.multdof3_S[joint_id](3,0) = 1.;
423  model.multdof3_S[joint_id](4,1) = 1.;
424  model.multdof3_S[joint_id](5,2) = 1.;
425  } else if (model.mJoints[joint_id].mJointType == JointTypeCustom) {
426  const Joint &joint = model.mJoints[joint_id];
427  CustomJoint *custom_joint
428  = model.mCustomJoints[joint.custom_joint_index];
429 
430  custom_joint->jcalc_X_lambda_S (model, joint_id, q);
431  } else {
432  std::cerr << "Error: invalid joint type!" << std::endl;
433  abort();
434  }
435 }
436 }
Compact representation of spatial transformations.
std::vector< Math::SpatialVector > v_J
Definition: Model.h:181
RBDL_DLLAPI void jcalc_X_lambda_S(Model &model, unsigned int joint_id, const VectorNd &q)
Definition: Joint.cc:271
Contains all information about the rigid body model.
Definition: Model.h:121
SpatialVector_t SpatialVector
Definition: rbdl_math.h:59
std::vector< CustomJoint * > mCustomJoints
Definition: Model.h:202
Namespace for all structures of the RigidBodyDynamics library.
Definition: Constraints.cc:23
std::vector< Math::SpatialTransform > X_lambda
Transformation from the parent body to the current body .
Definition: Model.h:236
SpatialTransform Xtrans(const Vector3d &r)
SpatialTransform Xrotx(const double &xrot)
3 DoF joint that uses Euler ZYX convention (faster than emulated multi DoF joints).
Definition: Joint.h:187
RBDL_DLLAPI Math::SpatialTransform jcalc_XJ(Model &model, unsigned int joint_id, const Math::VectorNd &q)
Definition: Joint.cc:222
virtual void jcalc(Model &model, unsigned int joint_id, const Math::VectorNd &q, const Math::VectorNd &qdot)=0
Math::Quaternion GetQuaternion(unsigned int i, const Math::VectorNd &Q) const
Definition: Model.h:461
SpatialTransform Xroty(const double &yrot)
3 DoF joint that uses Euler XYZ convention (faster than emulated multi DoF joints).
Definition: Joint.h:188
SpatialTransform Xrotz(const double &zrot)
RBDL_DLLAPI void jcalc(Model &model, unsigned int joint_id, const VectorNd &q, const VectorNd &qdot)
Computes all variables for a joint model.
Definition: Joint.cc:21
std::vector< Math::SpatialTransform > X_J
Definition: Model.h:180
std::vector< Math::SpatialVector > S
The joint axis for joint i.
Definition: Model.h:177
std::vector< Math::SpatialVector > c_J
Definition: Model.h:182
3 DoF joint using Quaternions for joint positional variables and angular velocity for joint velocity ...
Definition: Joint.h:186
Describes a joint relative to the predecessor body.
Definition: Joint.h:209
3 DoF joint that uses Euler YXZ convention (faster than emulated multi DoF joints).
Definition: Joint.h:189
virtual void jcalc_X_lambda_S(Model &model, unsigned int joint_id, const Math::VectorNd &q)=0
std::vector< Math::Matrix63 > multdof3_S
Motion subspace for joints with 3 degrees of freedom.
Definition: Model.h:196
SpatialTransform Xrot(double angle_rad, const Vector3d &axis)
std::vector< Joint > mJoints
All joints.
Definition: Model.h:175
User defined joints of varying size.
Definition: Joint.h:200
std::vector< Math::SpatialTransform > X_T
Transformations from the parent body to the frame of the joint.
Definition: Model.h:188