Robot Arm

William Rice

Spring 2025

For the final project in ME3460 I was tasked to program and simulate a robotic arm capable of performing a full pick-and-place task: reaching down to grab an object, lifting it, moving it across its workspace, and placing it precisely onto a pedestal. The robot we used in class was the PincherX-100, a 4-DOF serial arm. The Inverse Kinematics and Trajectory Generation for my project was written by myself.

Inverse Kinematics from Scratch

Forward kinematics is generally straightforward, given joint angles, multiply through your transformation matrices and you get the end-effector pose. Inverse kinematics is the harder and more interesting problem: given a target position and orientation in space what are the joint angles do you need?

To do this I worked on an analytical IK solution specific to the PincherX-100’s geometry. Starting from the full 4x4 transformation matrix of the end-effector pose, I factored out the known tool transform to isolate the wrist position, then solved for each joint angle geometrically.

The key steps were:

1. θ₁ — solved directly from the horizontal position of the wrist using atan2
2. θ₃ — solved using the law of cosines on the two-link planar arm formed by links 2 and 3
3. θ₂ — solved by decomposing the reach angle into a geometry angle and the elbow angle
4. θ₄ — recovered from the remaining rotation in the transformation after subtracting θ₂ and θ₃

function joint_angles = compute_aik(ee_pose)
    T4E = [1 0 0 0.1136; 0 0 -1 0; 0 1 0 0; 0 0 0 1];
    T04 = ee_pose * inv(T4E);

    l1 = 0.0931; l2 = 0.1059; l3 = 0.1;

    r    = sqrt(T04(1,4)^2 + T04(2,4)^2);
    z    = T04(3,4) - l1;
    c3   = (r^2 + z^2 - l2^2 - l3^2) / (2*l2*l3);
    beta = atan2(z, r);
    psi  = atan2(l3*sqrt(1 - c3^2), l2 + l3*c3);

    theta1 = atan2(-T04(1,3),  T04(2,3));
    theta2 = beta + psi;
    theta3 = atan2(-sqrt(1 - c3^2), c3);
    theta4 = atan2(-T04(3,1), -T04(3,2)) - theta2 - theta3;

    joint_angles = [theta1; -theta2+1.2341; -theta3-1.2341; -theta4];
end

Trajectory Generation and the Pick-and-Place Algorithm

With a working IK solver, the next challenge was planning the motion. The pick-and-place task was broken into a sequence of waypoints:

Home → Approach Pick → Pick → Approach Pick → Approach Drop → Drop → Approach Drop

The trajectory between any two waypoints is generated by a simple and effective algorithm: take the vector between the start and end position, compute how many steps are needed so that no single step exceeds the maximum allowed joint movement, then interpolate linearly. Each intermediate position is passed to compute_aik to get the corresponding joint angles.

PID Controller Tuning

To actually drive the joints, we implemented and tuned a PID controller. I tested six configurations, varying Kp, Ki, and Kv independently to understand their effects:

Controller 6 (Kp = 640, Kv = 4000) was the clear winner with no steady-state error, negligible overshoot, and sub-second settling time.

Reflections and What’s Next

This project was my real introduction to robotics. Working all the way from raw DH parameter math to the actual physical hardware gave me a deep understanding of how robot arms work and has peaked my interest for future robotics projects of my own.