Commit 86cb8a94 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

[TP2] Review after Guilhem new "generate" script.

parent 788c8ab3
%% 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
```
%% Output
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 time
import math
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')
```
%% Output
/opt/openrobots/lib/python3.6/site-packages/pinocchio/shortcuts.py:45: UserWarning: You passed package dir(s) via argument geometry_model and provided package_dirs.
geom_model = pin.buildGeomFromUrdf(model, filename, geometry_type, package_dirs)
%% 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)
```
%% Output
Nb joints = 7 (nq=6,nv=6)
Joint 0 universe: parent=0
Joint 1 shoulder_pan_joint: parent=0
Joint 2 shoulder_lift_joint: parent=1
Joint 3 elbow_joint: parent=2
Joint 4 wrist_1_joint: parent=3
Joint 5 wrist_2_joint: parent=4
Joint 6 wrist_3_joint: parent=5
%% 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')
```
%% Output
7
%% 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)
```
%% Output
0 universe
1 shoulder_pan_joint
2 shoulder_lift_joint
3 elbow_joint
4 wrist_1_joint
5 wrist_2_joint
6 wrist_3_joint
%% 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)
```
%% Output
universe attached to joint # 0
root_joint attached to joint # 0
world attached to joint # 0
world_joint attached to joint # 0
base_link attached to joint # 0
base_link-base_fixed_joint attached to joint # 0
base attached to joint # 0
shoulder_pan_joint attached to joint # 1
shoulder_link attached to joint # 1
shoulder_lift_joint attached to joint # 2
upper_arm_link attached to joint # 2
elbow_joint attached to joint # 3
forearm_link attached to joint # 3
wrist_1_joint attached to joint # 4
wrist_1_link attached to joint # 4
wrist_2_joint attached to joint # 5
wrist_2_link attached to joint # 5
wrist_3_joint attached to joint # 6
wrist_3_link attached to joint # 6
ee_fixed_joint attached to joint # 6
ee_link attached to joint # 6
wrist_3_link-tool0_fixed_joint attached to joint # 6
tool0 attached to joint # 6
%% 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.
```
%% Output
R =
-1 0 9.79328e-12
0 1 0
-9.79328e-12 0 -1
p = 0.81725 0.10915 -0.005491
%% 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)
```
%% Output
You can open the visualizer by visiting the following URL:
http://127.0.0.1:7000/static/
%% Cell type:code id: tags:
``` python
viz.viewer.jupyter_cell()
```
%% Output
<IPython.core.display.HTML object>
%% 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[0] = -0.375
q0[1] = -1.2
q0[2] = 1.71
q0[3] = -q0[1] - q0[2]
q0[4] = q0[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