[robot] review rotation

fundamental rotation

exercise

rotation around a fixed frame

rotation around an intermediate frame

representation of rotations

1. basic rotations
   
2. axis-angle notation
   
3. quaternions
   

Compute Axis-angle based on Rotation Matrix

Convert a given rotation matrix to axis-angle representation
Question

A rotation matrix R can have two sets of solutions:
(vec, theta) or (-vec, -theta). These solutions are equivalent.

In this task, we will only test for theta in [0, pi], so you can ignore the other solution.

Singularities

You also need to handle two singularity cases, theta = 0 and theta = +pi.
(1). theta = 0 represents no rotation, and there are infinite solutions for this. So vec must be [NaN NaN NaN] in this case.
(2). theta = pi has two equivalent solutions. In this case, vec must be 2x3 matrix containing both solutions.

(3).For all other cases, vec will be a 1x3 matrix containing a single solution.
Input Format
R will be a valid 3x3 rotation matrix with det(R) = 1
Output Format
vec will be a 1x3 matrix containing a single solution if only one solution exists in [0, pi]
vec will be a 2x3 matrix containing two solutions, if two solutions exist in [0, pi]
vec will be a 1x3 matrix of the form [NaN, NaN, NaN] if infinite solutions exist in [0, pi]

Every row in vec is of the form [x y z] if the vector is $x* i + y *j + z *k$
Unit
theta is always a 1x1 matrix, and in radians

The general form of the equation is:
$\theta = arccos(0.5*trace(R)-0.5)$
$\vec{r} = \frac{1}{2sin\theta}(R_{32}-R_{23},R_{13}-R_{31},R_{21}-R_{12})$
However, it can be seen that the above equation for r is not valid when sin(theta) = 0

These are the singularity cases at theta = 0, theta = pi, theta = -pi which need to be handled separately

When theta = 0, we have infinite solutions, as it represents no rotation. So r can be any arbitrary vector.

The below equation should be useful in the theta = pi scenarios:

where, $c_{\theta}$ is $cos(\theta)$, $s_{\theta}$ is $sin(\theta)$, $v_{\theta}$ is $1-cos(\theta)$.

function [vec, theta] = RotationMatrix2AxisAngle(R)
%UNTITLED6 此处显示有关此函数的摘要
%   此处显示详细说明
temp = 0.5*trace(R) - 0.5;
if abs(temp-1)< 1e-8
        % theta = 0
    theta = 0;
    vec = [NaN,NaN,NaN];
elseif abs(temp + 1) < 1e-8
    % theta = pi
    vec = zeros(2,3);
    theta = pi;
    rx = sqrt((R(1,1)+1)/2);
    if abs(rx) < 1e-8
        % rx == 0
        ry = sqrt(0.5*R(2,2)+0.5);
        if abs(ry) < 1e-8
            rz = sqrt(0.5*R(3,3)+0.5);
            vec(1,:) = [rx,ry,rz];
            vec(2,:) = [rx,ry,-rz];
        else
            rz = R(3,2) / (2*ry);
            vec(1,:) = [rx,ry,rz];
            ry = -sqrt(0.5*R(2,2)+0.5);
            rz = R(3,2) / (2*ry);
            vec(2,:) = [rx,ry,rz];
        end
    else
        rz = (R(1,3)+R(3,1)) / (4*rx);
        ry = R(2,1)/(2*rx);
        vec(1,:) = [rx,ry,rz];
        rx = -sqrt((R(1,1)+1)/2);
        rz = (R(1,3)+R(3,1)) / (4*rx);
        ry = R(2,1)/(2*rx);
        vec(2,:) = [rx,ry,rz];
    end
else
    theta = acos(temp);
    vec = 0.5*[R(3,2)-R(2,3),R(1,3)-R(3,1),R(2,1)-R(1,2)]/sin(theta);
end
end

SLERP

question

write a function quat_slerp to perform SLERP between two quaternions and return all the intermediate quaternions.

All quaternions are represented as [Qs, Qx, Qy, Qz] in matrix form if the quaternion has scalar part Qs and vector part Qx i + Qy j + Qz k.

1. q0 is the unit quaternion representing the starting orientation, 1x4 matrix
2. q1 is the unit quaternion representing the final orientation, 1x4 matrix
3. steps is the number of quaternions required to be returned, integer value

output

1. The first step is q0, and the last step is q1

2. q_int contains q0, steps-2 intermediate quaternions, q1 - in the order of ascending order of timesteps

3. q_int is a (steps x 4) matrix

hints

1. Since the quaternions are unit quaternions, we can find the angle between them using dot product.
2. $cos(\theta) = q_{0} \cdot q_{1}$
3. At time-step t, the intermediate quaternion is given as:
   $\frac{sin[(1-t)\theta]}{sin\theta}q_{0}+\frac{sin[t\theta]}{sin\theta}q_{1}$
   
   

function [ q_int ] = quat_slerp( q0, q1, steps )
%QUAT_SLERP Perform SLERP between two quaternions and return the intermediate quaternions
%   Usage: [ q_int ] = quat_slerp( q0, q1, steps )
%   Inputs:
%       q0 is the quaternion representing the starting orientation, 1x4 matrix
%       q1 is the quaternion representing the final orientation, 1x4 matrix
%       steps is the number of intermediate quaternions required to be returned, integer value
%       The first step is q0, and the last step is q1
%   Output:
%       q_int contains q0, steps-2 intermediate quaternions, q1
%       q_int is a (steps x 4) matrix


    q_int = zeros(steps, 4);
     cos_omega = q0*q1';
    if cos_omega < 0
        q1 = -q1;
        cos_omega = -cos_omega;
    end
    sin_omega = sqrt(1-cos_omega^2);
    omega = atan2(sin_omega, cos_omega);
    inv_sin_omega = 1.0 / sin_omega;
    num = (steps-1);

    for i=1:steps
        k0 = sin((1.0-(i-1)/num)*omega) * inv_sin_omega;
        k1 = sin((i-1)/num*omega) * inv_sin_omega;
        q_int(i,:) = k0 * q0 + k1 * q1;
    end

end