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

[tp1] ivigit.

parent d0975535
%% 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