Commit 0e9022be authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

[appendix] IVIGIT.

parent a08f120a
import time
import numpy as np
p0 = np.random.rand(3)
p1 = np.random.rand(3)
for t in np.arange(0, 1, .01):
p = p0 * (1 - t) + p1 * t
viz.applyConfiguration('world/box', list(p) + list(quat.coeffs()))
time.sleep(.01)
from time import sleep
import numpy as np
from numpy.linalg import norm
from pinocchio import SE3, AngleAxis, Quaternion
def _lerp(p0, p1, t):
return (1 - t) * p0 + t * p1
def slerp(q0, q1, t):
assert (t >= 0 and t <= 1)
a = AngleAxis(q0.inverse() * q1)
return Quaternion(AngleAxis(a.angle * t, a.axis))
def nlerp(q0, q1, t):
q0 = q0.coeffs()
q1 = q1.coeffs()
lerp = _lerp(q0, q1, t)
lerp /= norm(lerp)
return Quaternion(lerp[3], *list(lerp[:3]))
q0 = Quaternion(SE3.Random().rotation)
q1 = Quaternion(SE3.Random().rotation)
viz.applyConfiguration('world/box', [0, 0, 0] + list(q0.coeffs()))
sleep(.1)
for t in np.arange(0, 1, .01):
q = nlerp(q0, q1, t)
viz.applyConfiguration('world/box', [0, 0, 0] + list(q.coeffs()))
sleep(.01)
sleep(.1)
viz.applyConfiguration('world/box', [0, 0, 0] + list(q1.coeffs()))
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Quaternions representing rotations SO(3)\n",
"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."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"from pinocchio import AngleAxis\n",
"utheta = AngleAxis(quat)\n",
"print(utheta.angle, utheta.axis)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"You can display rotation in the viewer."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"from tp2.meshcat_viewer_wrapper import MeshcatVisualizer\n",
"viz = MeshcatVisualizer()\n",
"viz.viewer.jupyter_cell()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"viz.addBox('world/box', [.1, .2, .3], [1, 0, 0, 1])\n",
"viz.applyConfiguration('world/box', [.1, .2, .3] + list(quat.coeffs()))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Quaternion you said?\n",
"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",
"p = np.array([0.,0.,0.77]) # Translation (R3) vector)\n",
"M = SE3(R,p) # Create a nomogeneous matrix from R,P\n",
"\n",
"# Typical tool position\n",
"from pinocchio.utils import rotate\n",
"M = SE3(rotate('z',1.)@rotate('x',.2), np.array([0.1,0.02,.65]))"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.6.9"
}
},
"nbformat": 4,
"nbformat_minor": 2
}
%% Cell type:markdown id: tags:
# Quaternions representing rotations SO(3)
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
import pinocchio
from pinocchio import SE3, Quaternion
import numpy as np
from numpy.linalg import norm
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.
%% Cell type:code id: tags:
``` python
from pinocchio import AngleAxis
utheta = AngleAxis(quat)
print(utheta.angle, utheta.axis)
```
%% Cell type:markdown id: tags:
You can display rotation in the viewer.
%% Cell type:code id: tags:
``` python
from tp2.meshcat_viewer_wrapper import MeshcatVisualizer
viz = MeshcatVisualizer()
viz.viewer.jupyter_cell()
```
%% Cell type:code id: tags:
``` python
viz.addBox('world/box', [.1, .2, .3], [1, 0, 0, 1])
viz.applyConfiguration('world/box', [.1, .2, .3] + list(quat.coeffs()))
```
%% Cell type:markdown id: tags:
### Quaternion you said?
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
from numpy import arccos
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_load appendix/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_load appendix/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
p = np.array([0.,0.,0.77]) # Translation (R3) vector)
M = SE3(R,p) # Create a nomogeneous matrix from R,P
# Typical tool position
from pinocchio.utils import rotate
M = SE3(rotate('z',1.)@rotate('x',.2), np.array([0.1,0.02,.65]))
```
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment