pinocchio  2.2.1-dirty
3-invkine.md
1 # 3) Drag and Drop (aka Inverse kinematics)
2 
3 ## Objectives
4 
5 The main objective of the tutorial is to perform one or several tasks by
6 inverse kinematics, i.e. pseudo inversing a jacobian iteratively until
7 convergence of the task error.
8 
9 ## 3.0) Technical prerequisites
10 
11 ### Robots
12 
13 We are going to use again the UR5 robot model, however this time mounted
14 as a mobile robot. The source code of the mobile robot is
15 [available here](mobilerobot_8py_source.html).
16 The robot has 3+6 DOF and can
17 move (2 translations + 1 rotation) freely on the plane. Two operation
18 frames have been defined: at the front of the basis, and at the tip of
19 the tool. They are displayed when the robot moves.
20 
21 Example of how to use the robot is has below.
22 
23 ```py
24 from os.path import join
25 import pinocchio as se3
26 from mobilerobot import MobileRobotWrapper
27 from pinocchio.utils import *
28 
29 PKG = '/opt/openrobots/share'
30 URDF = join(PKG, 'ur5_description/urdf/ur5_gripper.urdf')
31 
32 robot = MobileRobotWrapper(URDF, [PKG])
33 robot.initDisplay(loadModel=True)
34 
35 # robot.viewer.gui.addFloor('world/floor')
36 
37 NQ, NV = robot.model.nq, robot.model.nv
38 
39 q = rand(NQ)
40 robot.display(q)
41 
42 IDX_TOOL = 24
43 IDX_BASIS = 23
44 
45 se3.framesKinematics(robot.model, robot.data)
46 Mtool = robot.data.oMf[IDX_TOOL]
47 Mbasis = robot.data.oMf[IDX_BASIS]
48 ```
49 
50 ## 3.1) Position the end effector
51 
52 The first task will be concerned with the end effector. First define a
53 goal placement.
54 
55 ```py
56 def place(name, M):
57  robot.viewer.gui.applyConfiguration(name, se3ToXYZQUAT(M))
58  robot.viewer.gui.refresh()
59 
60 def Rquat(x, y, z, w):
61  q = se3.Quaternion(x, y, z, w)
62  q.normalize()
63  return q.matrix()
64 
65 Mgoal = se3.SE3(Rquat(.4, .02, -.5, .7), np.matrix([.2, -.4, .7]).T)
66 robot.viewer.gui.addXYZaxis('world/framegoal', [1., 0., 0., 1.], .015, 4)
67 place('world/framegoal', Mgoal)
68 ```
69 
70 The current placement of the tool at configuration `q` is available as
71 follows:
72 
73 ```py
74 IDX_TOOL = 24
75 se3.forwardKinematics(robot.model, robot.data, q) # Compute joint placements
76 se3.framesKinematics(robot.model, robot.data) # Also compute operational frame placements
77 Mtool = robot.data.oMf[IDX_TOOL] # Get placement from world frame o to frame f oMf
78 ```
79 
80 The desired velocity of the tool in tool frame is given by the log:
81 
82 ```py
83 nu = se3.log(Mtool.inverse() * Mgoal).vector
84 ```
85 
86 The tool Jacobian, also in tool frame, is available as follows:
87 
88 ```py
89 J = se3.frameJacobian(robot.model, robot.data, IDX_TOOL, q)
90 ```
91 
92 Pseudoinverse operator is available in `numpy.linalg` toolbox.
93 
94 ```py
95 from numpy.linalg import pinv
96 ```
97 
98 The integration of joint velocity `vq` in configuration `q` can be done
99 directly (`q += vq * dt`). More generically, the se3 method integrate can be
100 used:
101 
102 ```py
103 q = se3.integrate(robot.model, q, vq * dt)
104 ```
105 
106 #### Question 1
107 
108 Implement a for-loop that computes the jacobian and the
109 desired velocity in tool frame, and deduced the joint velocity using the
110 pseudoinverse. At each iteration, also integrate the current velocity
111 and display the robot configuration.
112 
113 ## 3.2) Position the basis on a line
114 
115 A line displaying "x=0" is also displayed in Gepetto viewer. Next step
116 is to servo the front of the basis on this line.
117 
118 Similarly, the distance of the basis frame to the line, with
119 corresponding jacobian, are:
120 
121 ```py
122 error = Mbasis.translation[0]
123 J = se3.frameJacobian(robot.model, robot.data, IDX_BASIS, q)[0, :]
124 ```
125 
126 Implement a second loop to servo the basis on the line. It becomes
127 interesting when both tasks are performed together. We can do that
128 simply by summing both tasks. For that, the numpy method `vstack` can be
129 used to make a single error vector stacking the errors of tool and basis
130 tasks, and similarly for the jacobians.
131 
132 ```py
133 nu = np.vstack([nu1, nu2])
134 J = np.vstack([J1, J2])
135 ```
136 
137 However, it is stronger to move the basis only in the null space of the
138 basis. The null space projector of `J1` can be computed using the
139 pseudoinverse. Following the control law performing task 1 and task 2 in
140 the null space of task 1 is:
141 
142 \f$vq_1 = J_1^+ v_1^*\f$
143 
144 \f$P_1 = I_9 - J_1^+ J_1\f$
145 
146 \f$vq_2 = vq_1 + (J_2 P_1)^+ ( v_2^* - J_2 vq_1)\f$
147 
148 #### Question 2
149 
150 Implement two loops: the first one regulate the tool
151 placement alone. When the tool is properly placed, the second regulate
152 the tool placement and the basis position in the null-space of the tool.