Inverse Kinematics

CSCI 4302 Lab 1 - Inverse Kinematics

Overview

A UR5e arm stands next to a conveyor belt with a beverage can on it. Your job is to give the arm the motion commands industrial UR robots offer, MoveJ and MoveL, and use them to pick up the can: move above it, go straight down, close the Robotiq gripper and lift.

The lesson covers forward kinematics, numerical inverse kinematics with the Jacobian, joint-space and Cartesian motion, and a simple safety model: a keep-out plane that no joint of the arm may cross. Lab 2 reuses these motion primitives as provided code and moves on to depth perception.

Theoretical Background

Forward Kinematics

Forward kinematics maps the six joint angles \(q = (q_1, \dots, q_6)\) to the pose of the tool, a homogeneous transform:

where \(R\) is the \(3 \times 3\) rotation matrix of the tool and \(p\) the position of the tool center point (TCP), both in the world frame. For a serial arm, \(T\) is a product of fixed link transforms and joint rotations:

The UR5e's link offsets (in meters) are: base to shoulder 0.163 up; shoulder 0.138 sideways; upper arm 0.425 (and 0.131 back sideways at the elbow); forearm 0.392; wrist 1 to wrist 2 0.127; wrist 2 to wrist 3 0.1; TCP 0.1 beyond wrist 3. Joint 1 turns about \(z\), joints 2 to 4 and 6 about \(y\), joint 5 about \(z\). The base itself stands at \((0, -0.15, 0.61)\), turned by \(-\pi/2\) about \(z\). Along the way, the origin of each joint's frame is the joint center used by the safety check.

Inverse Kinematics

Inverse kinematics solves the opposite problem: find joint angles \(q\) that put the tool at a desired pose \(T_d\). A 6-DOF arm usually has several solutions (elbow up or down, wrist flipped), and none in closed form is needed here: we solve it numerically.

The pose error is a 6-vector, the position error and the rotation error as a rotation vector (axis times angle):

The Jacobian \(J(q)\) (\(6 \times 6\)) tells how the tool pose changes for small joint motions, \(\delta x \approx J\, \delta q\). The code estimates it numerically with central differences, one column per joint:

Each iteration takes a damped least-squares step, which behaves well near singularities where \(J\) is nearly not invertible:

with damping \(\lambda\) (IK_DAMPING) and step scale \(\alpha\) (IK_STEP_SCALE). The position rows of \(e\) and \(J\) are weighted more than the orientation rows. The loop stops when the position error is below 1 mm and the orientation error below 2 mrad. If a step would put a joint center across the keep-out plane, the step is halved until it is safe.

Starting the iteration from the current joint angles finds the solution closest to them, so the arm does not swing through a far-away configuration. inverse_kinematics() does this first and only tries other starting points (some joints turned by \(\pm\pi\)) when that fails.

MoveJ

MoveJ is a joint-space motion:

1. solve inverse kinematics for the target pose, starting from the current joints,
2. compute the joint displacement \(\Delta q\) (each angle wrapped to the shorter way round),
3. split it into waypoints no more than joint_speed * dt apart,
4. command the waypoints one control step at a time, checking each against the keep-out plane,
5. wait until the measured joints are within MOVEJ_TOLERANCE of the goal.

The motion is natural for the joints, but the tool does not move along a straight line.

MoveL

MoveL is a Cartesian motion: the tool moves along a straight line.

1. compute the current TCP position with forward kinematics,
2. place waypoints on the line to the target position, no more than cartesian_speed * dt apart, all with the target orientation,
3. call MoveJ for each waypoint.

Since consecutive waypoints are close, each inverse kinematics problem starts next to its solution and converges in a few iterations.

Keep-Out Plane

A plane is \(ax + by + cz + d = 0\). For a point \((x, y, z)\), the sign of \(ax + by + cz + d\) tells on which side of the plane it lies. Here the plane is horizontal at \(z = 0.55\) m with its normal pointing down, so the safe side (\(\le 0\)) is everything above it. isJointPosSafe(joints, plane) returns True only if every joint center (and the TCP) is on the safe side.

Task Description

Objective

Implement in lab1_inverse_kinematics.py:

1. forward_kinematics(joints, include_joint_centers=False): the tool pose \(T(q)\), and with include_joint_centers=True also the positions of the joint centers and the TCP.
2. isJointPosSafe(joints, plane): True when all joint centers are on the safe side of the plane.
3. solve_ik_from_seed(target_pose, seed_joints, safety_plane): the damped least-squares iteration (use pose_error, pose_is_close, damped_least_squares_step and take_safe_ik_step).
4. MoveJ(target_pose, ...): blocking joint-space motion.
5. MoveL(target_pose, ...): blocking Cartesian motion through MoveJ.

main() uses them to pick up the can: MoveJ 28 cm above the grasp pose, MoveL down to 16 cm and then to the grasp pose, close the gripper, MoveL 22 cm up.

System Configuration

• Robot: Universal Robots UR5e on a table, base at \((0, -0.15, 0.61)\) m, with a Robotiq 3F gripper
• Sensors: the six joint position sensors (get_joint_angles(), rad)
• Actuators: six joint motors in position control (set_joint_angles()), the three gripper fingers (open_gripper(), close_gripper())
• Control period: 32 ms (robot.dt)
• The can: on the conveyor at about \((0.394, -0.794, 0.661)\) m; the gripper surrounds it with the tool pointing down and the TCP at the can's position plus \((-0.15, 0, 0)\) m
• Keep-out plane: \(z = 0.55\) m; every joint center must stay above it
• robot.show_target_pose_marker(pose) shows a red sphere at a target position

Performance Metrics

A supervisor watches the can. The run ends when the can is lifted at least 10 cm and held there for 1 s (can_lifted), when the can is pushed 15 cm along the conveyor or tipped over without being lifted, or after 40 s. It writes:

Suggested Algorithm

Build the primitives from the bottom up, checking each before the next:

1. forward_kinematics: at the home pose (all joints 0) the TCP should be near \((0.234, -0.967, 0.673)\) m; main() prints it.
2. isJointPosSafe: the home pose is safe; a pose with the wrist below 0.55 m is not.
3. solve_ik_from_seed: compute $T = $ forward_kinematics(q) for some \(q\), then solve for \(T\) from a nearby seed; forward_kinematics of the result must match \(T\).
4. MoveJ: move above the can; the red marker shows where the grasp pose is.
5. MoveL: go straight down and up.

Diagnostic prints that help:

target_position = target_pose[:3, 3]
current_pose = forward_kinematics(robot.get_joint_angles())
print("target:", np.round(target_position, 3))
print("current:", np.round(current_pose[:3, 3], 3))
print("position error:", np.linalg.norm(target_position - current_pose[:3, 3]))

Validation Checklist

• The printed home tool position matches the expected one.
• Inverse kinematics of a forward-kinematics pose returns joints that reproduce that pose.
• MoveJ brings the arm above the can without swinging through a far-away configuration.
• During MoveL the gripper goes straight down, without sideways drift.
• The fingers close around the can, and the can rises with the gripper.
• No joint center crosses the keep-out plane (print the signed distances when isJointPosSafe rejects a pose).

References

• Universal Robots UR5e in Webots
• Inverse kinematics (Wikipedia)
• S. R. Buss, Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares methods
• N. Correll et al., Introduction to Autonomous Robots, chapter on forward and inverse kinematics
• CSCI 4302 course page

Extension Ideas

• Compute the Jacobian analytically instead of with finite differences and compare the speed.
• Pick up the can with MoveJ only and compare the tool path with MoveL.
• Add a second keep-out plane (for example a wall beside the conveyor) and plan around it.