pinocchio  2.6.3 A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
3) Drag and Drop (aka Inverse kinematics)

## Objectives

The main objective of the tutorial is to perform one or several tasks by inverse kinematics, i.e. pseudo inversing a jacobian iteratively until convergence of the task error.

## 3.0) Technical prerequisites

### Robots

We are going to use again the UR5 robot model, however this time mounted as a mobile robot. The source code of the mobile robot is available here. The robot has 3+6 DOF and can move (2 translations + 1 rotation) freely on the plane. Two operation frames have been defined: at the front of the basis, and at the tip of the tool. They are displayed when the robot moves.

Example of how to use the robot is has below.

from os.path import join
import pinocchio as se3
from mobilerobot import MobileRobotWrapper
from pinocchio.utils import *
PKG = '/opt/openrobots/share'
URDF = join(PKG, 'ur5_description/urdf/ur5_gripper.urdf')
robot = MobileRobotWrapper(URDF, [PKG])
NQ, NV = robot.model.nq, robot.model.nv
q = rand(NQ)
robot.display(q)
IDX_TOOL = 24
IDX_BASIS = 23
se3.framesKinematics(robot.model, robot.data)
Mtool = robot.data.oMf[IDX_TOOL]
Mbasis = robot.data.oMf[IDX_BASIS]

## 3.1) Position the end effector

The first task will be concerned with the end effector. First define a goal placement.

def place(name, M):
robot.viewer.gui.applyConfiguration(name, se3ToXYZQUAT(M))
robot.viewer.gui.refresh()
def Rquat(x, y, z, w):
q = se3.Quaternion(x, y, z, w)
q.normalize()
return q.matrix()
Mgoal = se3.SE3(Rquat(.4, .02, -.5, .7), np.matrix([.2, -.4, .7]).T)
robot.viewer.gui.addXYZaxis('world/framegoal', [1., 0., 0., 1.], .015, 4)
place('world/framegoal', Mgoal)

The current placement of the tool at configuration q is available as follows:

IDX_TOOL = 24
se3.forwardKinematics(robot.model, robot.data, q) # Compute joint placements
se3.framesKinematics(robot.model, robot.data) # Also compute operational frame placements
Mtool = robot.data.oMf[IDX_TOOL] # Get placement from world frame o to frame f oMf

The desired velocity of the tool in tool frame is given by the log:

nu = se3.log(Mtool.inverse() * Mgoal).vector

The tool Jacobian, also in tool frame, is available as follows:

J = se3.frameJacobian(robot.model, robot.data, IDX_TOOL, q)

Pseudoinverse operator is available in numpy.linalg toolbox.

from numpy.linalg import pinv

The integration of joint velocity vq in configuration q can be done directly (q += vq * dt). More generically, the se3 method integrate can be used:

q = se3.integrate(robot.model, q, vq * dt)

#### Question 1

Implement a for-loop that computes the jacobian and the desired velocity in tool frame, and deduced the joint velocity using the pseudoinverse. At each iteration, also integrate the current velocity and display the robot configuration.

## 3.2) Position the basis on a line

A line displaying "x=0" is also displayed in Gepetto viewer. Next step is to servo the front of the basis on this line.

Similarly, the distance of the basis frame to the line, with corresponding jacobian, are:

error = Mbasis.translation[0]
J = se3.frameJacobian(robot.model, robot.data, IDX_BASIS, q)[0, :]

Implement a second loop to servo the basis on the line. It becomes interesting when both tasks are performed together. We can do that simply by summing both tasks. For that, the numpy method vstack can be used to make a single error vector stacking the errors of tool and basis tasks, and similarly for the jacobians.

nu = np.vstack([nu1, nu2])
J = np.vstack([J1, J2])

However, it is stronger to move the basis only in the null space of the basis. The null space projector of J1 can be computed using the pseudoinverse. Following the control law performing task 1 and task 2 in the null space of task 1 is:

$$vq_1 = J_1^+ v_1^*$$

$$P_1 = I_9 - J_1^+ J_1$$

$$vq_2 = vq_1 + (J_2 P_1)^+ ( v_2^* - J_2 vq_1)$$

#### Question 2

Implement two loops: the first one regulate the tool placement alone. When the tool is properly placed, the second regulate the tool placement and the basis position in the null-space of the tool.