MATLAB: Simulation with 3 links, deltas and desired position

fminconoptimizationpeter corke toolbox

I would like to make a simulation with the information which is a serial link(by a_new values), q_new values, and T values(desired position). I would like to see newR serial robotic link goes to the first desired position with the q_new_1 and then the newR serial robotic link goes to the second desired position with the q_new_2 and so on. Also I would like to represent the desired position on the plot. Can you help me?
function a_new = manipul(a_initial)
      a_new = fmincon(@rfs, a_initial, [], []);
end
function costFun = rfs(a_initial)
L(1) = Link([0 0 a_initial(1,1) 0]);
L(2) = Link([0 0 a_initial(1,2) 0]);
L(3) = Link([0 0 a_initial(1,3) 0]);
R = SerialLink(L);
T_1 = transl([15,20,0]); 
T_2 = transl([20,25,0]);
T_3 = transl([27,8,0]);
T_4 = transl([23,5,0]);
T_5 = transl([25,3,0]);
q_new_1 = R.ikcon(T_1);  
q_new_2 = R.ikcon(T_2); 
q_new_3 = R.ikcon(T_3);  
q_new_4 = R.ikcon(T_4);  
q_new_5 = R.ikcon(T_5);  
T_real_1 = R.fkine(q_new_1); 
T_real_2 = R.fkine(q_new_2);
T_real_3 = R.fkine(q_new_3);
T_real_4 = R.fkine(q_new_4);
T_real_5 = R.fkine(q_new_5);
costFun = (T_real_1.t(1,1) - T_1(1,4))^2 + (T_real_1.t(2,1) - T_1(2,4))^2 + (T_real_1.t(3,1) - T_1(3,4))^2 +...
    (T_real_2.t(1,1) - T_2(1,4))^2 + (T_real_2.t(2,1) - T_2(2,4))^2 + (T_real_2.t(3,1) - T_2(3,4))^2 + ...
    (T_real_3.t(1,1) - T_3(1,4))^2 + (T_real_3.t(2,1) - T_3(2,4))^2 + (T_real_3.t(3,1) - T_3(3,4))^2 + ...
    (T_real_4.t(1,1) - T_4(1,4))^2 + (T_real_4.t(2,1) - T_4(2,4))^2 + (T_real_4.t(3,1) - T_4(3,4))^2 + ...
    (T_real_5.t(1,1) - T_5(1,4))^2 + (T_real_5.t(2,1) - T_5(2,4))^2 + (T_real_5.t(3,1) - T_5(3,4))^2;
end

Best Answer

 a_new = fmincon(@rfs, a_initial, [], []);
Related Question