PR2 Kinematics Pick-and-Place

What it did

The Kinematics module’s capstone in the Robotics nanodegree. Given a target end-effector pose (position + orientation) and the PR2’s 7-link arm, solve the inverse kinematics analytically: compute the six joint angles that put the gripper exactly there. Drive the resulting joint trajectory via ROS + MoveIt! to execute pick-and-place.

The math

PR2’s arm has a “spherical wrist” — the last three axes intersect at a single point. That decoupling lets you split IK into two solvable sub-problems:

1. Wrist center. Subtract the offset along the gripper z-axis from the target position. Solve θ1, θ2, θ3 geometrically (law of cosines on the upper arm + forearm triangle).
2. Wrist orientation. Compute R_3_6 = inv(R_0_3) · R_target via Sympy; extract θ4, θ5, θ6 from the rotation matrix.

Derive the DH parameters once (a, α, d, θ offset per link), then forward-kinematics via T = T1 · T2 · ... · T7 for verification.

What was actually tricky

• DH conventions are not standardized. Modified vs. classical DH; both give correct FK but produce different IK derivations. Half the project is matching the textbook’s convention to the URDF’s link layout.
• Singularities at θ5 = 0. When the wrist is “straight,” θ4 and θ6 become coupled — the IK has infinite solutions. Need fallback logic to pick the “elbow-up” or “elbow-down” branch consistently.
• Sympy is slow. Doing the symbolic inversion at request time is too slow for real-time control. Solution: lambdify the expressions ahead of time into numeric functions.

What I’d do differently with hindsight

• Use a numerical IK solver (e.g., TRAC-IK, KDL) instead of rolling analytical IK. They’re well-tested, handle singularities, and are within a few ms of the analytical solution.
• MoveIt! already does this. Half this project is re-implementing what move_group provides out of the box. The pedagogical point is “understand what’s happening under the hood,” but in production nobody hand-rolls IK for a 6/7-DOF arm.
• Add a Cartesian-path planner for the linear-pull motion (away from the bin). Pure joint-space planning produces curved arm trajectories that knock other objects over.

What it taught me

Forward kinematics is multiplication; inverse kinematics is craft. The “right” IK depends on the arm’s geometry (decoupled wrist or not), the singularity behavior you want, and how much you trust numerical solvers. Most of robotics is like this: the textbook answer exists, and then the engineering answer reshapes it for the actual robot.