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

IVIGIT TP4.

parent 4d617873
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Actuators\n",
"This notebook introduces ..."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"NB: as for all the tutorials, a magic command %do_not_load is introduced to hide the solutions to some questions. Change it for %load if you want to see (and execute) the solution.\n"
]
}
],
"source": [
"import magic_donotload"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Set up\n",
"We will need Pinocchio, Gepetto-Viewer, SciPy for the solvers"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [],
"source": [
"import math\n",
"import time\n",
"import numpy as np\n",
"from numpy.linalg import norm,inv,pinv,svd,eig\n",
"import pinocchio as pin\n",
"import example_robot_data as robex\n",
"from scipy.optimize import fmin_bfgs\n",
"from tp4.meshcat_viewer_wrapper import *"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"You can open the visualizer by visiting the following URL:\n",
"http://127.0.0.1:7000/static/\n"
]
},
{
"data": {
"text/html": [
"\n",
" <div style=\"height: 400px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
" <iframe src=\"http://127.0.0.1:7000/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
" </div>\n",
" "
],
"text/plain": [
"<IPython.core.display.HTML object>"
]
},
"execution_count": 7,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"robot = robex.load('solo12')\n",
"viz = MeshcatVisualizer(robot)\n",
"viz.viewer.jupyter_cell()"
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [],
"source": [
"viz.display(robot.q0)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"\n",
"Run the following code. Can you explain what just happened? Then correct it to have a proper optimization of ANYmal configuration."
]
},
{
"cell_type": "code",
"execution_count": 15,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Optimization terminated successfully.\n",
" Current function value: 0.614958\n",
" Iterations: 74\n",
" Function evaluations: 1620\n",
" Gradient evaluations: 81\n"
]
}
],
"source": [
"# %load -r 28- tp2/floating.py\n",
"\n",
"robot.feetIndexes = [ robot.model.getFrameId(frameName) for frameName in ['HR_FOOT','HL_FOOT','FR_FOOT','FL_FOOT' ] ]\n",
"\n",
"# --- Add box to represent target\n",
"colors = ['red','blue','green','magenta']\n",
"for color in colors:\n",
" viz.addSphere(\"world/%s\"%color, .05, color)\n",
" viz.addSphere(\"world/%s_des\"%color, .05, color)\n",
"\n",
"#\n",
"# OPTIM 6D #########################################################\n",
"#\n",
"\n",
"targets = [\n",
" np.array( [-0.7, -0.2, 1.2]),\n",
" np.array( [-0.3, 0.5, 0.8]),\n",
" np.array( [ 0.3, 0.1, -0.1]),\n",
" np.array( [ 0.9, 0.9, 0.5])\n",
" ]\n",
"for i in range(4): targets[i][2]+=1\n",
"\n",
"def cost(q):\n",
" '''Compute score from a configuration'''\n",
" cost = 0.\n",
" for i in range(4):\n",
" p_i = robot.framePlacement(q, robot.feetIndexes[i]).translation\n",
" cost += norm(p_i-targets[i])**2\n",
" cost += 100*(1-norm(q[3:7]))**2\n",
" return cost\n",
"\n",
"def callback(q):\n",
" viz.applyConfiguration('world/box', Mtarget)\n",
"\n",
" for i in range(4):\n",
" p_i = robot.framePlacement(q, robot.feetIndexes[i])\n",
" viz.applyConfiguration('world/%s'%colors[i], p_i)\n",
" viz.applyConfiguration('world/%s_des'%colors[i], list(targets[i])+[1,0,0,0])\n",
"\n",
" viz.display(q)\n",
" time.sleep(1e-1)\n",
"\n",
"Mtarget = pin.SE3(pin.utils.rotate('x',3.14/4), np.array([0.5, 0.1, 0.2])) # x,y,z\n",
"qopt = fmin_bfgs(cost, robot.q0, callback=callback)\n",
"\n"
]
},
{
"cell_type": "code",
"execution_count": 13,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"3.4645610585829423"
]
},
"execution_count": 13,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"norm(qopt[3:7])"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Configuration of parallel robots\n",
"A parallel robot is composed of several kinematic chains (called the robot legs) that are all attached to the same end effector. This imposes strict constraints in the configuration space of the robot: a configuration is valide iff all the legs meets the same end-effector placement. We consider here only the geometry aspect of parallel robots (additionnally, some joints are not actuated, which causes additional problems).\n",
"\n",
"The kinematic structure of a paralel robot indeed induces loops in the joint connection graph. In Pinocchio, we can only represents (one of) the underlying kinematic tree. The loop constraints have to be handled separately. An example that loads 4 manipulator arms is given below."
]
},
{
"cell_type": "code",
"execution_count": 18,
"metadata": {},
"outputs": [],
"source": [
"import tp2.load_ur5_parallel as robex2\n",
"robot = robex2.load()"
]
},
{
"cell_type": "code",
"execution_count": 19,
"metadata": {
"scrolled": true
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"You can open the visualizer by visiting the following URL:\n",
"http://127.0.0.1:7002/static/\n"
]
},
{
"data": {
"text/html": [
"\n",
" <div style=\"height: 400px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
" <iframe src=\"http://127.0.0.1:7002/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
" </div>\n",
" "
],
"text/plain": [
"<IPython.core.display.HTML object>"
]
},
"execution_count": 19,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"viz = MeshcatVisualizer(robot)\n",
"viz.viewer.jupyter_cell()"
]
},
{
"cell_type": "code",
"execution_count": 20,
"metadata": {},
"outputs": [],
"source": [
"viz.display(robot.q0)"
]
},
{
"cell_type": "code",
"execution_count": 21,
"metadata": {},
"outputs": [],
"source": [
"[w, h, d] = [0.5, 0.5, 0.005]\n",
"color = [red, green, blue, transparency] = [1, 1, 0.78, .8]\n",
"viz.addBox('world/robot0/toolplate', [w, h, d], color)\n",
"Mtool = pin.SE3(pin.utils.rotate('z', 1.268), np.array([0, 0, .75]))\n",
"viz.applyConfiguration('world/robot0/toolplate', Mtool)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The 4 legs of the robot are loaded in a single robot model. The 4 effector placements are computed by:"
]
},
{
"cell_type": "code",
"execution_count": 22,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
" R =\n",
" -0.707107 -0.707107 -3.46245e-12\n",
" 0.707107 -0.707107 -3.46236e-12\n",
"-6.12323e-17 -4.89664e-12 1\n",
" p = 0.306122 0.160483 0.749342"
]
},
"execution_count": 22,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"effIdxs = [ robot.model.getFrameId('tool0_#%d' % i) for i in range(4)]\n",
"robot.framePlacement(robot.q0,effIdxs[0])"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The loop constraints are that the relative placement of every leg end-effector must stay the same that in the initial configuration given as example in with the configuration *robot.q0* and the plate placement *Mtool*. To be valid, a configuration *q* must satisfy these 4 relative placement constraints.\n",
"\n",
"Consider now that the orientation of the tool plate is given by the following quaternion, with the translation that you like (see [the notebook about rotations if you need more details](appendix1_quaternions.ipynb)): \n",
"\n"
]
},
{
"cell_type": "code",
"execution_count": 23,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[[ 0.13978495 -0.8172043 0.55913978]\n",
" [ 0.98924731 0.13978495 -0.04301075]\n",
" [-0.04301075 0.55913978 0.82795699]]\n"
]
}
],
"source": [
"quat = pin.Quaternion(0.7,0.2,0.2,0.6).normalized()\n",
"print(quat.matrix())"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"**Find using the above optimization routines the configuration of each robot leg so that the loop constraints are all met** for the new orientation of the plate."
]
}
],
"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:
# Actuators
This notebook introduces ...
%% Cell type:code id: tags:
``` python
import magic_donotload
```
%%%% Output: stream
NB: as for all the tutorials, a magic command %do_not_load is introduced to hide the solutions to some questions. Change it for %load if you want to see (and execute) the solution.
%% Cell type:markdown id: tags:
## Set up
We will need Pinocchio, Gepetto-Viewer, SciPy for the solvers
%% Cell type:code id: tags:
``` python
import math
import time
import numpy as np
from numpy.linalg import norm,inv,pinv,svd,eig
import pinocchio as pin
import example_robot_data as robex
from scipy.optimize import fmin_bfgs
from tp4.meshcat_viewer_wrapper import *
```
%% Cell type:code id: tags:
``` python
robot = robex.load('solo12')
viz = MeshcatVisualizer(robot)
viz.viewer.jupyter_cell()
```
%%%% Output: stream
You can open the visualizer by visiting the following URL:
http://127.0.0.1:7000/static/
%%%% Output: execute_result
<IPython.core.display.HTML object>
%% Cell type:code id: tags:
``` python
viz.display(robot.q0)
```
%% Cell type:markdown id: tags:
Run the following code. Can you explain what just happened? Then correct it to have a proper optimization of ANYmal configuration.
%% Cell type:code id: tags:
``` python
# %load -r 28- tp2/floating.py
robot.feetIndexes = [ robot.model.getFrameId(frameName) for frameName in ['HR_FOOT','HL_FOOT','FR_FOOT','FL_FOOT' ] ]
# --- Add box to represent target
colors = ['red','blue','green','magenta']
for color in colors:
viz.addSphere("world/%s"%color, .05, color)
viz.addSphere("world/%s_des"%color, .05, color)
#
# OPTIM 6D #########################################################
#
targets = [
np.array( [-0.7, -0.2, 1.2]),
np.array( [-0.3, 0.5, 0.8]),
np.array( [ 0.3, 0.1, -0.1]),
np.array( [ 0.9, 0.9, 0.5])
]
for i in range(4): targets[i][2]+=1
def cost(q):
'''Compute score from a configuration'''
cost = 0.
for i in range(4):
p_i = robot.framePlacement(q, robot.feetIndexes[i]).translation
cost += norm(p_i-targets[i])**2
cost += 100*(1-norm(q[3:7]))**2
return cost
def callback(q):
viz.applyConfiguration('world/box', Mtarget)
for i in range(4):
p_i = robot.framePlacement(q, robot.feetIndexes[i])
viz.applyConfiguration('world/%s'%colors[i], p_i)
viz.applyConfiguration('world/%s_des'%colors[i], list(targets[i])+[1,0,0,0])
viz.display(q)
time.sleep(1e-1)
Mtarget = pin.SE3(pin.utils.rotate('x',3.14/4), np.array([0.5, 0.1, 0.2])) # x,y,z
qopt = fmin_bfgs(cost, robot.q0, callback=callback)
```
%%%% Output: stream
Optimization terminated successfully.
Current function value: 0.614958
Iterations: 74
Function evaluations: 1620
Gradient evaluations: 81
%% Cell type:code id: tags:
``` python
norm(qopt[3:7])
```
%%%% Output: execute_result
3.4645610585829423
%% Cell type:markdown id: tags:
## Configuration of parallel robots
A parallel robot is composed of several kinematic chains (called the robot legs) that are all attached to the same end effector. This imposes strict constraints in the configuration space of the robot: a configuration is valide iff all the legs meets the same end-effector placement. We consider here only the geometry aspect of parallel robots (additionnally, some joints are not actuated, which causes additional problems).
The kinematic structure of a paralel robot indeed induces loops in the joint connection graph. In Pinocchio, we can only represents (one of) the underlying kinematic tree. The loop constraints have to be handled separately. An example that loads 4 manipulator arms is given below.
%% Cell type:code id: tags:
``` python
import tp2.load_ur5_parallel as robex2
robot = robex2.load()
```
%% Cell type:code id: tags:
``` python
viz = MeshcatVisualizer(robot)
viz.viewer.jupyter_cell()
```
%%%% Output: stream
You can open the visualizer by visiting the following URL:
http://127.0.0.1:7002/static/
%%%% Output: execute_result
<IPython.core.display.HTML object>
%% Cell type:code id: tags:
``` python
viz.display(robot.q0)
```
%% Cell type:code id: tags:
``` python
[w, h, d] = [0.5, 0.5, 0.005]
color = [red, green, blue, transparency] = [1, 1, 0.78, .8]
viz.addBox('world/robot0/toolplate', [w, h, d], color)
Mtool = pin.SE3(pin.utils.rotate('z', 1.268), np.array([0, 0, .75]))
viz.applyConfiguration('world/robot0/toolplate', Mtool)
```
%% Cell type:markdown id: tags:
The 4 legs of the robot are loaded in a single robot model. The 4 effector placements are computed by:
%% Cell type:code id: tags:
``` python
effIdxs = [ robot.model.getFrameId('tool0_#%d' % i) for i in range(4)]
robot.framePlacement(robot.q0,effIdxs[0])
```
%%%% Output: execute_result
R =
-0.707107 -0.707107 -3.46245e-12
0.707107 -0.707107 -3.46236e-12
-6.12323e-17 -4.89664e-12 1
p = 0.306122 0.160483 0.749342
%% Cell type:markdown id: tags:
The loop constraints are that the relative placement of every leg end-effector must stay the same that in the initial configuration given as example in with the configuration *robot.q0* and the plate placement *Mtool*. To be valid, a configuration *q* must satisfy these 4 relative placement constraints.
Consider now that the orientation of the tool plate is given by the following quaternion, with the translation that you like (see [the notebook about rotations if you need more details](appendix1_quaternions.ipynb)):
%% Cell type:code id: tags:
``` python
quat = pin.Quaternion(0.7,0.2,0.2,0.6).normalized()
print(quat.matrix())
```
%%%% Output: stream
[[ 0.13978495 -0.8172043 0.55913978]
[ 0.98924731 0.13978495 -0.04301075]
[-0.04301075 0.55913978 0.82795699]]
%% Cell type:markdown id: tags:
**Find using the above optimization routines the configuration of each robot leg so that the loop constraints are all met** for the new orientation of the plate.
../tp2/meshcat_viewer_wrapper/
\ No newline at end of file
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