main.asciidoc 11.9 KB
Newer Older
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
1. Move your body (aka direct geoemtry)
=======================================

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.

Tutorial 1.0. Tips
------------------

Setup
~~~~~

For all the exercices, we suppose that Pinocchio is installed in a VirtualBox and the user ID is "student" (password student).
The VirtualBox contains all the software to install. 
Alternatively, Pinocchio might be installed directly in any Ubuntu 14.04 / 16.04, after some work in other Linux, OSX, and likely in Windows (if you like to gamble).

Two directories have been created at the root of the homedir. The directory "student" is where you should develop your scripts. The directory "models" contains the models of the robot that we will use during the class.

Pinocchio has been installed using a software for "installation from sources" named +robot-pkg+. The sources of Pinocchio are available in +~/robotpkg/wip/pinocchio/work.robotics/pinochio-1.2.2+. After compilation, the software has been installed in +/opt/openrobots'. See for example the python scripts in +/opt/openrobots/lib/python2.7+.

Python
~~~~~~
We remind you that you can open a python terminal in your own shell. Simply type :
bash terminal
....
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:
bash terminal
....
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.
[source, python]
----
import numpy as np
A = np.matrix([ [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.

[source,python]
----
from pinocchio.utils import *
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
skew(rand(3))             # Skew "cross-product" 3x3 matrix from a 3x1 vector
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.

[source,python]
----
import pinocchio as se3
R = eye(3); p = zero(3)
M0 = se3.SE3(R,p)
M = se3.SE3.Random()
M.translation = p; M.rotation = R
----

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

[source,python]
----
v = zero(3); w = zero(3)
nu0 = se3.Motion(v,w)
nu = se3.Motion.Random()
nu.linear = v; nu.angular = w
----

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

[source,python]
----
f = zero(3); tau = zero(3)
phi0 = se3.Force(f,tau)
phi = se3.Force.Random()
phi.linear = f; phi.angular = tau
----


Tutorial 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. 
The first class is called RobotWrapper and is generic. 

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

[source, python]
----
from pinocchio.robot_wrapper import RobotWrapper

pkg  = '/home/student/models/'
urdf = pkg + '/ur_description/urdf/ur5_gripper.urdf'
robot = RobotWrapper(urdf,[pkg,])
----
139
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.
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
140

141
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 attached ZIP archive. Uncompressed it in the VirtualBox, for example in the directory "/home/student/models".
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
142

Guilhem Saurel's avatar
Guilhem Saurel committed
143
* UR5 model and code link:http://homepages.laas.fr/nmansard/teach/supaero2017/ur5.zip[*ur5.zip*]
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265

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.

[source,python]
----
for name,function in robot.model.__class__.__dict__.items():
  print " **** ", 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:

[source,python]
----
# Get index of end effector
idx = robot.index('wrist_3_joint')

# Compute and get the placement of joint number idx
placement = robot.position(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.

[source,python]
----
q = zero(robot.nq)
v = rand(robot.nv)
robot.com(q)           # Compute the robot center of mass.
robot.position(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
[source,python]
----
robot.initDisplay(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.
[source,python]
----
visualObj  = robot.visual_model.geometryObjects[4] # 3D object representing the robot forarm
visualName = visualObj.name                        # Name associated to this object
visualRef  = robot.viewerNodeNames(visualObj)      # Viewer reference (string) representing this object
----

Moving one object 
[source,python]
----
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.
[source,python]
----
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+


Tutorial 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.
266
267
268
269
270
[source,python]
----
robot.viewer.gui.applyConfiguration("world/sphere",[.5,.1,.2, 1.,0.,0.,0.])
robot.viewer.gui.refresh() # Refresh the window.
----
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302

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.position(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.

////
Homework
--------

Send by mail at nmansard@laas.fr a mail containing a single python file. The subject of the mail should start with +[SUPAERO] TP1+ 
When executed, the script should place the robot at the initial starting position where the end effector touches the sphere (optionally the rectangle) and move the robot with the sphere attached to the end effector.

////