Objective

In this first series of exercises, we are going to start using the library Pinocchio. We will load in the simulator the model of a simple manipulator arm, the Universal Robot 5, or UR5. This model will be used for a positioning task using simple methods. This set of exercices emphasizes the first part of the class, about direct geometry.

The software Pinocchio is a C++ library provided with a python wrapping that allows us to control it with a python terminal. Let's see how it works.

1.0) Tips

Setup

For this tutorial, you will need Pinocchio, Gepetto GUI, and the description of the ur5 robot.

For this, the easiest way is to add robotpkg apt repository and launch:

sudo apt install robotpkg-pinocchio robotpkg-ur5-description robotpkg-py310-qt5-gepetto-viewer

Python

We remind you that you can open a python terminal in your own shell. Simply type :

student@student-virtualbox:~$ ipython

>>>

Afterwards you'll just have to type your commands in this newly opened terminal.

To close the python terminal, just type CTRL-D (CTRL-C first to interrupt any on-going execution).

You can also write you command lines in a file and launch this script without entering the interactive mode (ie. without starting a new python terminal). In your shell, just type:

student@student-virtualbox:~$ ipython script.py

>>>

Pinocchio

Basic mathematical objects:

In the following, we will use numpy Matrix class to represent matrices and vectors. In numpy, vectors simply are matrices with one column. See the following example.

import numpy as np
A = np.array([[1, 2, 3, 4], [5, 6, 7, 8]])  # Define a 2x4 matrix
b = np.zeros([4, 1])  # Define a 4 vector (ie a 4x1 matrix) initialized with 0
c = A @ b             # Obtain c by multiplying A by b.

A bunch of useful functions are packaged in the utils of pinocchio.

import pinocchio as pin
from pinocchio.utils import *
import numpy as np
eye(6)                      # Return a 6x6 identity matrix
zero(6)                     # Return a zero 6x1 vector
zero([6, 4])                # Return az zero 6x4 matrix
rand(6)                     # Random 6x1 vector
isapprox(zero(6), rand(6))  # Test epsilon equality
mprint(rand([6, 6]))        # Matlab-style print
pin.skew(rand(3))               # Skew "cross-product" 3x3 matrix from a 3x1 vector
np.cross(rand(3), rand(3))     # Cross product of R^3
rotate('x', 0.4)            # Build a rotation matrix of 0.4rad around X.

Specific classes are defined to represent objects of $SE(3)$, $se(3)$ and $se(3)^*$. Rigid displacements, elements of $SE(3)$, are represented by the class SE3.

import pinocchio as pin
R = eye(3)
p = zero(3)
M0 = pin.SE3(R, p)
M = pin.SE3.Random()
M.translation = p
M.rotation = R

Spatial velocities, elements of $se(3) = M^6$, are represented by the class Motion.

v = zero(3)
w = zero(3)
nu0 = pin.Motion(v, w)
nu = pin.Motion.Random()
nu.linear = v
nu.angular = w

Spatial forces, elements of $se(3)^* = F^6$, are represented by the class Force.

f = zero(3)
tau = zero(3)
phi0 = pin.Force(f, tau)
phi = pin.Force.Random()
phi.linear = f
phi.angular = tau

1.1) Creating and displaying the robot

Robot kinematic tree

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. load_robot_description can load common robot models without issue into pinocchio. First install the example-robot-data package using: 'sudo apt install robotpkg-example-robot-data'

from robot_descriptions.loaders.pinocchio import load_robot_description
 
robot = load_robot_description("ur5_description")
model = robot.model
collision_model = robot.collision_model
visual_model = robot.visual_model

The code of the RobotWrapper class can also be used to load local URDF's and can be found at /opt/openrobots/lib/python3.10/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.

UR5 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 package ‘robotpkg-example-robot-data’

Exploring the model

The robot model is available in robot.model. It contains the names of all the robot joint names, the kinematic tree parents (i.e. the graph of parents, 0 being the root and having no parents), the position of the current joint in the parent coordinate frame jointPosition, the mass, inertia and center-of-gravity position of all the bodies (condensed in a spatial inertia 6x6 matrix) inertias and the gravity of the associated world gravity. All these functions are documented and are available in the correponding class dictionnary.

