close all
clear
clc

robot = Robot(0.05);

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

while ~robot.complete()
    dx = robot.goal() - robot.pose(q);
    J = robot.jacobian(q);
    Jinv = J' * inv(J * J');
    q_new = q + Jinv * dx;
    if q_new(1) > pi / 4
        J(:, 1) = 0;
        Jinv = J' * inv(J * J');
        q_new = q + Jinv * dx;
    end
    q = q_new;
    robot.plot(q)
    robot.inc(0.02);
end