Commit 2873c30c authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

ci: test lint

parent 2a0eaaea
%% Cell type:markdown id: tags:
# Direct and inverse geometry of 3d robots
This notebook introduces the kinematic tree of Pinocchio for a serial manipulator, explain how to compute the forward and inverse geometry (from configuration to end-effector placements, and inversely). The ideas are examplified with a simplified case-study taken from parallel robotics.
%% Cell type:code id: tags:
``` python
import magic_donotload # noqa: F401
```
%% Cell type:markdown id: tags:
## Set up
We will need Pinocchio, Gepetto-Viewer, SciPy for the solvers
%% Cell type:code id: tags:
``` python
import time
import numpy as np
from numpy.linalg import norm
import pinocchio as pin
import example_robot_data as robex
from scipy.optimize import fmin_bfgs
```
%% Cell type:markdown id: tags:
## Kinematic tree in Pinocchio
Let's now play with 3D robots. We will load the models from URDF files.
*The robot UR5* is a low-cost manipulator robot with good performances. It is a fixed robot with one 6-DOF arms developed by the Danish company Universal Robot. All its 6 joints are revolute joints. Its configuration is in R^6 and is not subject to any constraint. The model of UR5 is described in a URDF file, with the visuals of the bodies of the robot being described as meshed (i.e. polygon soups) using the Collada format ".dae". Both the URDF and the DAE files are available in the repository in the model directory.
This robot model, as well as other models used in the notebooks, are installed from the apt paquet robotpkg-example-robot-data and stored in /opt/openrobots/share/example-robot-data.
%% Cell type:code id: tags:
``` python
# %load tp2/generated/simple_pick_and_place_1
robot = robex.load('ur5')
```
%% Cell type:markdown id: tags:
The kinematic tree is represented by two C++ objects called Model (which contains the model constants: lengths, masses, names, etc) and Data (which contains the working memory used by the model algorithms). Both C\++ objects are contained in a unique Python class. The first class is called RobotWrapper and is generic.
%% Cell type:code id: tags:
``` python
print(robot.model)
```
%% Cell type:markdown id: tags:
For the next steps, we are going to work with the RobotWrapper.
Import the class RobotWrapper and create an instance of this class in the python terminal. At initialization, RobotWrapper will read the model description in the URDF file given as argument. In the following, we will use the model of the UR5 robot, available in the directory "models" of pinocchio (available in the homedir of the VBox). The code of the RobotWrapper class is in /opt/openrobots/lib/python2.7/site-packages/pinocchio/robot_wrapper.py . Do not hesitate to have a look at it and to take inspiration from the implementation of the class functions.
Here are some import methods of the class.
%% Cell type:markdown id: tags:
* robot.q0 contains a reference initial configuration of the robot (not a pretty good one for the UR-5).
%% Cell type:markdown id: tags:
* robot.index('joint name') returns the index of the joint.
%% Cell type:code id: tags:
``` python
robot.index(' wrist_3_joint')
```
%% Cell type:markdown id: tags:
* robot.model.names is a container (~list) that contains all the joint names
%% Cell type:code id: tags:
``` python
for i, n in enumerate(robot.model.names):
print(i, n)
```
%% Cell type:markdown id: tags:
* robot.model.frames contains all the import frames attached to the robot.
%% Cell type:code id: tags:
``` python
for f in robot.model.frames:
print(f.name, 'attached to joint #', f.parent)
```
%% Cell type:markdown id: tags:
* robot.placement(idx) and robot.framePlacement(idx) returns the placement (i.e. translation+rotation of the joint / frame in argument.
%% Cell type:code id: tags:
``` python
robot.placement(robot.q0, 6) # Placement of the end effector.
```
%% Cell type:markdown id: tags:
The dimension of the configuration space (i.e. the number of joints) is given in:
%% Cell type:code id: tags:
``` python
NQ = robot.model.nq
NV = robot.model.nv # for this simple robot, NV == NQ
```
%% Cell type:markdown id: tags:
## Display simple geometries
The robot is displayed in the viewer. We are going to use Meshcat to visualize the 3d robot and scene. First open the viewer and load the robot geometries.
%% Cell type:code id: tags:
``` python
from utils.meshcat_viewer_wrapper import MeshcatVisualizer, colors # noqa: E402
viz = MeshcatVisualizer(robot)
```
%% Cell type:code id: tags:
``` python
viz.viewer.jupyter_cell()
```
%% Cell type:markdown id: tags:
A configuration *q* can be displayed in the viewer:
%% Cell type:code id: tags:
``` python
q = np.array([-1., -1.5, 2.1, -.5, -.5, 0])
viz.display(q)
```
%% Cell type:markdown id: tags:
Other geometries (cubes, spheres, etc) can be displayed as well.
%% Cell type:code id: tags:
``` python
# %load tp2/generated/simple_pick_and_place_2
# Add a red box in the viewer
ballID = "world/ball"
viz.addSphere(ballID, 0.1, colors.red)
# Place the ball at the position ( 0.5, 0.1, 0.2 )
# The viewer expect position and rotation, apppend the identity quaternion
q_ball = [0.5, 0.1, 0.2, 1, 0, 0, 0]
viz.applyConfiguration(ballID, q_ball)
```
%% Cell type:markdown id: tags:
# Forward (direct) geometry
First, let's do some forward geometry, i.e. use Pinocchio to compute where is the end effector knowning the robot configuration.
# Simple pick ...
Say we have a target at position [.5,.1,.2] and we would like the robot to grasp it.
First decide (by any way you want, e.g. trial and error) the configuration of the robot so that the end effector touches the ball.
%% Cell type:code id: tags:
``` python
# %load tp2/generated/simple_pick_and_place_3
q0 = np.zeros(NQ) # set the correct values here
q0[0] = 0.5
q0[1] = 0.
q0[2] = -1.5
q0[3] = 0.
q0[4] = 0.
q0[5] = 0.
viz.display(q0)
# Take care to explicitely mention copy when you want a copy of array.
q = q0.copy()
```
%% Cell type:markdown id: tags:
# ... and simple place
%% Cell type:markdown id: tags:
At the reference position you built, the end effector placement can be obtained by calling
%% Cell type:code id: tags:
``` python
robot.placement(q, 6).translation
```
%% Cell type:markdown id: tags:
Only the translation part of the placement has been selected. The rotation is free.
Now, choose any trajectory you want in the configuration space (it can be sinus-cosinus waves, polynomials, splines, straight lines). Make a for loop to display the robot at sampling positions along this trajectory. The function sleep can be used to slow down the loop.
%% Cell type:code id: tags:
``` python
time.sleep(.1) # in second
```
%% Cell type:markdown id: tags:
At each instant of your loop, recompute the position of the ball and display it so that it always "sticks" to the robot end effector.
%% Cell type:code id: tags:
``` python
# TODO ####################################################
# Replace here with your initial configuration
q = q0 = np.random.rand(NQ) * 6 - 3
# TODO ####################################################
# Compute initial translation between effector and box.
# Translation of end-eff wrt world at initial configuration
o_eff = robot.placement(q, 6).translation
# Translation of box wrt world
o_ball = q_ball[:3]
eff_ball = o_ball - o_eff
for i in range(100):
# Replace here by your choice of computing q(t)
q += np.random.rand(6) * 2e-1 - 1e-1
# TODO ####################################################
# Replace here by your computation of the new box position
o_ball = np.array([0., 0., 0.])
# /TODO ###################################################
# Display the new robot and box configurations.
# The viewer expect a placement (position-rotation).
viz.applyConfiguration(ballID, o_ball.tolist() + [1, 0, 0, 0])
viz.display(q)
time.sleep(0.1)
```
%% Cell type:markdown id: tags:
The solution is below, should you need it.
%% Cell type:code id: tags:
``` python
%do_not_load tp2/generated/simple_pick_and_place_4
```
%% Cell type:markdown id: tags:
### Pick and place in 3D
Say now that the object is a rectangle and not a sphere. Pick the object at a reference position with the rotation that is imposed, so that the end effector is aligned with one of the faces of the rectangle.
%% Cell type:code id: tags:
``` python
# %load tp2/generated/simple_pick_and_place_5
# Add a red box in the viewer
boxID = "world/box"
viz.delete(ballID)
viz.addBox(boxID, [0.1, 0.2, 0.1], colors.magenta)
# Place the box at the position (0.5, 0.1, 0.2)
q_box = [0.5, 0.1, 0.2, 1, 0, 0, 0]
viz.applyConfiguration('world/box', q_box)
```
%% Cell type:markdown id: tags:
A configuration with the arm nicely attached to the box is:
%% Cell type:code id: tags:
``` python
# %load tp2/generated/simple_pick_and_place_6
q0 = np.zeros(NQ)
q0[0] = -0.375
q0[1] = -1.2
q0[2] = 1.71
q0[3] = -q0[1] - q0[2]
q0[4] = q0[0]
viz.display(q0)
q = q0.copy()
```
%% Cell type:markdown id: tags:
Redo the same question as before, but now also choosing the orientation of the box. For that, at each robot configuration in your for-loop, compute the box placement wrt the world (let's denote it by oMbox) and display both the box and the robot configuration in the view.
%% Cell type:code id: tags:
``` python
%do_not_load tp2/generated/simple_pick_and_place_7
```
%% Cell type:markdown id: tags:
## Inverse geometry
We only yet computed the forward geometry, i.e. from configurations to end-effector placement. Let's to the inverse map not.
### Inverse geometry in 3D
Let's now first control the position (i.e. translation only) of the end effector of a manipulator robot to a given position. For this first part, we will use the fixed serial-chain robot model.
Recall first that the position (3D) of the joint with index "i=6" at position "q" can be access by the following two lines of code.
%% Cell type:code id: tags:
``` python
robot.placement(q, 6).translation
```
%% Cell type:markdown id: tags:
Using the scipy solver [used in the previous notebook](1_geometry_2d.ipynb#section_optim), compute a configuration q where the end effector reaches p. For that, implement a cost function that takes a configuration as argument and returns the squared distance between the end effetor and the target.
%% Cell type:code id: tags:
``` python
%do_not_load tp2/generated/invgeom3d_1
```
%% Cell type:markdown id: tags:
### Inverse geometry in 6D
6D means: translation and rotation. Change the previous cost function for a cost measuring the difference between the current placement root.placement(q,6) and a reference placement oMdes.
For that, you can use the SE(3) log function to score the distance between two placements. The log returns a 6D velocity, represented by a class Motion, that must be transformed to a vector of R^6 from which you can take the norm.
%% Cell type:code id: tags:
``` python
pin.log(pin.SE3.Identity()).vector
```
%% Cell type:code id: tags:
``` python
# %load tp2/generated/invgeom6d_1
# --- Add box to represent target
viz.addBox("world/box", [.05, .1, .2], [1., .2, .2, .5])
viz.addBox("world/blue", [.05, .1, .2], [.2, .2, 1., .5])
#
# OPTIM 6D #########################################################
#
def cost(q):
'''Compute score from a configuration'''
M = robot.placement(q, 6)
return norm(pin.log(M.inverse() * Mtarget).vector)
def callback(q):
viz.applyConfiguration('world/box', Mtarget)
viz.applyConfiguration('world/blue', robot.placement(q, 6))
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)
print('The robot finally reached effector placement at\n', robot.placement(qopt, 6))
```
%% Cell type:markdown id: tags:
### Optimizing in the quaternion space
Let's now work with a floating robot: the quadruped ANYmal. This robot has 12 joints, but Q-space of size 19 (robot.model.nq) and Q-tangent space of size 18 (robot.model.nv). This is because with need 7D vector to encode the robot placement in space, which indeed to only 6 DOF.
%% Cell type:code id: tags:
``` python
robot = robex.load('solo12')
viz = MeshcatVisualizer(robot)
viz.viewer.jupyter_cell()
```
%% 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 tp2/generated/floating_1
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
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.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)
```
%% 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
from utils import load_ur5_parallel # noqa: E402
robot = load_ur5_parallel()
```
%% Cell type:code id: tags:
``` python
viz = MeshcatVisualizer(robot)
viz.viewer.jupyter_cell()
```
%% 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])
```
%% 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())
```
%% 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.
......
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