close all
clear
clc

robot = Robot(0.05);

q = [0.45, -2, 0.3]';

q_nullspace = [0, 0, 0.5]';

while ~robot.complete()
    dx = robot.goal() - robot.pose(q);
    J = robot.jacobian(q);
    Jinv = J' * inv(J * J');
    q = q + Jinv * dx;
    q = q + (eye(3) - Jinv * J) * q_nullspace;
    robot.plot(q)
    robot.inc(0.02);
end