Revision history [back]

I found that my question was not stated correctly. Restated, it would be: I have Local Rotation Axis of all the joints available. How do I get the Global Rotation Axis for these joints?

I have not found the way to do this through TF, or KDL, but two solutions I found are:

1- use Rodrigues' rotation formula.

Given:
 v = Vector to Rotate 
k = the axis on which the vector has to be rotated on 
vRot = rotated vector

vRot = v . cos(theta) +(k x v )sin (theta) + k (k.v) (1-cos(theta))
where . is dot product and x is cross product

2- as decribed here

a' = ai . Wi-parent 
a' =  global axis 
a = local axis 
Wi-Parent  = Parent's link Quaternion 
It will be rotating a vector v with a quaternion 

void rotate_vector_by_quaternion(const Vector3& v, const Quaternion& q, Vector3& vprime)
{
  // Extract the vector part of the quaternion
  Vector3 u(q.x, q.y, q.z);
  // Extract the scalar part of the quaternion
  float s = q.w;
  // Do the math
  vprime = 2.0f * dot(u, v) * u + (s*s - dot(u, u)) * v + 2.0f * s * cross(u, v);
}

I found that my question was not stated correctly. Restated, it would be: I have Local Rotation Axis of all the joints available. How do I get the Global Rotation Axis for these joints?

I have not found the way to do this through TF, or KDL, but two solutions I found are:

1- use Rodrigues' rotation formula.

Given:
 v = Vector to Rotate 
k = the axis on which the vector has to be rotated on 
vRot = rotated vector

vRot = v . cos(theta) +(k x v )sin (theta) + k (k.v) (1-cos(theta))
where . is dot product and x is cross product

2- as decribed here

a' = ai . Wi-parent 
a' =  global axis 
a = local axis 
Wi-Parent  = Parent's link Quaternion 
It will be rotating a vector v with a quaternion 

void rotate_vector_by_quaternion(const Vector3& v, const Quaternion& q, Vector3& vprime)
{
  // Extract the vector part of the quaternion
  Vector3 u(q.x, q.y, q.z);
  // Extract the scalar part of the quaternion
  float s = q.w;
  // Do the math
  vprime = 2.0f * dot(u, v) * u + (s*s - dot(u, u)) * v + 2.0f * s * cross(u, v);
}

I found that my question was not stated correctly. Restated, it would be: I have Local Rotation Axis of all the joints available. How do I get the Global Rotation Axis for these joints?

I have not found the way to do this through TF, or KDL, but two solutions I found are:

1- use Rodrigues' rotation formula.

Given:
 v = Vector to Rotate 
k = the axis on which the vector has to be rotated on 
vRot = rotated vector

vRot = v . cos(theta) +(k x v )sin (theta) + k (k.v) (1-cos(theta))

2- as decribed here

a' //a' = ai . Wi-parent 
a' //a' =  global axis 
a //a = local axis 
Wi-Parent //Wi-Parent  = Parent's link Quaternion 
It //It will be rotating a vector v with a quaternion 

void rotate_vector_by_quaternion(const Vector3& v, const Quaternion& q, Vector3& vprime)
{
  // Extract the vector part of the quaternion
  Vector3 u(q.x, q.y, q.z);
  // Extract the scalar part of the quaternion
  float s = q.w;
  // Do the math
  vprime = 2.0f * dot(u, v) * u + (s*s - dot(u, u)) * v + 2.0f * s * cross(u, v);
}