Course Projects

This project involved the design and implementation of a high-performance control system for a CRS robotic manipulator to execute a complex sequence of tasks: precision peg-in-hole insertion, trajectory tracking through a constrained zigzag path, and delicate force control on a fragile object.

The control architecture utilizes Task Space PD Control with Impedance Control principles to manage interaction forces and trajectory tracking. To ensure high fidelity tracking and stability, Friction Compensation and Gravity Compensation were implemented. The control law can be described as:

$$ \tau = J^T F_{task} + \tau_{fric}(\dot{q}) + \tau_{grav}(q) $$

Where \( F_{task} \) is the force calculated in the task space to drive the end-effector to the desired position \( X_{des} \) with desired stiffness \( K_p \) and damping \( K_d \):

$$ F_{task} = K_p (X_{des} - X) + K_d (\dot{X}_{des} - \dot{X}) $$

To achieve precise control in specific directions (e.g., for the zigzag task), the error dynamics were computed in a rotated frame \( N \) aligned with the path constraints:

$$ F_N = K_p^N {}^N R_W (X_{des} - X) + K_d^N {}^N R_W (\dot{X}_{des} - \dot{X}) $$

Task 1: Peg-in-Hole Insertion

The robot executes a precision maneuver starting from the home position. The end-effector approaches the target coordinates, aligns vertically, and performs a controlled insertion. The controller maintains position holding for a dwell time of two seconds before retracting. This task demonstrates the system's positional accuracy and steady-state error minimization.

Task 2: Constrained Zigzag Trajectory

Following the insertion task, the manipulator navigates a 3D-printed zigzag channel. This requires continuous path planning and trajectory generation. A set of 8 waypoints defines the path, with higher density sampling at curvature discontinuities to ensure smooth motion. The impedance controller is tuned to be stiff in the tracking direction while compliant in orthogonal directions to prevent binding.

Task 3: Force Control & Interaction

The final phase involves applying a precise contact force to a fragile object (an egg). The control strategy switches from pure position control to an explicit force control scheme. The stiffness gain in the Z-direction (\( K_{pz} \)) is relaxed to zero, and a feedforward force command (\( F_{z,cmd} \)) is applied:

$$ \tau_{z} = J^T [0, 0, F_{z,cmd}]^T $$

Through experimental tuning, an optimal force command of \( -12.65 \text{ N} \) was identified to maintain the target contact force between \( 500 \sim 1000 \text{ grams} \) without damaging the specimen.

Implementation Challenges

A significant challenge was the seamless transition between stiff position control and compliant force control. The feedforward force term required precise calibration to account for unmodeled dynamics and sensor noise. The final implementation achieved a robust performance window, successfully maintaining the required force range.

Full Motion Demo

Trajectory Code Snippet

const point points[] = {
    {0.15, 0, 0.44, 0 , 1, XYZSTIFF},
    {.027, .357, .44, 0, 1, XYZSTIFF},      // above peg-hole
    {.027, .357, .21, 0, 1, XYZSTIFF},      // right above peg-hole
    {.027, .357, .125, 0, 2, ZSTIFF},       // into the hole
    {.027, .357, .125, 0, 1, ZSTIFF},       // into the hole
    {.027, .357, .44, 0, 2, ZSTIFF},        // above peg-hole
    {.26, .10, .38, 0, 1, XYZSTIFF},        // above egg for avoiding obstale
    {.391, .11, .21, 0, 1, XYZSTIFF},       // before entering zigzag
    {.432, .017, .21, PI / 6, 2, YZSTIFF},  // after zig
    {.322, .038, .21, 0, 2, XZSTIFF},       // before zag
    {.4, -.063, .21, PI / 6, 2, YZSTIFF},   // after zag
    {.39, -.06, .36, 0, 1, ZSTIFF},         // going up
    {.243, .203, .38, 0, 2, XYZSTIFF},      // above egg
    {.243, .203, .285, 0, 5, XYZSTIFF},     // press the egg
    {.243, .203, .285, 0, 2, XYZSTIFF},     // press the egg
    {0.15, 0, 0.44, 0 , 1, XYZSTIFF}        // back to start
};