for name, function in robot.model.__class__.__dict__.items():

print(' **** %s: %s' % (name, function.__doc__))

Similarly, the robot data are available in robot.data. All the variables allocated by the classical rigid-body dynamics algorithms are stored in robot.data and are available through the python wrapping. Similarly to the model object, the function are documented and are available from the class dictionnary. The most useful in the following will be the placement of the frame associated which each joint output stored in robot.data.oMi.

For example, the robot end effector corresponds to the output of the last joint, called wrist_1_joint. The ID of the joint in the joint list can be recovered from its name, and then used to access its placement:

# Get index of end effector
 
idx = robot.index('wrist_3_joint')
 
# Compute and get the placement of joint number idx
 
placement = robot.placement(q, idx)
# Be carreful, Python always returns references to values.
# You can often .copy() the object to avoid side effects
# Only get the placement
placement = robot.data.oMi[idx].copy()

Finally, some recurring datas (used in Model and Data) have been wrapped to functions in some python shortcuts, also available in RomeoWrapper:

The size of the robot configuration is given by nq.
The dimension of its tangent space (velocity) is nv.
The index of a joint in the tree can be accessed from its name by index (see above).
The classical algorithms are also binded: com, Jcom, mass, biais, joint gravity, position and velocity of each joint.

q = zero(robot.nq)
v = rand(robot.nv)
robot.com(q)  # Compute the robot center of mass.
robot.placement(q, 3)  # Compute the placement of joint 3

Display the robot

To display the robot, we need an external program called Gepetto Viewer. If you completed the installation in the previous page, you can launch this program, open a new terminal in an empty workspace.

student@student-virtualbox:~$ gepetto-gui

This will start a server waiting for instructions. We will now create a client that will ask the server to perform some requests (such as creating a window or displaying our robot)

In a python terminal you can now load the visual model of the robot in the viewer:

robot.initViewer(loadModel=True)

This will flush the robot model inside the GUI. The argument loadModel=True is mandatory when you start or restart the GUI. In later call to your scripts, you can set the argument to False. A side effector of =True is that it will move the viewpoint inside the GUI to a reference zero position.

More details about loading the model (optionnal)

You can access the visual object composing the robot model by robot.visual_model.geometryObject.

visualObj = robot.visual_model.geometryObjects[4]  # 3D object representing the robot forarm
visualName = visualObj.name                        # Name associated to this object
visualRef = robot.getViewerNodeName(visualObj, pin.GeometryType.VISUAL)    # Viewer reference (string) representing this object

Moving one object

q1 = (1, 1, 1, 1, 0, 0, 0)  # x, y, z, quaternion
robot.viewer.gui.applyConfiguration(visualRef, q1)
robot.viewer.gui.refresh()  # Refresh the window.

Additional objects can be created, like a sphere as follows.

rgbt = [1.0, 0.2, 0.2, 1.0] # red, green, blue, transparency

robot.viewer.gui.addSphere("world/sphere", .1, rgbt) # .1 is the radius

The exhaustive list of the object that can be created is available in the IDL of the GUI: /opt/openrobots/share/idl/gepetto/corbaserver/graphical-interface.idl

1.2) Simple pick and place

Objectives: Display the robot at a given configuration or along a given trajectory

Pick:

Say we have a target at position [.5, .1, .2] and we would like the robot to grasp it.

robot.viewer.gui.applyConfiguration("world/sphere", (.5, .1, .2, 1.,0.,0.,0. ))

robot.viewer.gui.refresh() # Refresh the window.

First display a small sphere at this position to visualize it.

Then decide by any mean you want a configuration of the robot so that the end effector is touching the sphere.

At the reference position you built, the end effector placement can be obtained by robot.placement(q, 6). Only the translation part of the placement has been selected. The rotation is free.

Optional 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.

Place:

Choose any trajectory you want in the configuration space, starting from the reference position built in the previous exercice (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 in module time (from time import sleep) can be used to slow down the loop.

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.