v_initial = zeros(1, 6);
theta1_i = theta_inital(1);
theta2_i = theta_inital(2);
theta3_i = theta_inital(3);
theta4_i = theta_inital(4);
Jx1_i = - a1*sin(theta1_i) - a2*sin(theta1_i + theta2_i)- a3*sin(theta1_i + theta2_i + theta3_i) - a4*sin(theta1_i + theta2_i + theta3_i + theta4_i);
Jx2_i = - a2*sin(theta1_i + theta2_i) - a3*sin(theta1_i + theta2_i + theta3_i) - a4*sin(theta1_i + theta2_i + theta3_i + theta4_i);
Jx3_i = - a3*sin(theta1_i + theta2_i + theta3_i) - a4*sin(theta1_i + theta2_i + theta3_i + theta4_i);
Jx4_i = - a4*sin(theta1_i + theta2_i + theta3_i + theta4_i);
Jy1_i = a1*cos(theta1_i) + a2*cos(theta1_i + theta2_i) + a3*cos(theta1_i + theta2_i + theta3_i) + a4*cos(theta1_i + theta2_i + theta3_i + theta4_i);
Jy2_i = a2*cos(theta1_i + theta2_i) + a3*cos(theta1_i + theta2_i + theta3_i) + a4*cos(theta1_i + theta2_i + theta3_i + theta4_i);
Jy3_i = a3*cos(theta1_i + theta2_i + theta3_i)+ a4*cos(theta1_i + theta2_i + theta3_i + theta4_i);
Jy4_i = a4*cos(theta1_i + theta2_i + theta3_i + theta4_i);

J_initial = [   Jx1_i   Jx2_i   Jx3_i   Jx4_i
                Jy1_i   Jy2_i   Jy3_i   Jy4_i
                0       0       0       0
                0       0       0       0
                0       0       0       0
                1       1       1       1
                ];