I am using RobotStudio to move an end-effector from one coordinate and direction to another coordinate and direction. I understand how to move the robot to all my points, but for the direction T need four quaternian values q1, q2, q3 and q4. I know the directional vector for each point, but the question is:
How do i calculate quaternion values from two vectors?
The end-effector's rotation about its own $z$-axis can be anything and does not matter for me. I have tried to calculate the two vectors to a rotational matrix from which I can easily get the quaternions, but with every different method for calculating the rotational matrix I get different results.
Using the method given by Jur van der Berg in Calculate Rotation Matrix to align Vector A to Vector B in 3d? in MATLAB I get these calculations:
a=[-1.308199919 -2.545574849 -10.6211422]';
b=[-1.422934396 -2.595777939 -10.59420571]';
v = cross(a,b);
vx = [0 -v(3) v(2) ,
v(3) 0 -v(1),
-v(2) v(1) 0 ];
c = dot(a,b);
I = eye(3);
R1=I+vx+vx^2*(1/(1+c))
This my result:
R1 =
0.9867 0.2202 1.2550
-0.2326 0.9966 0.5995
-1.2527 -0.6041 0.9841
However, using Shiyu's method in same thread, I get what I believe is the correct answer:
R2 =
0.9999 0.0018 0.0104
-0.0019 1.0000 0.0050
-0.0104 -0.0050 0.9999
Do you know what is the problem with my calculations or whether there is a better way to do it?
I know I ask two questions, I would prefer a good method to calculate the quaternions directly from two vectors, but if you can fix my calculations it would be really appreciated.
Thanks a lot, I hope you can help.
Best Answer
Assume you have vectors $u,v\in\mathbb R^3$ and you need to find a unit quaternion $q\in\mathbb S^3\subseteq\mathbb R^4$ that rotates the direction of $u$ onto the direction of $v$, meaning $u^p/\|u\|=v/\|v\|$ with the Euclidean norm $\|\bullet\|$ and where $\bullet^p$ is defined by $$ \begin{bmatrix}0\\u^p\end{bmatrix} = p*\begin{bmatrix}0\\u\end{bmatrix}*\overline{p}, $$ where $\overline{p}$ is the quaternionic conjugate and $*$ is the quaternion multiplication.
A quaternion that you can use is $$ p_{u\to v} = \begin{bmatrix}\|u\|\|v\| + u^T{\cdot}v\\u\times v\end{bmatrix}\frac1{\bigl\|u\|v\| + \|u\|v \bigr\|}, $$ where $u^T{\cdot}v$ is the dot product and $\times$ is the cross product. (The only case where this does not work is when $u$ and $v$ point in opposing direction, ie. when there is a $\lambda<0$ with $u=\lambda v$.)
We can see that we are using the directon of $u\times v$ that is perpendicular to $u$ and $v$ as the axis of rotation. Since the dot product and cross product are already related to the sine and cosine of the enclosing angle, we don't need to calculate any trig functions.
Note that this $p_{u\to v}$ is not the only quaternion that does this, but it is the only one (except for its antipode $-p_{u\to v}$, because antipodal quaternions encode the same rotation) that does not introduce torque. The quaternion that introduced torque around $u$ by angle $\theta$ is given by $$ p_{u,\theta} = \begin{bmatrix}\cos\tfrac\theta2\\ \frac u{\|u\|}\cos\tfrac\theta2\end{bmatrix}. $$
Now we can express all quaternions that rotate $u$ onto $v$, where a torque with angle $\theta$ is introduced by $$ p_{u,\theta\to v} = p_{u\to v}*p_{u,\theta}. $$