pinocchio  2.6.3
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
doc/d-practical-exercises/1-directgeom.md
1 # 1) Move your body (aka direct geometry)
2 
3 ## Objective
4 
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.
10 
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
13 works.
14 
15 ## 1.0) Tips
16 
17 ### Setup
18 
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.
21 
22 For this, the easiest way is to add [robotpkg apt repository](http://robotpkg.openrobots.org/debian.html) and launch:
23 ```
24 sudo apt install robotpkg-py27-pinocchio robotpkg-ur5-description robotpkg-py27-qt4-gepetto-viewer-corba robotpkg-osg-dae
25 ```
26 
27 ### Python
28 
29 We remind you that you can open a python terminal in
30 your own shell. Simply type :
31 ```
32 student@student-virtualbox:~$ ipython
33 >>>
34 ```
35 Afterwards you'll just have to type your commands in this newly opened terminal.
36 
37 To close the python terminal, just type CTRL-D (CTRL-C first to
38 interrupt any on-going execution).
39 
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:
43 ```
44 student@student-virtualbox:~$ ipython script.py
45 >>>
46 ```
47 
48 ### Pinocchio
49 
50 #### Basic mathematical objects:
51 
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.
55 
56 ```py
57 import numpy as np
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.
61 ```
62 
63 A bunch of useful functions are packaged in the utils of pinocchio.
64 
65 ```py
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.
76 ```
77 
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
80 the class `SE3`.
81 
82 ```py
83 import pinocchio as pin
84 R = eye(3); p = zero(3)
85 M0 = pin.SE3(R, p)
86 M = pin.SE3.Random()
87 M.translation = p; M.rotation = R
88 ```
89 
90 Spatial velocities, elements of \f$se(3) = M^6\f$, are represented by the
91 class `Motion`.
92 
93 ```py
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
98 ```
99 
100 Spatial forces, elements of \f$se(3)^* = F^6\f$, are represented by the
101 class `Force`.
102 
103 ```py
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
108 ```
109 
110 ## 1.1) Creating and displaying the robot
111 
112 ### Robot kinematic tree
113 
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.
119 
120 For the next steps, we are going to work with the RobotWrapper.
121 
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).
127 
128 ```py
129 from pinocchio.robot_wrapper import RobotWrapper
130 
131 URDF = '/opt/openrobots/share/ur5_description/urdf/ur5_gripper.urdf'
132 robot = RobotWrapper.BuildFromURDF(URDF)
133 ```
134 
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.
139 
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`
147 
148 ### Exploring the model
149 
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.
158 
159 ```py
160 for name, function in robot.model.__class__.__dict__.items():
161  print(' **** %s: %s' % (name, function.__doc__))
162 ```
163 
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
170 `robot.data.oMi`.
171 
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
175 placement:
176 
177 ```py
178 # Get index of end effector
179 
180 idx = robot.index('wrist_3_joint')
181 
182 # Compute and get the placement of joint number idx
183 
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()
189 ```
190 
191 Finally, some recurring datas (used in Model and Data) have been wrapped
192 to functions in some python shortcuts, also available in RomeoWrapper:
193 
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.
198 
199 ```py
200 q = zero(robot.nq)
201 v = rand(robot.nv)
202 robot.com(q) # Compute the robot center of mass.
203 robot.placement(q, 3) # Compute the placement of joint 3
204 ```
205 
206 ### Display the robot
207 
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.
211 
212 ```
213 student@student-virtualbox:~$ gepetto-gui
214 ```
215 
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)
219 
220 In a python terminal you can now load the visual model of the robot in
221 the viewer:
222 
223 ```py
224 robot.initViewer(loadModel=True)
225 ```
226 
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.
232 
233 ### More details about loading the model (optionnal)
234 
235 You can access the visual object composing the robot model by
236 `robot.visual_model.geometryObject`.
237 
238 ```py
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
242 ```
243 
244 Moving one object
245 
246 ```py
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.
250 ```
251 
252 Additional objects can be created, like a sphere as follows.
253 
254 ```py
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
257 ```
258 
259 The exhaustive list of the object that can be created is available in
260 the IDL of the GUI:
261 `/opt/openrobots/share/idl/gepetto/corbaserver/graphical-interface.idl`
262 
263 ## 1.2) Simple pick and place
264 
265 *Objectives:* Display the robot at a given configuration or along a
266 given trajectory
267 
268 ### Pick:
269 
270 Say we have a target at position `[.5, .1, .2]` and we would like the
271 robot to grasp it.
272 
273 ```py
274 robot.viewer.gui.applyConfiguration("world/sphere", (.5, .1, .2, 1.,0.,0.,0. ))
275 robot.viewer.gui.refresh() # Refresh the window.
276 ```
277 
278 First display a small sphere at this position to visualize it.
279 
280 Then decide by any mean you want a configuration of the robot so that
281 the end effector is touching the sphere.
282 
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.
286 
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
290 the rectangle.
291 
292 ### Place:
293 
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).
297 
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.
301 
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.