I have a body in 3D-space and I would like to calculate the rotation axis when the body moves from A to B. I know the location (x, y and z) and the orientation (rx, ry and rz (axis angles)) at both A and B.
I have read about Rodriguez' rotation formula and know how to find the axis of rotation, but how can I incorporate the translation components as well? Any help is appreciated!
/Daniel
Ok, I tried a different way: using three points (p0, q0 and r0) on the rigid body.
I hope you can stand some matlab lingo.
%points before (x, y, z)
p0=[0.25 -0.34 -2.57]'+[1556 24.9 618.7]';
q0 = [-0.3995 -0.069 -1.487]'+[1600 -445 763]';
r0 = [-0.278 0.1376 -3.57]'+[1525 475 850.5]';
%points after
p1 = [-0.17 -0.47 -2.46]'+[1556 24.9 618.7]';
q1 = [0.99 -0.045 -1.42]'+[1600 -445 763]';
r1 = [0.52 0.11 -3.53]'+[1525 475 850.5]';
I then calculate the rotation vector as:
w = cross(((q1-q0)-(r1-r0)), ((p1-p0)-(r1-r0)))/dot(((q1-q0)-(r1-r0)), ((p1+p0)-(r1+r0)));
This seems to give reasonable results. Assuming this is correct I know the direction vector of the rotation, but how can I find the position of this vector?
/Daniel
Thanks! However, the calculation of a point on the rotation vector is not clear to me. It was many years since I did this kind of computations, so please bear with me. I found a short paper about this: http://robotics.caltech.edu/~jwb/courses/ME115/handouts/rodriguez.pdf
I have calculated the rotation vector as above, which corresponds to equation 8 in the paper. Now I also use equation 10 (in the paper) to calculate a point on the rotation vector.
p = 0.5*((cross(w, (p1-p0))/rot_angle) - (dot(w, p1 + p0).*w + p0 + p1));
where
rot_angle = norm(w) %this is tan(theta/2)
With the figures I have equation 10 yields:
p = [-1551.09 105.76 -555.65]
Is this a correct way to do it?
/Daniel