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 first “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 different 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.


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” configuration that I decide is best. I implement the control trajectory in three different ways:

  1. Inverse kinematics. Using inverse kinematics, move the robot along the desired Cartesian path by finding the corresponding path in joint space.
  2. Rate control using differential kinematics. Find the desired velocity (or small, differntial Cartesian offset), and “pull that back” through the inverse of the Jacobian to find the joint space velocity (increment) that moves in the right direction.
  3. 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 specification of the gripper is available online. In our setup, this gripper is attached to the UR5 end-effector, and controlled by UR5 tool digital output.


Use these functions to control the gripper:

  1. ur5.init gripper Run this function before sending commands to initialize the gripper.
  2. ur5.open gripper Open the gripper
  3. 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.

  1. Input: g_{06} (or T_{06} as defined in [1]), the 4×4 homogeneous transformation matrix.
  2. 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.

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-effector. In resolved rate control, what the joint angles of the robot would be in a specific 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-effector 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-effector frame, and this algorithm needs to relate the joint angles with the end-effector 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 difference 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 find 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 first 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 fixed.

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 defined the following stability proof:

Define an error twist \xi from g_d to g_{st} and its derivative and its derivative:

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

due to the definition of the Jacobian and its relation between joint velocities and task space velocities. Using a positive gain K, define 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 definite, 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 flipped 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 find this phenomenon occurred, resulting in an infinite 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 different methods and the demonstration of the creative part:


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

Where is the code?

As always, check out my GitHub:


Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s