Overview
The Kinova Gen3 has seven joints, so every Cartesian target has many joint solutions and singularities are never far away. Instead of a toolbox IK object, this project implements the solver from first principles and plans smooth motion between the results.
Five waypoints with the gripper pointing straight down are solved, joined with cubic polynomials and animated. The derivations are written up in the repository's algorithm notes, and every module is covered by tests.
How it works
Pose error
Each iteration builds a 6D error vector from the orientation and position difference between the current and the target end-effector pose.
Damped least squares
The joint update uses the damped pseudoinverse Jᵀ(JJᵀ + λ²I)⁻¹, which stays stable near singularities, with a step size α and joint-limit clamping after every step.
Cubic segments
Each joint-space segment is a cubic q(t) = a₀ + a₁t + a₂t² + a₃t³ fitted to position and velocity at both ends, with interior velocities from central differences.
Check and animate
Position errors are reported per waypoint against a 1 cm threshold, then the full 240-frame motion plays in the Robotics System Toolbox visualiser.
What's next
- Interpolating orientation between waypoints
- Collision checking, plus velocity and torque limits
- Natural cubic splines for continuous acceleration
- Warm-starting each IK solve from the previous waypoint