01 Overview
Everything runs in task space: I derived and verified forward and inverse kinematics in MATLAB, calibrated joint encoder offsets, and implemented the control loop in C on a TMS320F28335 DSP via Code Composer Studio, updating every 1 ms (1 kHz). A Simulink interface gave live visualization and on-the-fly gain tuning. The result is a single state machine that sequences five very different sub-tasks, each demanding a different control strategy.
02 The task sequence
Straight-line moves
Cartesian point-to-point motion via task-space PD control, with the Jacobian transpose mapping desired Cartesian forces into joint torques.
Peg insertion
Insert the peg to a marked depth and hold 0.5 s. Z-axis force regulation with deliberately softened gains (impedance tuning) lets it seat smoothly without fighting the hole.
Zig-zag obstacle course
An inverse-dynamics inner loop cancels the arm's nonlinearities; an outer PD + feedforward loop tracks the path. Gains are weakened perpendicular to the zig-zag axis using frame transformations — impedance control in a rotated frame — so the peg stays compliant where it needs to bend.
Egg pressing (500–1000 g)
Force-based impedance control in Z, with the commanded vertical force applied through the Jacobian transpose and friction compensation tuned to stop the arm from "bouncing" on contact.
Return home
A damped task-space trajectory removes contact force and returns to the calibrated rest pose without jerk or overshoot.
03 Control architecture
- Joint-space PD + feedforward — baseline trajectory tracking.
- Friction compensation — applied to every torque command for low jitter.
- Inverse-dynamics control — cancels nonlinear coupling during the zig-zag.
- Task-space PD — peg insertion and obstacle clearance.
- Impedance control — compliant behavior in rotated frames + egg press.
- Z-force via Jacobian transpose — precise contact force on the egg.
04 Results
Achieved sub-centimeter peg alignment and insertion-depth control, navigated the zig-zag with axis-specific impedance shaping, and held egg force within a ±50 g margin — a robust hybrid of model-based and feedback control with low joint-torque jitter thanks to friction compensation.