---
###### tags: `ROS`
---
# Quaternion and Rotation

## Syntax
### Basic
***Init***

Kinematics k = new DefaultKinematics();
private Point point = new Point(pos_x, pos_y, pos_z);
private Quaternion quaternion = new Quaternion((float)qua_x, (float)qua_y,
                (float)qua_z, (float)qua_w);

***moveTo***
Point:絕對座標
Quaternion:相對旋轉向量

api.MoveTo(point, quaternion, true);

***RelativeMoveTo***
Point:移動向量
Quaternion:相對旋轉向量

### Advance
***RelativeMoveToWrapper***
>[color=#02FE9D]Thread.sleep()避免**Stack Overflow**
>**Point為double**
>**Quaternion為float**

public void RelativeMoveToWrapper(double pos_x, double pos_y, double pos_z,
                                      double qua_x, double qua_y, double qua_z,
                                      float qua_w){

        final Point point = new Point(pos_x, pos_y, pos_z);
        final Quaternion quaternion = new Quaternion((float)qua_x, (float)qua_y,
                (float)qua_z, (float)qua_w);

        api.relativeMoveTo(point, quaternion, true);
        try{
            Thread.sleep(200);
        }catch(Exception e){
            Log.e("moveToWrapper",e.getMessage());
        }
    }

***Call function***

RelativeMoveToWrapper(-0.0125,0.04021,0.16992,0,0,-0.7071068,(float)0.7071068);

***Get Value***

double PZ = k.getPosition().getZ();
float QX = k.getOrientation().getX();

***Fix Position Error***

if((k.getPosition().getX() - tpx) * (k.getPosition().getX() - tpx) + (k.getPosition().getY()-tpy) * (k.getPosition().getY()-tpy) + (k.getPosition().getZ() - tpz) * (k.getPosition().getZ() - tpz) > 0.4)
                moveToWrapper(tpx,tpy,tpz,tqx,tqy,tqz,tqw);

## [Quaternion Orientation Simulator](https://quaternions.online/)

## <font color="#08EA8C">Quaternion Orientation</font>

### basic concept
***<font color="#9539D6">$\left(
\begin{array}{ccc}
x" \\
y" \\
z" \\
\end{array}
\right)=\left(
\begin{array}{ccc}
1-2(y^2+z^2) & 2(xy-wz) & 2(wx+xz) \\
2(xy+wz) & 1-2(x^2+z^2) & 2(yz-wx) \\
2(xz-wy) & 2(yz+wx) & 1-2(x^2+y^2) \\
\end{array}
\right)\left(
\begin{array}{ccc}
x' \\
y' \\
z' \\
\end{array}
\right)$</font>***

'是原始向量
”是變成的向量
中間的矩陣:$Rot(P)=qpq^-1$

$q= 四元數：w+xi+yj+zk=[cosθ, u*sinθ]=[S+V]$
$p= 原始向量（w=0,所以沒有w項）$
$q^-1 = w - x i - y j -ｚ k = [S - V]$

透過這個運算，使得q'繞著原始向量旋轉$2θ$

### $\overrightarrow{\rm n}$ and negative Quarternion Orientation
Ex:$(x,y,z,w)=(0,0.7,0,0.7)$
則 steps:
1) $(0,-0.7,0,0.7) or (0,0.7,0,-0.7)$
2) $qpq^-1$
3) 得轉移後向量即為相機應對法向量

### 用x,y,z求w
**因為 $x'^2+y'^2+z'^2=1$
所以 $sin \theta(x',y',z')=(x,y,z)$
$=> √(1-sin^2\theta)=cos\theta$
$w=cos(\theta/2)=\sqrt(1-x^2-y^2-z^2)$**

public float w()
{
    return (float)Math.sqrt(1-x*x-y*y-z*z);
}

## Resources
### 3 blue 1 brown Videos
>[color=red] **Visualizing Quaternions with Stereographic Projection**
>
> {%youtube d4EgbgTm0Bg %}

>[color=red] **Quaternion and 3D Rotation**
>
> {%youtube zjMuIxRvygQ %}

### RelativeMoveTo()可參考