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/rbdl_config.h"
13#include "rbdl/Logging.h"
14
15#include "rbdl/Model.h"
16#include "rbdl/Joint.h"
17#include "rbdl/rbdl_errors.h"
18#include "rbdl/rbdl_math.h"
19
20
21#ifdef RBDL_USE_CASADI_MATH
22
23inline void sincosp(RBDLCasadiMath::MX_Xd_scalar x, RBDLCasadiMath::MX_Xd_scalar* sinp, RBDLCasadiMath::MX_Xd_scalar* cosp) {
24 *sinp = std::sin(x);
25 *cosp = std::cos(x);
26}
27
28#else
29#include <cmath>
30
31#ifdef __APPLE__
32inline void sincosp(RigidBodyDynamics::Math::Scalar x, RigidBodyDynamics::Math::Scalar* sinp, RigidBodyDynamics::Math::Scalar* cosp) {
33 return __sincos(x, sinp, cosp);
34}
35#else
36
37inline void sincosp(RigidBodyDynamics::Math::Scalar x, RigidBodyDynamics::Math::Scalar* sinp, RigidBodyDynamics::Math::Scalar* cosp) {
38 *sinp = sin(x);
39 *cosp = cos(x);
40}
41#endif
42
43#endif
44
45namespace RigidBodyDynamics {
46
47using namespace Math;
48
49RBDL_DLLAPI void jcalc (
50 Model &model,
51 unsigned int joint_id,
52 const VectorNd &q,
53 const VectorNd &qdot
54 ) {
55 // exception if we calculate it for the root body
56 assert (joint_id > 0);
57
58 if (model.mJoints[joint_id].mJointType == JointTypeRevoluteX) {
59 Scalar s, c;
60 sincosp (q[model.mJoints[joint_id].q_index], &s, &c);
61
62 model.X_lambda[joint_id].E = Matrix3d (
63 model.X_T[joint_id].E(0, 0),
64 model.X_T[joint_id].E(0, 1),
65 model.X_T[joint_id].E(0, 2),
66
67 c * model.X_T[joint_id].E(1, 0) + s * model.X_T[joint_id].E(2, 0),
68 c * model.X_T[joint_id].E(1, 1) + s * model.X_T[joint_id].E(2, 1),
69 c * model.X_T[joint_id].E(1, 2) + s * model.X_T[joint_id].E(2, 2),
70
71 -s * model.X_T[joint_id].E(1, 0) + c * model.X_T[joint_id].E(2, 0),
72 -s * model.X_T[joint_id].E(1, 1) + c * model.X_T[joint_id].E(2, 1),
73 -s * model.X_T[joint_id].E(1, 2) + c * model.X_T[joint_id].E(2, 2));
74
75 model.X_lambda[joint_id].r = model.X_T[joint_id].r;
76
77 model.v_J[joint_id][0] = qdot[model.mJoints[joint_id].q_index];
78 } else if (model.mJoints[joint_id].mJointType == JointTypeRevoluteY) {
79 Scalar s, c;
80 sincosp (q[model.mJoints[joint_id].q_index], &s, &c);
81
82 model.X_lambda[joint_id].E = Matrix3d (
83 c * model.X_T[joint_id].E(0, 0) + -s * model.X_T[joint_id].E(2, 0),
84 c * model.X_T[joint_id].E(0, 1) + -s * model.X_T[joint_id].E(2, 1),
85 c * model.X_T[joint_id].E(0, 2) + -s * model.X_T[joint_id].E(2, 2),
86
87 model.X_T[joint_id].E(1, 0),
88 model.X_T[joint_id].E(1, 1),
89 model.X_T[joint_id].E(1, 2),
90
91 s * model.X_T[joint_id].E(0, 0) + c * model.X_T[joint_id].E(2, 0),
92 s * model.X_T[joint_id].E(0, 1) + c * model.X_T[joint_id].E(2, 1),
93 s * model.X_T[joint_id].E(0, 2) + c * model.X_T[joint_id].E(2, 2));
94
95 model.X_lambda[joint_id].r = model.X_T[joint_id].r;
96
97 model.v_J[joint_id][1] = qdot[model.mJoints[joint_id].q_index];
98 } else if (model.mJoints[joint_id].mJointType == JointTypeRevoluteZ) {
99 Scalar s, c;
100 sincosp (q[model.mJoints[joint_id].q_index], &s, &c);
101
102 model.X_lambda[joint_id].E = Matrix3d (
103 c * model.X_T[joint_id].E(0, 0) + s * model.X_T[joint_id].E(1, 0),
104 c * model.X_T[joint_id].E(0, 1) + s * model.X_T[joint_id].E(1, 1),
105 c * model.X_T[joint_id].E(0, 2) + s * model.X_T[joint_id].E(1, 2),
106
107 -s * model.X_T[joint_id].E(0, 0) + c * model.X_T[joint_id].E(1, 0),
108 -s * model.X_T[joint_id].E(0, 1) + c * model.X_T[joint_id].E(1, 1),
109 -s * model.X_T[joint_id].E(0, 2) + c * model.X_T[joint_id].E(1, 2),
110
111 model.X_T[joint_id].E(2, 0),
112 model.X_T[joint_id].E(2, 1),
113 model.X_T[joint_id].E(2, 2));
114
115 model.X_lambda[joint_id].r = model.X_T[joint_id].r;
116
117 model.v_J[joint_id][2] = qdot[model.mJoints[joint_id].q_index];
118 } else if (model.mJoints[joint_id].mJointType == JointTypeHelical) {
119 SpatialTransform X_J = jcalc_XJ (model, joint_id, q);
120 jcalc_X_lambda_S(model, joint_id, q);
121 Scalar Jqd = qdot[model.mJoints[joint_id].q_index];
122 model.v_J[joint_id] = model.S[joint_id] * Jqd;
123
124 Vector3d St = model.S[joint_id].block(0,0,3,1);
125 Vector3d c = X_J.E * model.mJoints[joint_id].mJointAxes[0].block(3,0,3,1);
126 c = St.cross(c);
127 c *= -Jqd * Jqd;
128 model.c_J[joint_id] = SpatialVector(0,0,0,c[0],c[1],c[2]);
129 model.X_lambda[joint_id] = X_J * model.X_T[joint_id];
130 } else if (model.mJoints[joint_id].mDoFCount == 1 &&
131 model.mJoints[joint_id].mJointType != JointTypeCustom) {
132 SpatialTransform X_J = jcalc_XJ (model, joint_id, q);
133 model.v_J[joint_id] =
134 model.S[joint_id] * qdot[model.mJoints[joint_id].q_index];
135 model.X_lambda[joint_id] = X_J * model.X_T[joint_id];
136 } else if (model.mJoints[joint_id].mJointType == JointTypeSpherical) {
137 SpatialTransform X_J = SpatialTransform (model.GetQuaternion (joint_id, q).toMatrix(),
138 Vector3d (0., 0., 0.));
139
140 model.multdof3_S[joint_id](0,0) = 1.;
141 model.multdof3_S[joint_id](1,1) = 1.;
142 model.multdof3_S[joint_id](2,2) = 1.;
143
144 Vector3d omega (qdot[model.mJoints[joint_id].q_index],
145 qdot[model.mJoints[joint_id].q_index+1],
146 qdot[model.mJoints[joint_id].q_index+2]);
147
148 model.v_J[joint_id] = SpatialVector (
149 omega[0], omega[1], omega[2],
150 0., 0., 0.);
151 model.X_lambda[joint_id] = X_J * model.X_T[joint_id];
152 } else if (model.mJoints[joint_id].mJointType == JointTypeEulerZYX) {
153 Scalar q0 = q[model.mJoints[joint_id].q_index];
154 Scalar q1 = q[model.mJoints[joint_id].q_index + 1];
155 Scalar q2 = q[model.mJoints[joint_id].q_index + 2];
156
157 Scalar s0 = sin (q0);
158 Scalar c0 = cos (q0);
159 Scalar s1 = sin (q1);
160 Scalar c1 = cos (q1);
161 Scalar s2 = sin (q2);
162 Scalar c2 = cos (q2);
163
164 SpatialTransform X_J (Matrix3d(
165 c0 * c1, s0 * c1, -s1,
166 c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2,
167 c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2
168 ),
169 Vector3d::Zero());
170
171 model.multdof3_S[joint_id](0,0) = -s1;
172 model.multdof3_S[joint_id](0,2) = 1.;
173
174 model.multdof3_S[joint_id](1,0) = c1 * s2;
175 model.multdof3_S[joint_id](1,1) = c2;
176
177 model.multdof3_S[joint_id](2,0) = c1 * c2;
178 model.multdof3_S[joint_id](2,1) = - s2;
179
180 Scalar qdot0 = qdot[model.mJoints[joint_id].q_index];
181 Scalar qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
182 Scalar qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
183
184 model.v_J[joint_id] =
185 model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2);
186
187 model.c_J[joint_id].set(
188 -c1*qdot0*qdot1,
189 -s1*s2*qdot0*qdot1 + c1*c2*qdot0*qdot2 - s2*qdot1*qdot2,
190 -s1*c2*qdot0*qdot1 - c1*s2*qdot0*qdot2 - c2*qdot1*qdot2,
191 0.,0., 0.);
192 model.X_lambda[joint_id] = X_J * model.X_T[joint_id];
193 } else if (model.mJoints[joint_id].mJointType == JointTypeEulerXYZ) {
194 Scalar q0 = q[model.mJoints[joint_id].q_index];
195 Scalar q1 = q[model.mJoints[joint_id].q_index + 1];
196 Scalar q2 = q[model.mJoints[joint_id].q_index + 2];
197
198 Scalar s0 = sin (q0);
199 Scalar c0 = cos (q0);
200 Scalar s1 = sin (q1);
201 Scalar c1 = cos (q1);
202 Scalar s2 = sin (q2);
203 Scalar c2 = cos (q2);
204
205 SpatialTransform X_J (Matrix3d(
206 c2 * c1, s2 * c0 + c2 * s1 * s0, s2 * s0 - c2 * s1 * c0,
207 -s2 * c1, c2 * c0 - s2 * s1 * s0, c2 * s0 + s2 * s1 * c0,
208 s1, -c1 * s0, c1 * c0
209 ),
210 Vector3d::Zero());
211
212 model.multdof3_S[joint_id](0,0) = c2 * c1;
213 model.multdof3_S[joint_id](0,1) = s2;
214
215 model.multdof3_S[joint_id](1,0) = -s2 * c1;
216 model.multdof3_S[joint_id](1,1) = c2;
217
218 model.multdof3_S[joint_id](2,0) = s1;
219 model.multdof3_S[joint_id](2,2) = 1.;
220
221 Scalar qdot0 = qdot[model.mJoints[joint_id].q_index];
222 Scalar qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
223 Scalar qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
224
225 model.v_J[joint_id] =
226 model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2);
227
228 model.c_J[joint_id].set(
229 -s2*c1*qdot2*qdot0 - c2*s1*qdot1*qdot0 + c2*qdot2*qdot1,
230 -c2*c1*qdot2*qdot0 + s2*s1*qdot1*qdot0 - s2*qdot2*qdot1,
231 c1*qdot1*qdot0,
232 0., 0., 0.
233 );
234 model.X_lambda[joint_id] = X_J * model.X_T[joint_id];
235 } else if (model.mJoints[joint_id].mJointType == JointTypeEulerYXZ) {
236 Scalar q0 = q[model.mJoints[joint_id].q_index];
237 Scalar q1 = q[model.mJoints[joint_id].q_index + 1];
238 Scalar q2 = q[model.mJoints[joint_id].q_index + 2];
239
240 Scalar s0 = sin (q0);
241 Scalar c0 = cos (q0);
242 Scalar s1 = sin (q1);
243 Scalar c1 = cos (q1);
244 Scalar s2 = sin (q2);
245 Scalar c2 = cos (q2);
246
247 SpatialTransform X_J (Matrix3d(
248 c2 * c0 + s2 * s1 * s0, s2 * c1, -c2 * s0 + s2 * s1 * c0,
249 -s2 * c0 + c2 * s1 * s0, c2 * c1, s2 * s0 + c2 * s1 * c0,
250 c1 * s0, - s1, c1 * c0),
251 Vector3d::Zero());
252
253 model.multdof3_S[joint_id](0,0) = s2 * c1;
254 model.multdof3_S[joint_id](0,1) = c2;
255
256 model.multdof3_S[joint_id](1,0) = c2 * c1;
257 model.multdof3_S[joint_id](1,1) = -s2;
258
259 model.multdof3_S[joint_id](2,0) = -s1;
260 model.multdof3_S[joint_id](2,2) = 1.;
261
262 Scalar qdot0 = qdot[model.mJoints[joint_id].q_index];
263 Scalar qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
264 Scalar qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
265
266 model.v_J[joint_id] =
267 model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2);
268
269 model.c_J[joint_id].set(
270 c2*c1*qdot2*qdot0 - s2*s1*qdot1*qdot0 - s2*qdot2*qdot1,
271 -s2*c1*qdot2*qdot0 - c2*s1*qdot1*qdot0 - c2*qdot2*qdot1,
272 -c1*qdot1*qdot0,
273 0., 0., 0.
274 );
275 model.X_lambda[joint_id] = X_J * model.X_T[joint_id];
276 } else if (model.mJoints[joint_id].mJointType == JointTypeEulerZXY) {
277 Scalar q0 = q[model.mJoints[joint_id].q_index];
278 Scalar q1 = q[model.mJoints[joint_id].q_index + 1];
279 Scalar q2 = q[model.mJoints[joint_id].q_index + 2];
280
281 Scalar s0 = sin (q0);
282 Scalar c0 = cos (q0);
283 Scalar s1 = sin (q1);
284 Scalar c1 = cos (q1);
285 Scalar s2 = sin (q2);
286 Scalar c2 = cos (q2);
287
288 model.X_lambda[joint_id] = SpatialTransform (
289 Matrix3d(
290 -s0 * s1 * s2 + c0 * c2, s0 * c2 + s1 * s2 * c0, -s2 * c1,
291 -s0 * c1, c0 * c1, s1,
292 s0 * s1 * c2 + s2 * c0, s0 * s2 - s1 * c0 * c2, c1 * c2
293 ),
294 Vector3d::Zero())
295 * model.X_T[joint_id];
296
297 model.multdof3_S[joint_id](0,0) = -s2 * c1;
298 model.multdof3_S[joint_id](0,1) = c2;
299
300 model.multdof3_S[joint_id](1,0) = s1;
301 model.multdof3_S[joint_id](1,2) = 1;
302
303 model.multdof3_S[joint_id](2,0) = c1 * c2;
304 model.multdof3_S[joint_id](2,1) = s2;
305
306 Scalar qdot0 = qdot[model.mJoints[joint_id].q_index];
307 Scalar qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
308 Scalar qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
309
310 model.v_J[joint_id] =
311 model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2);
312
313 model.c_J[joint_id].set(
314 (-c1 * c2 * qdot2 + s1 * s2 * qdot1) * qdot0 - s2 * qdot1 * qdot2,
315 c1 * qdot1 * qdot0,
316 (-s1 * c2 * qdot1 - c1 * s2 * qdot2) * qdot0 + c2 * qdot2 * qdot1,
317 0., 0., 0.
318 );
319 } else if(model.mJoints[joint_id].mJointType == JointTypeTranslationXYZ){
320 Scalar q0 = q[model.mJoints[joint_id].q_index];
321 Scalar q1 = q[model.mJoints[joint_id].q_index + 1];
322 Scalar q2 = q[model.mJoints[joint_id].q_index + 2];
323
324 model.multdof3_S[joint_id](3,0) = 1.;
325 model.multdof3_S[joint_id](4,1) = 1.;
326 model.multdof3_S[joint_id](5,2) = 1.;
327
328 Scalar qdot0 = qdot[model.mJoints[joint_id].q_index];
329 Scalar qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
330 Scalar qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
331
332 model.v_J[joint_id] =
333 model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2);
334
335 model.c_J[joint_id].set(0., 0., 0., 0., 0., 0.);
336 model.X_lambda[joint_id].E = model.X_T[joint_id].E;
337 model.X_lambda[joint_id].r = model.X_T[joint_id].r + model.X_T[joint_id].E.transpose() * Vector3d (q0, q1, q2);
338 } else if (model.mJoints[joint_id].mJointType == JointTypeCustom) {
339 const Joint &joint = model.mJoints[joint_id];
340 CustomJoint *custom_joint =
341 model.mCustomJoints[joint.custom_joint_index];
342 custom_joint->jcalc (model, joint_id, q, qdot);
343 } else {
344 std::ostringstream errormsg;
345 errormsg << "Error: invalid joint type " << model.mJoints[joint_id].mJointType << " at id " << joint_id << std::endl;
346 throw Errors::RBDLError(errormsg.str());
347 }
348}
349
350RBDL_DLLAPI Math::SpatialTransform jcalc_XJ (
351 Model &model,
352 unsigned int joint_id,
353 const Math::VectorNd &q) {
354 // exception if we calculate it for the root body
355 assert (joint_id > 0);
356
357 if (model.mJoints[joint_id].mDoFCount == 1
358 && model.mJoints[joint_id].mJointType != JointTypeCustom) {
359 if (model.mJoints[joint_id].mJointType == JointTypeRevolute) {
360 return Xrot (q[model.mJoints[joint_id].q_index], Vector3d (
361 model.mJoints[joint_id].mJointAxes[0][0],
362 model.mJoints[joint_id].mJointAxes[0][1],
363 model.mJoints[joint_id].mJointAxes[0][2]
364 ));
365 } else if (model.mJoints[joint_id].mJointType == JointTypePrismatic) {
366 return Xtrans ( Vector3d (
367 model.mJoints[joint_id].mJointAxes[0][3]
368 * q[model.mJoints[joint_id].q_index],
369 model.mJoints[joint_id].mJointAxes[0][4]
370 * q[model.mJoints[joint_id].q_index],
371 model.mJoints[joint_id].mJointAxes[0][5]
372 * q[model.mJoints[joint_id].q_index]
373 )
374 );
375 } else if (model.mJoints[joint_id].mJointType == JointTypeHelical) {
376 SpatialTransform rot = Xrot(
377 q[model.mJoints[joint_id].q_index], Vector3d (
378 model.mJoints[joint_id].mJointAxes[0][0],
379 model.mJoints[joint_id].mJointAxes[0][1],
380 model.mJoints[joint_id].mJointAxes[0][2]
381 ));
382 SpatialTransform trans = Xtrans ( Vector3d (
383 model.mJoints[joint_id].mJointAxes[0][3]
384 * q[model.mJoints[joint_id].q_index],
385 model.mJoints[joint_id].mJointAxes[0][4]
386 * q[model.mJoints[joint_id].q_index],
387 model.mJoints[joint_id].mJointAxes[0][5]
388 * q[model.mJoints[joint_id].q_index]
389 )
390 );
391 return rot * trans;
392 }
393 }
394
395 std::ostringstream errormsg;
396 errormsg << "Error: invalid joint type: " << model.mJoints[joint_id].mJointType << std::endl;
397 throw Errors::RBDLError(errormsg.str());
398
399 //never reaches this
400 return SpatialTransform();
401}
402
403RBDL_DLLAPI void jcalc_X_lambda_S (
404 Model &model,
405 unsigned int joint_id,
406 const VectorNd &q
407 ) {
408 // exception if we calculate it for the root body
409 assert (joint_id > 0);
410
411 if (model.mJoints[joint_id].mJointType == JointTypeRevoluteX) {
412 Scalar s, c;
413 sincosp (q[model.mJoints[joint_id].q_index], &s, &c);
414
415 model.X_lambda[joint_id].E = Matrix3d (
416 model.X_T[joint_id].E(0, 0),
417 model.X_T[joint_id].E(0, 1),
418 model.X_T[joint_id].E(0, 2),
419
420 c * model.X_T[joint_id].E(1, 0) + s * model.X_T[joint_id].E(2, 0),
421 c * model.X_T[joint_id].E(1, 1) + s * model.X_T[joint_id].E(2, 1),
422 c * model.X_T[joint_id].E(1, 2) + s * model.X_T[joint_id].E(2, 2),
423
424 -s * model.X_T[joint_id].E(1, 0) + c * model.X_T[joint_id].E(2, 0),
425 -s * model.X_T[joint_id].E(1, 1) + c * model.X_T[joint_id].E(2, 1),
426 -s * model.X_T[joint_id].E(1, 2) + c * model.X_T[joint_id].E(2, 2));
427
428 model.X_lambda[joint_id].r = model.X_T[joint_id].r;
429
430 model.S[joint_id][0] = 1.0;
431 } else if (model.mJoints[joint_id].mJointType == JointTypeRevoluteY) {
432 Scalar s, c;
433 sincosp (q[model.mJoints[joint_id].q_index], &s, &c);
434
435 model.X_lambda[joint_id].E = Matrix3d (
436 c * model.X_T[joint_id].E(0, 0) + -s * model.X_T[joint_id].E(2, 0),
437 c * model.X_T[joint_id].E(0, 1) + -s * model.X_T[joint_id].E(2, 1),
438 c * model.X_T[joint_id].E(0, 2) + -s * model.X_T[joint_id].E(2, 2),
439
440 model.X_T[joint_id].E(1, 0),
441 model.X_T[joint_id].E(1, 1),
442 model.X_T[joint_id].E(1, 2),
443
444 s * model.X_T[joint_id].E(0, 0) + c * model.X_T[joint_id].E(2, 0),
445 s * model.X_T[joint_id].E(0, 1) + c * model.X_T[joint_id].E(2, 1),
446 s * model.X_T[joint_id].E(0, 2) + c * model.X_T[joint_id].E(2, 2));
447
448 model.X_lambda[joint_id].r = model.X_T[joint_id].r;
449
450 model.S[joint_id][1] = 1.;
451 } else if (model.mJoints[joint_id].mJointType == JointTypeRevoluteZ) {
452 Scalar s, c;
453 sincosp (q[model.mJoints[joint_id].q_index], &s, &c);
454
455 model.X_lambda[joint_id].E = Matrix3d (
456 c * model.X_T[joint_id].E(0, 0) + s * model.X_T[joint_id].E(1, 0),
457 c * model.X_T[joint_id].E(0, 1) + s * model.X_T[joint_id].E(1, 1),
458 c * model.X_T[joint_id].E(0, 2) + s * model.X_T[joint_id].E(1, 2),
459
460 -s * model.X_T[joint_id].E(0, 0) + c * model.X_T[joint_id].E(1, 0),
461 -s * model.X_T[joint_id].E(0, 1) + c * model.X_T[joint_id].E(1, 1),
462 -s * model.X_T[joint_id].E(0, 2) + c * model.X_T[joint_id].E(1, 2),
463
464 model.X_T[joint_id].E(2, 0),
465 model.X_T[joint_id].E(2, 1),
466 model.X_T[joint_id].E(2, 2));
467
468 model.X_lambda[joint_id].r = model.X_T[joint_id].r;
469
470 model.S[joint_id][2] = 1.;
471 } else if (model.mJoints[joint_id].mJointType == JointTypeHelical){
472 SpatialTransform XJ = jcalc_XJ (model, joint_id, q);
473 model.X_lambda[joint_id] = XJ * model.X_T[joint_id];
474 // Set the joint axis
475 Vector3d trans = XJ.E * model.mJoints[joint_id].mJointAxes[0].block(3,0,3,1);
476
477 model.S[joint_id] = SpatialVector(model.mJoints[joint_id].mJointAxes[0][0],
478 model.mJoints[joint_id].mJointAxes[0][1],
479 model.mJoints[joint_id].mJointAxes[0][2],
480 trans[0], trans[1], trans[2]);
481 } else if (model.mJoints[joint_id].mDoFCount == 1
482 && model.mJoints[joint_id].mJointType != JointTypeCustom){
483 model.X_lambda[joint_id] =
484 jcalc_XJ (model, joint_id, q) * model.X_T[joint_id];
485 model.S[joint_id] = model.mJoints[joint_id].mJointAxes[0];
486 } else if (model.mJoints[joint_id].mJointType == JointTypeSpherical) {
487 model.X_lambda[joint_id] = SpatialTransform (
488 model.GetQuaternion (joint_id, q).toMatrix(),
489 Vector3d (0., 0., 0.))
490 * model.X_T[joint_id];
491
492 model.multdof3_S[joint_id](0,0) = 1.;
493 model.multdof3_S[joint_id](1,1) = 1.;
494 model.multdof3_S[joint_id](2,2) = 1.;
495 } else if (model.mJoints[joint_id].mJointType == JointTypeEulerZYX) {
496 Scalar q0 = q[model.mJoints[joint_id].q_index];
497 Scalar q1 = q[model.mJoints[joint_id].q_index + 1];
498 Scalar q2 = q[model.mJoints[joint_id].q_index + 2];
499
500 Scalar s0 = sin (q0);
501 Scalar c0 = cos (q0);
502 Scalar s1 = sin (q1);
503 Scalar c1 = cos (q1);
504 Scalar s2 = sin (q2);
505 Scalar c2 = cos (q2);
506
507 model.X_lambda[joint_id] = SpatialTransform (
508 Matrix3d(
509 c0 * c1, s0 * c1, -s1,
510 c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2,
511 c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2
512 ),
513 Vector3d (0., 0., 0.))
514 * model.X_T[joint_id];
515
516 model.multdof3_S[joint_id](0,0) = -s1;
517 model.multdof3_S[joint_id](0,2) = 1.;
518
519 model.multdof3_S[joint_id](1,0) = c1 * s2;
520 model.multdof3_S[joint_id](1,1) = c2;
521
522 model.multdof3_S[joint_id](2,0) = c1 * c2;
523 model.multdof3_S[joint_id](2,1) = - s2;
524 } else if (model.mJoints[joint_id].mJointType == JointTypeEulerXYZ) {
525 Scalar q0 = q[model.mJoints[joint_id].q_index];
526 Scalar q1 = q[model.mJoints[joint_id].q_index + 1];
527 Scalar q2 = q[model.mJoints[joint_id].q_index + 2];
528
529 Scalar s0 = sin (q0);
530 Scalar c0 = cos (q0);
531 Scalar s1 = sin (q1);
532 Scalar c1 = cos (q1);
533 Scalar s2 = sin (q2);
534 Scalar c2 = cos (q2);
535
536 model.X_lambda[joint_id] = SpatialTransform (
537 Matrix3d(
538 c2 * c1, s2 * c0 + c2 * s1 * s0, s2 * s0 - c2 * s1 * c0,
539 -s2 * c1, c2 * c0 - s2 * s1 * s0, c2 * s0 + s2 * s1 * c0,
540 s1, -c1 * s0, c1 * c0
541 ),
542 Vector3d (0., 0., 0.))
543 * model.X_T[joint_id];
544
545 model.multdof3_S[joint_id](0,0) = c2 * c1;
546 model.multdof3_S[joint_id](0,1) = s2;
547
548 model.multdof3_S[joint_id](1,0) = -s2 * c1;
549 model.multdof3_S[joint_id](1,1) = c2;
550
551 model.multdof3_S[joint_id](2,0) = s1;
552 model.multdof3_S[joint_id](2,2) = 1.;
553 } else if (model.mJoints[joint_id].mJointType == JointTypeEulerYXZ ) {
554 Scalar q0 = q[model.mJoints[joint_id].q_index];
555 Scalar q1 = q[model.mJoints[joint_id].q_index + 1];
556 Scalar q2 = q[model.mJoints[joint_id].q_index + 2];
557
558 Scalar s0 = sin (q0);
559 Scalar c0 = cos (q0);
560 Scalar s1 = sin (q1);
561 Scalar c1 = cos (q1);
562 Scalar s2 = sin (q2);
563 Scalar c2 = cos (q2);
564
565 model.X_lambda[joint_id] = SpatialTransform (
566 Matrix3d(
567 c2 * c0 + s2 * s1 * s0, s2 * c1, -c2 * s0 + s2 * s1 * c0,
568 -s2 * c0 + c2 * s1 * s0, c2 * c1, s2 * s0 + c2 * s1 * c0,
569 c1 * s0, - s1, c1 * c0
570 ),
571 Vector3d (0., 0., 0.))
572 * model.X_T[joint_id];
573
574 model.multdof3_S[joint_id](0,0) = s2 * c1;
575 model.multdof3_S[joint_id](0,1) = c2;
576
577 model.multdof3_S[joint_id](1,0) = c2 * c1;
578 model.multdof3_S[joint_id](1,1) = -s2;
579
580 model.multdof3_S[joint_id](2,0) = -s1;
581 model.multdof3_S[joint_id](2,2) = 1.;
582 } else if (model.mJoints[joint_id].mJointType == JointTypeEulerZXY ) {
583 Scalar q0 = q[model.mJoints[joint_id].q_index];
584 Scalar q1 = q[model.mJoints[joint_id].q_index + 1];
585 Scalar q2 = q[model.mJoints[joint_id].q_index + 2];
586
587 Scalar s0 = sin (q0);
588 Scalar c0 = cos (q0);
589 Scalar s1 = sin (q1);
590 Scalar c1 = cos (q1);
591 Scalar s2 = sin (q2);
592 Scalar c2 = cos (q2);
593
594 model.X_lambda[joint_id] = SpatialTransform (
595 Matrix3d(
596 -s0 * s1 * s2 + c0 * c2, s0 * c2 + s1 * s2 * c0, -s2 * c1,
597 -s0 * c1, c0 * c1, s1,
598 s0 * s1 * c2 + s2 * c0, s0 * s2 - s1 * c0 * c2, c1 * c2
599 ),
600 Vector3d::Zero())
601 * model.X_T[joint_id];
602
603 model.multdof3_S[joint_id](0,0) = -s2 * c1;
604 model.multdof3_S[joint_id](0,1) = c2;
605
606 model.multdof3_S[joint_id](1,0) = s1;
607 model.multdof3_S[joint_id](1,2) = 1;
608
609 model.multdof3_S[joint_id](2,0) = c1 * c2;
610 model.multdof3_S[joint_id](2,1) = s2;
611 } else if (model.mJoints[joint_id].mJointType == JointTypeTranslationXYZ) {
612 Scalar q0 = q[model.mJoints[joint_id].q_index];
613 Scalar q1 = q[model.mJoints[joint_id].q_index + 1];
614 Scalar q2 = q[model.mJoints[joint_id].q_index + 2];
615
616 model.X_lambda[joint_id] = SpatialTransform (
617 Matrix3d::Identity (),
618 Vector3d (q0, q1, q2))
619 * model.X_T[joint_id];
620
621 model.multdof3_S[joint_id](3,0) = 1.;
622 model.multdof3_S[joint_id](4,1) = 1.;
623 model.multdof3_S[joint_id](5,2) = 1.;
624 } else if (model.mJoints[joint_id].mJointType == JointTypeCustom) {
625 const Joint &joint = model.mJoints[joint_id];
626 CustomJoint *custom_joint
627 = model.mCustomJoints[joint.custom_joint_index];
628
629 custom_joint->jcalc_X_lambda_S (model, joint_id, q);
630 } else {
631 throw Errors::RBDLError("Error: invalid joint type!");
632 }
633}
634}
sincosp
void sincosp(RBDLCasadiMath::MX_Xd_scalar x, RBDLCasadiMath::MX_Xd_scalar *sinp, RBDLCasadiMath::MX_Xd_scalar *cosp)
Definition: Joint.cc:23
RBDLCasadiMath::MX_Xd_dynamic
Definition: MX_Xd_dynamic.h:21
RBDLCasadiMath::MX_Xd_scalar
Definition: MX_Xd_scalar.h:19
RigidBodyDynamics::Errors::RBDLError
Base class for all RBDL exceptions.
Definition: rbdl_errors.h:35
Vector3_t
Definition: rbdl_eigenmath.h:61
RigidBodyDynamics::Math::Matrix3d
Matrix3_t Matrix3d
Definition: rbdl_math.h:60
RigidBodyDynamics::Math::SpatialVector
SpatialVector_t SpatialVector
Definition: rbdl_math.h:61
RigidBodyDynamics::Math::Xtrans
SpatialTransform Xtrans(const Vector3d &r)
Definition: SpatialAlgebraOperators.h:396
RigidBodyDynamics::Math::Vector3d
Vector3_t Vector3d
Definition: rbdl_math.h:57
RigidBodyDynamics::Math::Xrot
SpatialTransform Xrot(Scalar angle_rad, const Vector3d &axis)
Definition: SpatialAlgebraOperators.h:330
RigidBodyDynamics
Definition: Joint.cc:45
RigidBodyDynamics::jcalc
RBDL_DLLAPI void jcalc(Model &model, unsigned int joint_id, const VectorNd &q, const VectorNd &qdot)
Definition: Joint.cc:49
RigidBodyDynamics::jcalc_XJ
RBDL_DLLAPI Math::SpatialTransform jcalc_XJ(Model &model, unsigned int joint_id, const Math::VectorNd &q)
Definition: Joint.cc:350
RigidBodyDynamics::jcalc_X_lambda_S
RBDL_DLLAPI void jcalc_X_lambda_S(Model &model, unsigned int joint_id, const VectorNd &q)
Definition: Joint.cc:403
std::cos
MX_Xd_scalar cos(const MX_Xd_scalar &x)
Definition: MX_Xd_utils.h:351
std::sin
MX_Xd_scalar sin(const MX_Xd_scalar &x)
Definition: MX_Xd_utils.h:345
rbdl_errors.h
rbdl_math.h
RigidBodyDynamics::Math::SpatialTransform
Compact representation of spatial transformations.
Definition: SpatialAlgebraOperators.h:145
RigidBodyDynamics::Math::SpatialTransform::E
Matrix3d E
Definition: SpatialAlgebraOperators.h:311