Notes written by lgswdn when working on this lab.

Just for reference.

---

## rotation

3-d axis angle representation: $(x,y,z)$ as the axis vector, $||(x,y,z)||_2$ as the angle 

Basic idea of quat as rotation: use $\cos(\frac{\theta}{2})+\sin(\frac{\theta}{2})u$ as the rotation quaternion produce the rotation with $u$ as axis and $\theta$ as angle.

assume $q$ already normalized

#### quat_rotate

when rotate, the answer is $v'=qv\bar q$

#### quat_relative_angle

calculate the inner product, then arccos to get the relative angle

*Note*: the actual rotation angle of a quaternion is $2\times$ the arccos value.

#### interpolate_quat

calculate relative angle, then use $\frac{\sin (ratio\times \alpha)}{\sin \alpha}$ as the true ratio.

*Note*: if inner product $<0$, we need to set $q_2=-q_2$.

*Note*: this is the interpolation on the 4-D unit ball instead of real rotation, thus we don't need to $2\times$ the arccos value.

#### quat_to_axis_angle

if $w<0$, we need to set $q=-q$ (otherwise the angle would > $\pi$)

#### uniform_random_quat

uniform random on $Q$ is equivalent to uniform random on $SO(3)$. Simply uniform random from $N(0,I_{3\times 3})$ and normalize.

## forward kinetics

*Note*: the `qpos` only list the angle of revolute joints. check the type of current joint is needed.

Algo steps:

1. set the first link as $I_{4\times 4}$.

2. for each joint, get $T^{\text{link}_i}_{\text{joint}_i}=\left(\begin{matrix}R_{3\times 3} & t_{3\times 1} \\ 0 & 1\end{matrix}\right)$ from the joint information.

3. If the current joint is revolute joint, get the rotate angle $\theta$,

   and build rotation matrix $\left(\begin{matrix}\cos \theta& -\sin \theta & 0\\ \sin \theta & \cos \theta& 0 \\0 & 0 & 1\end{matrix} \right)$.

4. do matrix multiplication to get $T^{\text{link}_i}_{\text{link}_{i+1}}=T^{\text{link}_i}_{\text{joint}_i}\times T^{\text{joint}_i}_{\text{link}_{i+1}}$.

5. do matrix multiplication to get $T^{\text{link}_0}_{\text{link}_{i+1}}=T^{\text{link}_0}_{\text{link}_i}\times T^{\text{link}_i}_{\text{link}_{i+1}}$.