### [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: ## 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 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