UR5 Robot Arm Manipulation

Project Goal

The objective of this project is to use the UR5 robot to perform the classic pick-and-place operation automatically. I will implement a program that allows me to ﬁrst “teach” the UR5 the position of the start and target locations and then automatically completes the pick-and-place operation using a gripper and several diﬀerent types of control. To ”teach” is a term used for the process of manually jogging or moving a robot to the desired location and then recording that position and orientation for later use in the program.

Figure 1: UR5 placing the cube on the target location

The goal is to move a wooden cube from the start position to the target position, both of which will be somewhere on the lab table inside the workspace of the UR5. Grippers will be installed on the end of the UR5 and they can be initialized, opened, and closed as needed using the functions in ur5 interface.m.

Methods

Three implementations of a pick-and-place task are created using the API provided in UR5.interface. Using the frame locations that I “teach”, I will plan a control trajectory for the robot in Cartesian space, i.e. in SE(3). The process should begin and end with the robot in some ”home” conﬁguration that I decide is best. I implement the control trajectory in three diﬀerent ways:

Inverse kinematics. Using inverse kinematics, move the robot along the desired Cartesian path by ﬁnding the corresponding path in joint space.
Rate control using diﬀerential kinematics. Find the desired velocity (or small, diﬀerntial Cartesian oﬀset), and “pull that back” through the inverse of the Jacobian to ﬁnd the joint space velocity (increment) that moves in the right direction.
Gradient-based control. Do the same thing, but now use the transpose, not inverse, of the Jacobian matrix.

Controlling the Gripper

Mechanical Interface

The gripper used in the project is EU-20. The detailed speciﬁcation of the gripper is available online. In our setup, this gripper is attached to the UR5 end-eﬀector, and controlled by UR5 tool digital output.

API

Use these functions to control the gripper:

ur5.init gripper Run this function before sending commands to initialize the gripper.
ur5.open gripper Open the gripper
ur5.close gripper Close the gripper

Inverse Kinematics

The function ur5InvKin.m will be provided that calculates all eight possible solutions to the inverse kinematics for the UR5 at a given frame in the workspace. Several of the solutions will not be practical to use and you will need to select the “best” solution to move the robot to during your ”Inverse kinematics” control.

Input: $g_{06}$ (or $T_{06}$ as deﬁned in [1]), the 4×4 homogeneous transformation matrix.
Output: A 6×8 matrix, each column represents one possible solution of joint angles

The function computes the solutions following the Denavit-Hartenberg convention described in [1]. The UR5 schematic is illustarted in Figure 2 for reference.

(a) Description of ”S” and ”T” frames

(b) New RVIZ model with ”S” and ”T” frames

Figure 2: Possible solutions to the inverse kinematics for the simulated start and target frame described above

Resolved Rate Control

The Jacobian matrix determines the relationship between the Cartesian space velocity and joint angular velocity through which we can control the velocity of the end-eﬀector. In resolved rate control, what the joint angles of the robot would be in a speciﬁc time step is what this algorithm controls and calculated based on the error, which is a twist from the target position $t^*$ to the current position t. This algorithm needs iterations to move the end-eﬀector to the target position, and the control function for each iteration is $q_{k+1} = q_k - KT_{step} [J_{st}^b (q_k )]^{-1} \xi_k$ , where $e^{\xi_k} = g_{t * t}$ . Also the body jacobian is used here, because the twist $\xi_k$ is calculated relative to the body frame, which is the end-eﬀector frame, and this algorithm needs to relate the joint angles with the end-eﬀector frame movements. In our algorithm, T step and K are given and constant. Iset T step to be 0.5 and K to be 1.

The resolved rate control needs to avoid the situation where the joints angles make the robot reach a singularity position, because the control function needs the calculated inverse Jacobian which is not available for the singular matrix.

Gradient Descent Control

The gradient descent algorithm is based on the resolved rate control algorithm. The diﬀerence between the current transformation and the desired transformation is transformed into a twist $\xi$ , which is multiplied by the transpose of the Jacobian and the gain K to ﬁnd the desired set of joint velocities. The timestep for moving the joints along those velocities is determined dynamically using the size of the error, so bigger motions will bigger steps and smaller ones will take smaller, more accurate steps. There are two conditions for ending the loop. The ﬁrst is when the error twist reaches a threshold of acceptable distance from the goal (measured in cm and degrees). The second is a safety check using the table constraint function. If the proposed motion would cause the robot to hit the table, the action is canceled, the user is alerted, and the code pauses so the problem can be ﬁxed.

The mathematics of transpose Jacobian control are examined by Chiacchio and Siciliano in their 1989 paper A Closed-Loop Jacobian Transpose Scheme for Solving the Inverse Kinematics of Nonredundant and Redundant Wrists in the Journal of Robotic Systems. They deﬁned the following stability proof:

Deﬁne an error twist $\xi$ from $g_d$ to $g_{st}$ and its derivative and its derivative:

$\xi = -J(q) \dot{q}$

due to the deﬁnition of the Jacobian and its relation between joint velocities and task space velocities. Using a positive gain K, deﬁne a Lyapunov function

$V =\frac{1}{2} \xi^T K\xi$

The derivative of the function is

$V = -\xi^T K^T J(q)\dot{q}$

Choosing $\dot{q} = J^T K\xi$ results in

$V = -T\mid \mid KTJ(q)\mid \mid T$

which is negative deﬁnite, so the system is asymptotically stable.

Chiacchio and Siciliano break up their proof into position and orientation Lyapunov proofs, showing that they are separable and that together they show a convergent system. They caution that there is one exception to this stability is a local minimum in when the orientation is ﬂipped 180 degrees. This is similar to the unstable equilibrium of an inverted pendulum, but as long as the system does not start at such a location, the system should not end up there during its trajectory. In testing this algorithm in simulation, I did ﬁnd this phenomenon occurred, resulting in an inﬁnite loop.

Transpose Jacobian control is not an optimal control law, but it has the advantage over Resolved Rate control that it does not require any matrix inversions. This makes the system less dependent on avoiding singularities and much less computationally expensive.

Video Results

Here is the link of 3 successful pick-and-place implementations using the three diﬀerent methods and the demonstration of the creative part:

References

[1] Ryan Keating. UR5 Inverse Kinematics. 2014. UR5_Inverse_Kinematics

Where is the code?

As always, check out my GitHub:

https://github.com/stytim/UR5_Robot_Arm_Manipulation

If this post helped you, please consider supporting me.