"This notebook is a short sumamry of how SO(3) is handled in Pinocchio, and mostly quaternion representation. \n",
"## SO(3), Euler angles, rotations"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import pinocchio\n",
"from pinocchio import SE3, Quaternion\n",
"import numpy as np\n",
"from numpy.linalg import norm\n",
"M = SE3.Random()\n",
"R = M.rotation\n",
"p = M.translation\n",
"print('R =\\n' + str(R))\n",
"print('p =\\n' + str(p.T))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"A rotation is simply a 3x3 matrix. It has a unit norm:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"print(R @ R.T)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"It can be equivalently represented by a quaternion. Here we have made the choice to create a specific class for quaternions (i.e. they are not vectors, and can be e.g. multiplied), but you can get the 4 coefficients with the adequate method. Note that the corresponding vector is also of norm 1."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"quat = Quaternion(R)\n",
"print(norm(quat.coeffs()))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Angle-axis representation are also implemented in the class AngleAxis."
"Quaternions are \"complex of complex\", introduced form complex as complex are from reals. Let's try to understand what they contains in practice. Quaternions are of the form w+xi+yj+zk, with w,x,y,z real values, and i,j,k the 3 imaginary numbers. We store them as 4-d vectors, with the real part first: quat = [x,y,z,w]. We can interprete w as encoding the cosinus of the rotation angle. Let's see that."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"from numpy import arccos\n",
"print(arccos(quat[3]))\n",
"print(AngleAxis(quat).angle)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Indeed, w = cos(θ/2). Why divided by two? For that, let's see how the quaternion can be used to represent a rotation. We can encode a 3D vector in the imaginary part of a quaternion."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"p = np.random.rand(3)\n",
"qp = Quaternion(0., p[0], p[1], p[2])\n",
"print(qp)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The real product extends over quaternions, so let's try to multiply quat with p:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"print(quat * qp)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Well that's not a pure imaginary quaternion anymore. And the imaginary part does not contains somethig that looks like the rotated point:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"print(quat.matrix() @ p)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The pure quaternion is obtained by multiplying again on the left by the conjugate (w,-x,-y,-z)."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"print(quat * qp * quat.conjugate())"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"That is a pure quaternion, hence encoding a point, and does corresponds to R*p. Magic, is it not? We can prove that the double product of quaternion does corresponds to the rotation. Indeed, a quaternion rather encode an action (a rotation) in $R^4$, but which moves our point p outside of $R^3$. The conjugate rotation brings it back in $R^3$ but applies a second rotation. Since we rotate twice, it is necessary to apply only half of the angle each time.\n",
"What if we try to apply the rotation quat on the imaginary part of the quaternion?"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"qim = Quaternion(quat) # copy\n",
"qim[3] = 0\n",
"print(qim, quat * qim * quat.conjugate())"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"What kind of conclusion can we get from this? What geometrical interpretation can we give to $q_{im}$? What about $||q_{im}||$?"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### The SLERP example\n",
"Let's practice! Implement a linear interpolation between two position p0 and p1, i.e. find the position p(t) with t varying from 0 to 1, with p(0)=p0, p(1)=p1 and continuity between the two extrema."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"%do_not_load appendix/solution_lerp.py"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"LERP with quaternions is not working because they are not normalize. Instead we can take either the normalization of the LERP (NLERP), or the spherical LERP (SLERP). "
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"%do_not_load appendix/solution_slerp.py"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Cheat sheet\n",
"\n",
"You can convert quaternion to rotation matrix and create SE3 objects as follows:\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"qu = Quaternion(.7,.2,.2,.6)# Quaternion: take care that norm <= 1 (and approx 1)\n",
"R = qu.matrix() # Create a rotation matrix from quaternion\n",
This notebook is a short sumamry of how SO(3) is handled in Pinocchio, and mostly quaternion representation.
## SO(3), Euler angles, rotations
%% Cell type:code id: tags:
``` python
importpinocchio
frompinocchioimportSE3,Quaternion
importnumpyasnp
fromnumpy.linalgimportnorm
M=SE3.Random()
R=M.rotation
p=M.translation
print('R =\n'+str(R))
print('p =\n'+str(p.T))
```
%% Cell type:markdown id: tags:
A rotation is simply a 3x3 matrix. It has a unit norm:
%% Cell type:code id: tags:
``` python
print(R@R.T)
```
%% Cell type:markdown id: tags:
It can be equivalently represented by a quaternion. Here we have made the choice to create a specific class for quaternions (i.e. they are not vectors, and can be e.g. multiplied), but you can get the 4 coefficients with the adequate method. Note that the corresponding vector is also of norm 1.
%% Cell type:code id: tags:
``` python
quat=Quaternion(R)
print(norm(quat.coeffs()))
```
%% Cell type:markdown id: tags:
Angle-axis representation are also implemented in the class AngleAxis.
Quaternions are "complex of complex", introduced form complex as complex are from reals. Let's try to understand what they contains in practice. Quaternions are of the form w+xi+yj+zk, with w,x,y,z real values, and i,j,k the 3 imaginary numbers. We store them as 4-d vectors, with the real part first: quat = [x,y,z,w]. We can interprete w as encoding the cosinus of the rotation angle. Let's see that.
%% Cell type:code id: tags:
``` python
fromnumpyimportarccos
print(arccos(quat[3]))
print(AngleAxis(quat).angle)
```
%% Cell type:markdown id: tags:
Indeed, w = cos(θ/2). Why divided by two? For that, let's see how the quaternion can be used to represent a rotation. We can encode a 3D vector in the imaginary part of a quaternion.
%% Cell type:code id: tags:
``` python
p=np.random.rand(3)
qp=Quaternion(0.,p[0],p[1],p[2])
print(qp)
```
%% Cell type:markdown id: tags:
The real product extends over quaternions, so let's try to multiply quat with p:
%% Cell type:code id: tags:
``` python
print(quat*qp)
```
%% Cell type:markdown id: tags:
Well that's not a pure imaginary quaternion anymore. And the imaginary part does not contains somethig that looks like the rotated point:
%% Cell type:code id: tags:
``` python
print(quat.matrix()@p)
```
%% Cell type:markdown id: tags:
The pure quaternion is obtained by multiplying again on the left by the conjugate (w,-x,-y,-z).
%% Cell type:code id: tags:
``` python
print(quat*qp*quat.conjugate())
```
%% Cell type:markdown id: tags:
That is a pure quaternion, hence encoding a point, and does corresponds to R*p. Magic, is it not? We can prove that the double product of quaternion does corresponds to the rotation. Indeed, a quaternion rather encode an action (a rotation) in $R^4$, but which moves our point p outside of $R^3$. The conjugate rotation brings it back in $R^3$ but applies a second rotation. Since we rotate twice, it is necessary to apply only half of the angle each time.
What if we try to apply the rotation quat on the imaginary part of the quaternion?
%% Cell type:code id: tags:
``` python
qim=Quaternion(quat)# copy
qim[3]=0
print(qim,quat*qim*quat.conjugate())
```
%% Cell type:markdown id: tags:
What kind of conclusion can we get from this? What geometrical interpretation can we give to $q_{im}$? What about $||q_{im}||$?
%% Cell type:markdown id: tags:
### The SLERP example
Let's practice! Implement a linear interpolation between two position p0 and p1, i.e. find the position p(t) with t varying from 0 to 1, with p(0)=p0, p(1)=p1 and continuity between the two extrema.
%% Cell type:code id: tags:
``` python
%do_not_loadappendix/solution_lerp.py
```
%% Cell type:markdown id: tags:
LERP with quaternions is not working because they are not normalize. Instead we can take either the normalization of the LERP (NLERP), or the spherical LERP (SLERP).
%% Cell type:code id: tags:
``` python
%do_not_loadappendix/solution_slerp.py
```
%% Cell type:markdown id: tags:
## Cheat sheet
You can convert quaternion to rotation matrix and create SE3 objects as follows:
%% Cell type:code id: tags:
``` python
qu=Quaternion(.7,.2,.2,.6)# Quaternion: take care that norm <= 1 (and approx 1)
R=qu.matrix()# Create a rotation matrix from quaternion
