Commit a08f120a authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

[tp1] ivigit.

parent d0975535
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Direct and inverse geometry of 2d robots"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"This notebook the main concept of kinematic tree, direct geometry and inverse geometry, but without the kinematic tree of Pinocchio. We only use the basic geometries of the our 3d viewer for displaying the simple robot that is used in this tutorial."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import magic_donotload"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Set up\n",
"We will need NumPy, SciPy, and MeshCat Viewer for vizualizing the robot.\n",
"Scipy is a collection of scientific tools for Python. It contains, in particular, a set of optimizers that we are going to use for solving the inverse-geometry problem. If not done yet, install scipy with `sudo apt-get install python3-scipy`"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import time\n",
"import numpy as np\n",
"from scipy.optimize import fmin_bfgs,fmin_slsqp\n",
"import meshcat\n",
"from numpy.linalg import norm,inv,pinv,svd,eig\n",
"import tp1"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<a id='section_display_objects'></a>\n",
"## Displaying objects\n",
"Let's first learn how to open a 3D viewer, in which we will build our simulator. We will use the viewer MeshCat which directly displays in a browser. Open it as follows:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"viz = meshcat.Visualizer()\n",
"viz.jupyter_cell()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The following <viz> object is a client of the viewer, i.e. it will be use to pass display command to the viewer. The first commands are to create objects:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"from meshcat.geometry import Cylinder,Box,Sphere\n",
"from tp1 import colors\n",
"\n",
"viz['ball'].set_object(Sphere(radius=.2),colors.blue)\n",
"viz['cylinder'].set_object(Cylinder(height=1, radius=.1),colors.red)\n",
"viz['box'].set_object(Box([.5,.2,.4]),colors.yellow)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"\n",
"You can re-set objects under the same name, which will simply replace your object by another one. If you want to erase your world and all your objects, just run:\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"viz['ball'].delete()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Placing objects can be done using the set_transform command, and specifying the placement as a homogeneous 4x4 matrix."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"from tp1.transfo import translation\n",
"viz['cylinder'].set_transform(translation(.1,.2,.3))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"In a first time, we will work in 2D. Here is a shortcut to place an object from x,y,theta 2d placement."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"def t2d(x, y, theta): \n",
" s,c=np.sin(theta/2),np.cos(theta / 2)\n",
" return tp1.transfo.t3d(0, x, y, s,0,0,c) # Rotation around X"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"An example of a shorter positioning of a 2D object using this shortcut is:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"viz['box'].set_transform(t2d(0.1, 0.2, np.pi / 3))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Creating a 2d robot\n",
"This robot will have 2 joints, named shoulder and elbow, with link of length 1 to connect them. First let's first remove the previous objects and create the 5 geometry objects for the robot (plus one for the target)."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"viz['box'].delete()\n",
"viz['cylinder'].delete()\n",
"viz['ball'].delete()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 16-21 tp1/configuration_reduced.py\n",
"viz['joint1'].set_object(Sphere(.1),colors.red)\n",
"viz['joint2'].set_object(Sphere(.1),colors.red)\n",
"viz['joint3'].set_object(Sphere(.1),colors.red)\n",
"viz['arm1'].set_object(Cylinder(.75,.05),colors.grey)\n",
"viz['arm2'].set_object(Cylinder(.75,.05),colors.grey)\n",
"viz['target'].set_object(Sphere(.1001),colors.green)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Given a configuration vector q of dimension 2, compute the position of the centers of each object, and display correctly the robot."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"q = np.random.rand(2) * 6 - 3"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 23-36 tp1/configuration_reduced.py\n",
"\n",
"def display(q):\n",
" '''Display the robot in Gepetto Viewer. '''\n",
" assert (q.shape == (2,))\n",
" c0 = np.cos(q[0])\n",
" s0 = np.sin(q[0])\n",
" c1 = np.cos(q[0] + q[1])\n",
" s1 = np.sin(q[0] + q[1])\n",
" viz['joint1'].set_transform(t2d(0, 0, 0))\n",
" viz['arm1' ].set_transform(t2d(c0 / 2, s0 / 2, q[0]))\n",
" viz['joint2'].set_transform(t2d(c0, s0, q[0]))\n",
" viz['arm2' ].set_transform(t2d(c0 + c1 / 2, s0 + s1 / 2, q[0] + q[1]))\n",
" viz['joint3'].set_transform(t2d(c0 + c1, s0 + s1, q[0] + q[1]))\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"display(q) # Display the robot in the viewer"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The end effector is already computed in the previous method. Let's build a dedicated method to return the same value."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 36-45 tp1/configuration_reduced.py\n",
"\n",
"def endeffector(q):\n",
" '''Return the 2D position of the end effector of the robot at configuration q. '''\n",
" assert (q.shape == (2,))\n",
" c0 = np.cos(q[0])\n",
" s0 = np.sin(q[0])\n",
" c1 = np.cos(q[0] + q[1])\n",
" s1 = np.sin(q[0] + q[1])\n",
" return np.array([c0 + c1, s0 + s1])\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"endeffector(q)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"From now, we will try to (pseudo) invert the function *endeffector*, by seeking for a configuration *q* such that the end effector reaches a given position. You can first try to reach the position (0.5;0.5) either by trial-and-error, or by manually inverting the function (in the case of such a simple robot, inverting is easy)."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<a id='section_optim'></a>\n",
"## Optimize the configuration "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Optimization will be done with the BFGS solver of scipy, which simply takes an intial guess and a cost function. Here the cost will be the squared distance to a given target."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 45-51 tp1/configuration_reduced.py\n",
"target = np.array([.5, .5])\n",
"viz['target'].set_transform(translation(0,target[0],target[1]))\n",
" \n",
"def cost(q):\n",
" eff = endeffector(q)\n",
" return np.linalg.norm(eff - target)**2\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"In SciPy, BFGS also accepts a callback function, that we will use to display in the viewer the current value of the decision variable."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 51-55 tp1/configuration_reduced.py\n",
"\n",
"def callback(q):\n",
" display(q)\n",
" time.sleep(.5)\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"And that is it, let's call BFGS."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 55- tp1/configuration_reduced.py\n",
"\n",
"x0 = np.array([0.0, 0.0])\n",
"xopt_bfgs = fmin_bfgs(cost, x0, callback=callback)\n",
"print('\\n *** Xopt in BFGS = %s \\n\\n\\n\\n' % xopt_bfgs)\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## What configuration to optimize?\n",
"It seems logical to optimize over the angles $q_1,q_2$. However, other representations of the configuration are possible. Consider for example the explicit representation, where the placement of each body 1,2,3 is stored. For each body, we get $x,y,\\theta$, so 9 parameters in total. In addition, each body position is constrained with respect to the placement of the previous body, with 6 constraints in total. \n",
"\n",
"What are the pros and cons? The effector position is now a trivial function of the representation, hence the cost function is very simple. The trade-off is that we have to explicitly satisfy the constraints. \n",
"\n",
"Let's start by defining the configuration."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"x1, y1, th1, x2, y2, th2, x3, y3, th3 = x0 = np.zeros(9)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The cost function is now just a sparse difference on x3,y3:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 32-37 tp1/configuration_extended.py\n",
"\n",
"def endeffector_9(ps):\n",
" assert (ps.shape == (9, ))\n",
" x1, y1, t1, x2, y2, t2, x3, y3, t3 = ps\n",
" return np.array([x3, y3])\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"%do_not_load -r 41-44 tp1/configuration_extended.py"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The constraint function should return a vector, each coefficient corresponding to one of the 6 constraints:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 44-56 tp1/configuration_extended.py\n",
"\n",
"def constraint_9(ps):\n",
" assert (ps.shape == (9, ))\n",
" x1, y1, t1, x2, y2, t2, x3, y3, t3 = ps\n",
" res = np.zeros(6)\n",
" res[0] = x1 - 0\n",
" res[1] = y1 - 0\n",
" res[2] = x1 + np.cos(t1) - x2\n",
" res[3] = y1 + np.sin(t1) - y2\n",
" res[4] = x2 + np.cos(t2) - x3\n",
" res[5] = y2 + np.sin(t2) - y3\n",
" return res\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"For example, the configuration with the 9-vector set to 0 is not satisfying the constraints."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"print(cost_9(x0), constraint_9(x0))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We can similarly redefined the display function and the callback"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 22-32 tp1/configuration_extended.py\n",
"\n",
"def display_9(ps):\n",
" '''Display the robot in Gepetto Viewer. '''\n",
" assert (ps.shape == (9, ))\n",
" x1, y1, t1, x2, y2, t2, x3, y3, t3 = ps\n",
" viz['joint1'].set_transform(t2d(x1, y1, t1))\n",
" viz['arm1' ].set_transform(t2d(x1 + np.cos(t1) / 2, x1 + np.sin(t1) / 2, t1))\n",
" viz['joint2'].set_transform(t2d(x2, y2, t2))\n",
" viz['arm2' ].set_transform(t2d(x2 + np.cos(t2) / 2, y2 + np.sin(t2) / 2, t2))\n",
" viz['joint3'].set_transform(t2d(x3, y3, t3))\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 59-63 tp1/configuration_extended.py\n",
"\n",
"def callback_9(ps):\n",
" display_9(ps)\n",
" time.sleep(.5)\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Solve with a penalty cost\n",
"The BFGS solver defined above cannot be used directly to optimize over equality constraints. A dirty trick is to add the constraint as a penalty, i.e. a high-weigth term in the cost function: $penalty(x) = cost(x) + 100*||constraint(x)||^2$ . Here, we are in a good case where the optimum corresponds to the 0 of both the constraint and the cost. The penalty with any weight would lead to the optimum and perfect constraint satisfaction. Yet the solver suffers to reach the optimum, because of the way we have described the constraint."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Define a new function *penalty*, corresponding to the previous cost function plus a huge weight multiplying the (squared) norm of the constraint."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"scrolled": true
},
"outputs": [],
"source": [
"%do_not_load -r 56-59 tp1/configuration_extended.py"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"xopt = fmin_bfgs(penalty, x0, callback=callback_9)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Solve with a constrained solver\n",
"Alternatively, the solver S-LS-QP (sequential least-square quadratic-program) optimizes over equality constraints."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"xopt = fmin_slsqp(cost_9, x0, callback=callback_9, f_eqcons=constraint_9, iprint=2, full_output=1)[0]"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"When properly defining the constraint, the solver converges quickly. It is difficult to say a-priori whether it is better to optimize with the q (and consequently a dense cost and no constraint) or with the x-y-theta (and consequently a sparse cost and constraints). Here, we empirically observe no significant difference. "
]
}
],
"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:
# Direct and inverse geometry of 2d robots
%% Cell type:markdown id: tags:
This notebook the main concept of kinematic tree, direct geometry and inverse geometry, but without the kinematic tree of Pinocchio. We only use the basic geometries of the our 3d viewer for displaying the simple robot that is used in this tutorial.
%% Cell type:code id: tags:
``` python
import magic_donotload
```
%% Cell type:markdown id: tags:
## Set up
We will need NumPy, SciPy, and MeshCat Viewer for vizualizing the robot.
Scipy is a collection of scientific tools for Python. It contains, in particular, a set of optimizers that we are going to use for solving the inverse-geometry problem. If not done yet, install scipy with `sudo apt-get install python3-scipy`
%% Cell type:code id: tags:
``` python
import time
import numpy as np
from scipy.optimize import fmin_bfgs,fmin_slsqp
import meshcat
from numpy.linalg import norm,inv,pinv,svd,eig
import tp1
```
%% Cell type:markdown id: tags:
<a id='section_display_objects'></a>
## Displaying objects
Let's first learn how to open a 3D viewer, in which we will build our simulator. We will use the viewer MeshCat which directly displays in a browser. Open it as follows:
%% Cell type:code id: tags:
``` python
viz = meshcat.Visualizer()
viz.jupyter_cell()
```
%% Cell type:markdown id: tags:
The following <viz> object is a client of the viewer, i.e. it will be use to pass display command to the viewer. The first commands are to create objects:
%% Cell type:code id: tags:
``` python
from meshcat.geometry import Cylinder,Box,Sphere
from tp1 import colors
viz['ball'].set_object(Sphere(radius=.2),colors.blue)
viz['cylinder'].set_object(Cylinder(height=1, radius=.1),colors.red)
viz['box'].set_object(Box([.5,.2,.4]),colors.yellow)
```
%% Cell type:markdown id: tags:
You can re-set objects under the same name, which will simply replace your object by another one. If you want to erase your world and all your objects, just run:
%% Cell type:code id: tags:
``` python
viz['ball'].delete()
```
%% Cell type:markdown id: tags:
Placing objects can be done using the set_transform command, and specifying the placement as a homogeneous 4x4 matrix.
%% Cell type:code id: tags:
``` python
from tp1.transfo import translation
viz['cylinder'].set_transform(translation(.1,.2,.3))
```
%% Cell type:markdown id: tags:
In a first time, we will work in 2D. Here is a shortcut to place an object from x,y,theta 2d placement.
%% Cell type:code id: tags:
``` python
def t2d(x, y, theta):
s,c=np.sin(theta/2),np.cos(theta / 2)
return tp1.transfo.t3d(0, x, y, s,0,0,c) # Rotation around X
```
%% Cell type:markdown id: tags:
An example of a shorter positioning of a 2D object using this shortcut is:
%% Cell type:code id: tags:
``` python
viz['box'].set_transform(t2d(0.1, 0.2, np.pi / 3))
```
%% Cell type:markdown id: tags:
## Creating a 2d robot
This robot will have 2 joints, named shoulder and elbow, with link of length 1 to connect them. First let's first remove the previous objects and create the 5 geometry objects for the robot (plus one for the target).
%% Cell type:code id: tags:
``` python
viz['box'].delete()
viz['cylinder'].delete()
viz['ball'].delete()
```
%% Cell type:code id: tags:
``` python
# %load -r 16-21 tp1/configuration_reduced.py
viz['joint1'].set_object(Sphere(.1),colors.red)
viz['joint2'].set_object(Sphere(.1),colors.red)
viz['joint3'].set_object(Sphere(.1),colors.red)