Geometric_jacobian.m

function J = Geometric_jacobian(q)
q1 = q(1);
q2 = q(2);
q3 = q(3);

J = [ - sin(q1 + q2 + q3) - sin(q1 + q2) - sin(q1), - sin(q1 + q2 + q3) - sin(q1 + q2), -sin(q1 + q2 + q3);
        cos(q1 + q2 + q3) + cos(q1 + q2) + cos(q1),   cos(q1 + q2 + q3) + cos(q1 + q2),  cos(q1 + q2 + q3)];
end