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

IVIGIT.

parent 5ea83c4c
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Introduction to numerical robotics\n",
"\n",
"This notebook is a very general introduction to Pinocchio. It presents the main method to manipulate the geometry model of a manipulator robot: set the configuration, compute the position of an effector, check the collision or the distance to the obstacle. The main idea is to give a brief introduction of the general topic: how to discover and learn a robot movement constrained by the environment, using iterative optimization methods.\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import magic_donotload"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Set up\n",
"\n",
"For this class, we need the UR5 robot model (urdf, in the robotpkg_example_robot_data), the pinocchio python software, the python optimizers from scipy and for the display of the results: the gepetto viewer and the python matplotlib. We have that with this set of imports:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import pinocchio as pin\n",
"import time\n",
"import numpy as np\n",
"from numpy.linalg import inv,norm,pinv,svd,eig\n",
"import matplotlib.pylab as plt\n",
"from scipy.optimize import fmin_bfgs, fmin_slsqp\n",
"from tp0.load_environment import createRobotWithObstacles,Target"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Let's first load the robot model and display it. You should first start Gepetto Viewer: in a shell (CTRL-ALT-T to open a terminal), run the command: gepetto-gui. A new GUI widow will open (with the logo of our team): this is the viewer. You are not suppose to close this window during the rest of the class, and if you do close it, remember to reopen it before trying to display anything from Python."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"For this class, I wrapped the methods to load the robot model and create the obstacle field."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"robot = createRobotWithObstacles()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The next few lines initialize a 3D viewer."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"#Viewer = pin.visualize.GepettoVisualizer\n",
"Viewer = pin.visualize.MeshcatVisualizer\n",
"viz = Viewer(robot.model, robot.collision_model, robot.visual_model)\n",
"viz.initViewer(loadModel=True)\n",
"viz.display(robot.q0)\n",
"hasattr(viz.viewer, 'jupyter_cell') and viz.viewer.jupyter_cell()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The scene should not be displayed in the viewer (check it). The robot and the red obstacles are encoded in the robot object (we will not need to see in depth what is inside this object). The object Target is the green dot that the robot should reach. You can change the target position by editing target.position, and display the new position with target.display().\n",
"\n",
"You can display a new configuration of the robot with viz.display (take a numpy.array of dimension 6 in input):"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"viz.display(np.array([0,1.,-1.5,0,0,0]))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We also set up a target with is visualized as a green dot:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"target = Target(viz,position = np.array([.5,.5]))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Using the robot model\n",
"The robot is a 6 degrees-of-freedom (dof) manipulator. To make the example simple, we will only use the joints 1 and 2. We will then typically represent a configuration of the robot by a vector q2 of dimension 2. Yet the robot model is dimension 6, so let's define 2 helper functions to pass from 2 to 6 and from 6 to 2.\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 17-27 tp0/solution.py\n",
"\n",
"def q2_to_q6(q2):\n",
" '''\n",
" Transform a vector 2 into a vector 6, corresponding to locking 4 joints of the 6-dof arm. \n",
" '''\n",
" q6 = np.zeros(6)\n",
" q6.flat[[1,2]] = q2\n",
" return q6\n",
"\n",
"def q6_to_q2(q6):\n",
" return q6[1:3]"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The following function computes the position of the end effector (in 2d):"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 28-32 tp0/solution.py\n",
"\n",
"def endef(q):\n",
" '''Return the 2d position of the end effector.'''\n",
" pin.forwardKinematics(robot.model,robot.data,q)\n",
" return robot.data.oMi[-1].translation[[0,2]]"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The next function computes the distance between the end effector and the target."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 33-37 tp0/solution.py\n",
"\n",
"def dist(q):\n",
" '''Return the distance between the end effector end the target (2d).'''\n",
" return norm(endef(q)-target.position)\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The last function checks if the robot is in collision, and returns True if a collision is detected."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 37-42 tp0/solution.py\n",
"\n",
"def coll(q):\n",
" '''Return true if in collision, false otherwise.'''\n",
" pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q)\n",
" return pin.computeCollisions(robot.collision_model,robot.collision_data,False)\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Random search of a valid configuration\n",
"The free space is difficult to represent explicitely. We can sample the configuration space until a free configuration is found:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"def qrand(check=False):\n",
" '''\n",
" Return a random configuration. If check is True, this\n",
" configuration is not in collision\n",
" '''\n",
" while True:\n",
" q = q2_to_q6(np.random.rand(2)*6-3) # sample between -3 and +3.\n",
" if not check or not coll(q): return q\n",
"\n",
"viz.display(qrand(check=True))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Let's now find a valid configuration that is arbitrarily close to the target: sample until dist is small enough and coll is false (you may want to display the random trials inside the loop)."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 63-74 tp0/solution.py\n",
"\n",
"# Sample a random free configuration where dist is small enough.\n",
"def qrandTarget(threshold=5e-2, display=True):\n",
" while True:\n",
" q = qrand()\n",
" if display:\n",
" viz.display(q)\n",
" time.sleep(1e-3)\n",
" if not coll(q) and dist(q)<threshold:\n",
" return q\n",
"viz.display(qrandTarget())\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## From a random configuration to the target\n",
"Let' s now start from a random configuration. How can we find a path that bring the robot toward the target without touching the obstacles. Any idea?"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"%do_not_load -r 82-89 tp0/solution.py"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Configuration space\n",
"Let's try to have a better look of the configuration space. In this case, it is easy, as it is dimension 2: we can sample it exhaustively and plot it in 2d. For that, let's introduce another function to compute the distance to collision:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 51-59 tp0/solution.py\n",
"\n",
"def collisionDistance(q):\n",
" '''Return the minimal distance between robot and environment. '''\n",
" threshold = 1e-2\n",
" pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q)\n",
" if pin.computeCollisions(robot.collision_model,robot.collision_data,False): return -threshold\n",
" idx = pin.computeDistances(robot.collision_model,robot.collision_data)\n",
" return pin.computeDistance(robot.collision_model,robot.collision_data,idx).min_distance - threshold\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now, let's sample the configuration space and plot the distance-to-target and the distance-to-obstacle field (I put 500 samples to spare your CPU, but we need at least 10x more for obtaining a good picture)."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 95-129 tp0/solution.py\n",
"\n",
"def sampleSpace(nbSamples=500):\n",
" '''\n",
" Sample nbSamples configurations and store them in two lists depending\n",
" if the configuration is in free space (hfree) or in collision (hcol), along\n",
" with the distance to the target and the distance to the obstacles.\n",
" '''\n",
" hcol = []\n",
" hfree = []\n",
" for i in range(nbSamples):\n",
" q = qrand(False)\n",
" if not coll(q):\n",
" hfree.append( list(q6_to_q2(q).flat) + [ dist(q), collisionDistance(q) ])\n",
" else:\n",
" hcol.append( list(q6_to_q2(q).flat) + [ dist(q), 1e-2 ])\n",
" return hcol,hfree\n",
"\n",
"def plotConfigurationSpace(hcol,hfree):\n",
" '''\n",
" Plot 2 \"scatter\" plots: the first one plot the distance to the target for \n",
" each configuration, the second plots the distance to the obstacles (axis q1,q2, \n",
" distance in the color space).\n",
" '''\n",
" htotal = hcol + hfree\n",
" h=np.array(htotal)\n",
" plt.subplot(2,1,1)\n",
" plt.scatter(h[:,0],h[:,1],c=h[:,2],s=20,lw=0)\n",
" plt.title(\"Distance to the target\")\n",
" plt.subplot(2,1,2)\n",
" plt.scatter(h[:,0],h[:,1],c=h[:,3],s=20,lw=0)\n",
" plt.title(\"Distance to the obstacles\")\n",
"\n",
"hcol,hfree = sampleSpace(100)\n",
"plotConfigurationSpace(hcol,hfree)\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Optimize the distance under non-collision constraint\n",
"Finally, let's use one of the classic python solvers (from scipy) to search for a robot configuration that optimizes the distance to the target, under the constraint that the distance to collision is positive.\n",
"For that, we need to define a cost function (taking the robot configuration and return a scalar) and a constraint function (taking again the robot configuration and returning a scalar or a vector of scalar that should be positive). We additionally use the \"callback\" functionnality of the solver, to render the robot configuration corresponding to the current value of the decision variable inside the solver algorithm.\n",
"We are going to use the \"SLSQP\" solver from scipy, which implements a \"sequential quadratic program\" algorithm and accepts constraints."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# %load -r 133- tp0/solution.py\n",
"\n",
"def cost(q):\n",
" '''\n",
" Cost function: distance to the target\n",
" '''\n",
" return dist(q2_to_q6(q))\n",
" \n",
"def constraint(q):\n",
" '''\n",
" Constraint function: distance to the obstacle should be positive.\n",
" '''\n",
" return collisionDistance(q2_to_q6(q))\n",
" \n",
"def callback(q):\n",
" '''\n",
" At each optimization step, display the robot configuration in gepetto-viewer.\n",
" '''\n",
" viz.display(q2_to_q6(q))\n",
" time.sleep(.01)\n",
"\n",
"def optimize():\n",
" '''\n",
" Optimize from an initial random configuration to discover a collision-free\n",
" configuration as close as possible to the target. \n",
" '''\n",
" return fmin_slsqp(x0=qrand(check=True),\n",
" func=cost,\n",
" f_ieqcons=constraint,callback=callback)\n",
"\n",
"optimize()\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Look at the output of the solver. It always returns a variable value, but sometimes the algorithm fails being traped in an unfeasible region. Most of the time, the solver converges to a local minimum where the final distance to the target is nonzero"
]
}
],
"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:
# Introduction to numerical robotics
This notebook is a very general introduction to Pinocchio. It presents the main method to manipulate the geometry model of a manipulator robot: set the configuration, compute the position of an effector, check the collision or the distance to the obstacle. The main idea is to give a brief introduction of the general topic: how to discover and learn a robot movement constrained by the environment, using iterative optimization methods.
%% Cell type:code id: tags:
``` python
import magic_donotload
```
%% Cell type:markdown id: tags:
## Set up
For this class, we need the UR5 robot model (urdf, in the robotpkg_example_robot_data), the pinocchio python software, the python optimizers from scipy and for the display of the results: the gepetto viewer and the python matplotlib. We have that with this set of imports:
%% Cell type:code id: tags:
``` python
import pinocchio as pin
import time
import numpy as np
from numpy.linalg import inv,norm,pinv,svd,eig
import matplotlib.pylab as plt
from scipy.optimize import fmin_bfgs, fmin_slsqp
from tp0.load_environment import createRobotWithObstacles,Target
```
%% Cell type:markdown id: tags:
Let's first load the robot model and display it. You should first start Gepetto Viewer: in a shell (CTRL-ALT-T to open a terminal), run the command: gepetto-gui. A new GUI widow will open (with the logo of our team): this is the viewer. You are not suppose to close this window during the rest of the class, and if you do close it, remember to reopen it before trying to display anything from Python.
%% Cell type:markdown id: tags:
For this class, I wrapped the methods to load the robot model and create the obstacle field.
%% Cell type:code id: tags:
``` python
robot = createRobotWithObstacles()
```
%% Cell type:markdown id: tags:
The next few lines initialize a 3D viewer.
%% Cell type:code id: tags:
``` python
#Viewer = pin.visualize.GepettoVisualizer
Viewer = pin.visualize.MeshcatVisualizer
viz = Viewer(robot.model, robot.collision_model, robot.visual_model)
viz.initViewer(loadModel=True)
viz.display(robot.q0)
hasattr(viz.viewer, 'jupyter_cell') and viz.viewer.jupyter_cell()
```
%% Cell type:markdown id: tags:
The scene should not be displayed in the viewer (check it). The robot and the red obstacles are encoded in the robot object (we will not need to see in depth what is inside this object). The object Target is the green dot that the robot should reach. You can change the target position by editing target.position, and display the new position with target.display().
You can display a new configuration of the robot with viz.display (take a numpy.array of dimension 6 in input):
%% Cell type:code id: tags:
``` python
viz.display(np.array([0,1.,-1.5,0,0,0]))
```
%% Cell type:markdown id: tags:
We also set up a target with is visualized as a green dot:
%% Cell type:code id: tags:
``` python
target = Target(viz,position = np.array([.5,.5]))
```
%% Cell type:markdown id: tags:
## Using the robot model
The robot is a 6 degrees-of-freedom (dof) manipulator. To make the example simple, we will only use the joints 1 and 2. We will then typically represent a configuration of the robot by a vector q2 of dimension 2. Yet the robot model is dimension 6, so let's define 2 helper functions to pass from 2 to 6 and from 6 to 2.
%% Cell type:code id: tags:
``` python
# %load -r 17-27 tp0/solution.py
def q2_to_q6(q2):
'''
Transform a vector 2 into a vector 6, corresponding to locking 4 joints of the 6-dof arm.
'''
q6 = np.zeros(6)
q6.flat[[1,2]] = q2
return q6
def q6_to_q2(q6):
return q6[1:3]
```
%% Cell type:markdown id: tags:
The following function computes the position of the end effector (in 2d):
%% Cell type:code id: tags:
``` python
# %load -r 28-32 tp0/solution.py
def endef(q):
'''Return the 2d position of the end effector.'''
pin.forwardKinematics(robot.model,robot.data,q)
return robot.data.oMi[-1].translation[[0,2]]
```
%% Cell type:markdown id: tags:
The next function computes the distance between the end effector and the target.
%% Cell type:code id: tags:
``` python
# %load -r 33-37 tp0/solution.py
def dist(q):
'''Return the distance between the end effector end the target (2d).'''
return norm(endef(q)-target.position)
```
%% Cell type:markdown id: tags:
The last function checks if the robot is in collision, and returns True if a collision is detected.
%% Cell type:code id: tags:
``` python
# %load -r 37-42 tp0/solution.py
def coll(q):
'''Return true if in collision, false otherwise.'''
pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q)
return pin.computeCollisions(robot.collision_model,robot.collision_data,False)
```
%% Cell type:markdown id: tags:
## Random search of a valid configuration
The free space is difficult to represent explicitely. We can sample the configuration space until a free configuration is found:
%% Cell type:code id: tags:
``` python
def qrand(check=False):
'''
Return a random configuration. If check is True, this
configuration is not in collision
'''
while True:
q = q2_to_q6(np.random.rand(2)*6-3) # sample between -3 and +3.
if not check or not coll(q): return q
viz.display(qrand(check=True))
```
%% Cell type:markdown id: tags:
Let's now find a valid configuration that is arbitrarily close to the target: sample until dist is small enough and coll is false (you may want to display the random trials inside the loop).
%% Cell type:code id: tags:
``` python
# %load -r 63-74 tp0/solution.py
# Sample a random free configuration where dist is small enough.
def qrandTarget(threshold=5e-2, display=True):
while True:
q = qrand()
if display:
viz.display(q)
time.sleep(1e-3)
if not coll(q) and dist(q)<threshold:
return q
viz.display(qrandTarget())
```
%% Cell type:markdown id: tags:
## From a random configuration to the target
Let' s now start from a random configuration. How can we find a path that bring the robot toward the target without touching the obstacles. Any idea?
%% Cell type:code id: tags:
``` python
%do_not_load -r 82-89 tp0/solution.py
```
%% Cell type:markdown id: tags:
## Configuration space
Let's try to have a better look of the configuration space. In this case, it is easy, as it is dimension 2: we can sample it exhaustively and plot it in 2d. For that, let's introduce another function to compute the distance to collision:
%% Cell type:code id: tags:
``` python
# %load -r 51-59 tp0/solution.py
def collisionDistance(q):
'''Return the minimal distance between robot and environment. '''
threshold = 1e-2
pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q)
if pin.computeCollisions(robot.collision_model,robot.collision_data,False): return -threshold
idx = pin.computeDistances(robot.collision_model,robot.collision_data)
return pin.computeDistance(robot.collision_model,robot.collision_data,idx).min_distance - threshold
```
%% Cell type:markdown id: tags:
Now, let's sample the configuration space and plot the distance-to-target and the distance-to-obstacle field (I put 500 samples to spare your CPU, but we need at least 10x more for obtaining a good picture).
%% Cell type:code id: tags:
``` python
# %load -r 95-129 tp0/solution.py
def sampleSpace(nbSamples=500):
'''
Sample nbSamples configurations and store them in two lists depending
if the configuration is in free space (hfree) or in collision (hcol), along
with the distance to the target and the distance to the obstacles.
'''
hcol = []
hfree = []
for i in range(nbSamples):
q = qrand(False)
if not coll(q):
hfree.append( list(q6_to_q2(q).flat) + [ dist(q), collisionDistance(q) ])
else:
hcol.append( list(q6_to_q2(q).flat) + [ dist(q), 1e-2 ])
return hcol,hfree
def plotConfigurationSpace(hcol,hfree):
'''
Plot 2 "scatter" plots: the first one plot the distance to the target for
each configuration, the second plots the distance to the obstacles (axis q1,q2,
distance in the color space).
'''
htotal = hcol + hfree
h=np.array(htotal)