1 # 1) Move your body (aka direct geometry)
5 In this first series of exercises, we are going to start using the
6 library Pinocchio. We will load in the simulator the model of a simple
7 manipulator arm, the Universal Robot 5, or UR5. This model will be used
8 for a positioning task using simple methods. This set of exercices
9 emphasizes the first part of the class, about direct geometry.
11 The software Pinocchio is a C++ library provided with a python wrapping
12 that allows us to control it with a python terminal. Let's see how it
19 For this tutorial, you will need [Pinocchio](http://stack-of-tasks.github.io/pinocchio/download.html),
20 [Gepetto GUI](https://github.com/humanoid-path-planner/gepetto-viewer-corba), and the description of the ur5 robot.
22 For this, the easiest way is to add [robotpkg apt repository](http://robotpkg.openrobots.org/debian.html) and launch:
24 sudo apt install robotpkg-py27-pinocchio robotpkg-ur5-description robotpkg-py27-qt4-gepetto-viewer-corba robotpkg-osg-dae
29 We remind you that you can open a python terminal in
30 your own shell. Simply type :
32 student@student-virtualbox:~$ ipython
35 Afterwards you'll just have to type your commands in this newly opened terminal.
37 To close the python terminal, just type CTRL-D (CTRL-C first to
38 interrupt any on-going execution).
40 You can also write you command lines in a file and launch this script
41 without entering the interactive mode (ie. without starting a new python
42 terminal). In your shell, just type:
44 student@student-virtualbox:~$ ipython script.py
50 #### Basic mathematical objects:
52 In the following, we will use numpy `Matrix` class to represent matrices
53 and vectors. In numpy, vectors simply are matrices with one column. See
54 the following example.
58 A = np.matrix([[1, 2, 3, 4], [5, 6, 7, 8]]) # Define a 2x4 matrix
59 b = np.zeros([4, 1]) # Define a 4 vector (ie a 4x1 matrix) initialized with 0
60 c = A * b # Obtain c by multiplying A by b.
63 A bunch of useful functions are packaged in the utils of pinocchio.
66 from pinocchio.utils import *
67 eye(6) # Return a 6x6 identity matrix
68 zero(6) # Return a zero 6x1 vector
69 zero([6, 4]) # Return az zero 6x4 matrix
70 rand(6) # Random 6x1 vector
71 isapprox(zero(6), rand(6)) # Test epsilon equality
72 mprint(rand([6, 6])) # Matlab-style print
73 skew(rand(3)) # Skew "cross-product" 3x3 matrix from a 3x1 vector
74 cross(rand(3), rand(3)) # Cross product of R^3
75 rotate('x', 0.4) # Build a rotation matrix of 0.4rad around X.
78 Specific classes are defined to represent objects of \f$SE(3)\f$, \f$se(3)\f$ and
79 \f$se(3)^*\f$. Rigid displacements, elements of \f$SE(3)\f$, are represented by
83 import pinocchio as pin
84 R = eye(3); p = zero(3)
87 M.translation = p; M.rotation = R
90 Spatial velocities, elements of \f$se(3) = M^6\f$, are represented by the
94 v = zero(3); w = zero(3)
95 nu0 = pin.Motion(v, w)
96 nu = pin.Motion.Random()
97 nu.linear = v; nu.angular = w
100 Spatial forces, elements of \f$se(3)^* = F^6\f$, are represented by the
104 f = zero(3); tau = zero(3)
105 phi0 = pin.Force(f, tau)
106 phi = pin.Force.Random()
107 phi.linear = f; phi.angular = tau
110 ## 1.1) Creating and displaying the robot
112 ### Robot kinematic tree
114 The kinematic tree is represented by two C++ objects called Model (which
115 contains the model constants: lengths, masses, names, etc) and Data
116 (which contains the working memory used by the model algorithms). Both
117 C++ objects are contained in a unique Python class. The first class is
118 called RobotWrapper and is generic.
120 For the next steps, we are going to work with the RobotWrapper.
122 Import the class `RobotWrapper` and create an instance of this class in
123 the python terminal. At initialization, RobotWrapper will read the model
124 description in the URDF file given as argument. In the following, we
125 will use the model of the UR5 robot, available in the directory "models"
126 of pinocchio (available in the homedir of the VBox).
129 from pinocchio.robot_wrapper import RobotWrapper
131 URDF = '/opt/openrobots/share/ur5_description/urdf/ur5_gripper.urdf'
132 robot = RobotWrapper.BuildFromURDF(URDF)
135 The code of the RobotWrapper class is in
136 `/opt/openrobots/lib/python2.7/site-packages/pinocchio/robot_wrapper.py`.
137 Do not hesitate to have a look at it and to take inspiration from the
138 implementation of the class functions.
140 UR5 is a fixed robot with one 6-DOF arms developed by the Danish company
141 Universal Robot. All its 6 joints are revolute joints. Its configuration
142 is in R^6 and is not subject to any constraint. The model of UR5 is
143 described in a URDF file, with the visuals of the bodies of the robot
144 being described as meshed (i.e. polygon soups) using the Collada format
145 ".dae". Both the URDF and the DAE files are available in the package
146 `robotpkg-ur5-description`
148 ### Exploring the model
150 The robot model is available in `robot.model`. It contains the names of
151 all the robot joint `names`, the kinematic tree `parents` (i.e. the
152 graph of parents, 0 being the root and having no parents), the position
153 of the current joint in the parent coordinate frame `jointPosition`,
154 the mass, inertia and center-of-gravity position of all the bodies
155 (condensed in a spatial inertia 6x6 matrix) `inertias` and the gravity
156 of the associated world `gravity`. All these functions are documented
157 and are available in the correponding class dictionnary.
160 for name, function in robot.model.__class__.__dict__.items():
161 print(' **** %s: %s' % (name, function.__doc__))
164 Similarly, the robot data are available in `robot.data`. All the variables
165 allocated by the classical rigid-body dynamics algorithms are stored in
166 `robot.data` and are available through the python wrapping. Similarly to
167 the model object, the function are documented and are available from the
168 class dictionnary. The most useful in the following will be the
169 placement of the frame associated which each joint output stored in
172 For example, the robot end effector corresponds to the output of the
173 last joint, called `wrist_1_joint`. The ID of the joint in the joint
174 list can be recovered from its name, and then used to access its
178 # Get index of end effector
180 idx = robot.index('wrist_3_joint')
182 # Compute and get the placement of joint number idx
184 placement = robot.placement(q, idx)
185 # Be carreful, Python always returns references to values.
186 # You can often .copy() the object to avoid side effects
187 # Only get the placement
188 placement = robot.data.oMi[idx].copy()
191 Finally, some recurring datas (used in Model and Data) have been wrapped
192 to functions in some python shortcuts, also available in RomeoWrapper:
194 - The size of the robot configuration is given by `nq`.
195 - The dimension of its tangent space (velocity) is `nv`.
196 - The index of a joint in the tree can be accessed from its name by index (see above).
197 - The classical algorithms are also binded: com, Jcom, mass, biais, joint gravity, position and velocity of each joint.
202 robot.com(q) # Compute the robot center of mass.
203 robot.placement(q, 3) # Compute the placement of joint 3
206 ### Display the robot
208 To display the robot, we need an external program called *Gepetto
209 Viewer*. If you completed the installation in the previous page, you can
210 launch this program, open a new terminal in an empty workspace.
213 student@student-virtualbox:~$ gepetto-gui
216 This will start a server waiting for instructions. We will now create a client that will
217 ask the server to perform some requests (such as creating a window or
218 displaying our robot)
220 In a python terminal you can now load the visual model of the robot in
224 robot.initViewer(loadModel=True)
227 This will flush the robot model inside the GUI. The argument
228 `loadModel=True` is mandatory when you start or restart the GUI. In
229 later call to your scripts, you can set the argument to `False`. A side
230 effector of `=True` is that it will move the viewpoint inside the GUI to
231 a reference zero position.
233 ### More details about loading the model (optionnal)
235 You can access the visual object composing the robot model by
236 `robot.visual_model.geometryObject`.
239 visualObj = robot.visual_model.geometryObjects[4] # 3D object representing the robot forarm
240 visualName = visualObj.name # Name associated to this object
241 visualRef = robot.getViewerNodeName(visualObj, pin.GeometryType.VISUAL) # Viewer reference (string) representing this object
247 q1 = (1, 1, 1, 1, 0, 0, 0) # x, y, z, quaternion
248 robot.viewer.gui.applyConfiguration(visualRef, q1)
249 robot.viewer.gui.refresh() # Refresh the window.
252 Additional objects can be created, like a sphere as follows.
255 rgbt = [1.0, 0.2, 0.2, 1.0] # red, green, blue, transparency
256 robot.viewer.gui.addSphere("world/sphere", .1, rgbt) # .1 is the radius
259 The exhaustive list of the object that can be created is available in
261 `/opt/openrobots/share/idl/gepetto/corbaserver/graphical-interface.idl`
263 ## 1.2) Simple pick and place
265 *Objectives:* Display the robot at a given configuration or along a
270 Say we have a target at position `[.5, .1, .2]` and we would like the
274 robot.viewer.gui.applyConfiguration("world/sphere", (.5, .1, .2, 1.,0.,0.,0. ))
275 robot.viewer.gui.refresh() # Refresh the window.
278 First display a small sphere at this position to visualize it.
280 Then decide by any mean you want a configuration of the robot so that
281 the end effector is touching the sphere.
283 At the reference position you built, the end effector placement can be
284 obtained by `robot.placement(q, 6)`. Only the translation part of the
285 placement has been selected. The rotation is free.
287 *Optional* Say now that the object is a rectangle and not a sphere.
288 Pick the object at a reference position with the rotation that is
289 imposed, so that the end effector is aligned with one of the faces of
294 Choose any trajectory you want in the configuration space, starting from
295 the reference position built in the previous exercice (it can be
296 sinus-cosinus waves, polynomials, splines, straight lines).
298 Make a for loop to display the robot at sampling positions along this
299 trajectory. The function sleep in module time (from time import sleep)
300 can be used to slow down the loop.
302 At each instant of your loop, recompute the position of the ball and
303 display it so that it always "sticks" to the robot end effector.