Project setup

# put the package in the workspace
$ cd ~/catkin_ws/
$ catkin_make
$ source devel/setup.bash


In this project, we program the inverse kinematic algorithm for a robot, which will move its joints so that it follows a desired path with the end-effector.

It is composed of two parts:

  • A 3 DOF scara robot, with an inverse kinematic solution that is possible in analytic form.
  • A 7 DOF kuka robot, with the inverse kinematic solution to be implemented with iterative algorithms.

Scara robot

The following image shows the distances between the joints and the end-effector frame, in the robot’s zero configuration. Two joints (q1 and q2) are revolute, and one (q3) is prismatic. Notice that the end-effector frame and the base frame are at the same height, which means that the end-effector z coordinate coincides with the value of the last prismatic joint (q3).

Kuka robot

This robot has a kinematics structure much more complex than the scara, therefore it is not feasible to obtain an analytic solution for the inverse kinematics problem. The inputs to this function are:

  • point = (x, y, z), the desired position of the end-effector.
  • R = 3x3 rotation matrix, the desired orientation of the end-effector.
  • joint_positions = (q1, q2, q3, q4, q5, q5, q7) the current joint positions.

The output of this function is a vector q containing the 7 joint values that give the desired pose of the end-effector.

This is the DH table of the kuka robot, with the depicted frames:

The DH table follows this convention:

  • a_i distance between z_i-1 and z_i along the axis x_i
  • alpha_i angle between z_i-1 and z_i about the axis x_i
  • d_i distance between x_i-1 and x_i along the axis z_i-1
  • theta_i angle between x_i-1 and x_i about the axis z_i-1

The frame transformation can be found as:


x_hat = K(q_hat)
eps_x = x_hat – x
eps_q = inv(J) @ q_hat @ eps_x
q_hat = q_hat – eps_q

# initial guess
q_hat = q + eps_q
while eps_x > tolerance:
    x_hat = K(q_hat)
    eps_x = x_hat - x
    eps_q = inv(J) @ q_hat @ eps_x
    q_hat = q_hat - eps_q

Run the simulator

Scara robot

$ roslaunch kinematics_assignment scara_launch.launch

Kuka robot

$ roslaunch kinematics_assignment kuka_launch.launch

Simulation result (kuka)


View Github