Hello I would like to know if there is a MATLAB script available to calculate the inverse kinematics of the UR5 robot? I am trying to calculate it, however there are always some errors in my results. If anyone could provide a functional script, it would be great! 
Kind Regards,
Christian
This may not help you exactly but consider: Return calibrated pose - #9 by abiard
Every robot has calibration data which determines its real forward kinematics.
If you use only the generic robot model your inverse kinematics will always be off vs what the controller says.
Thanks for the suggestion, however I am using RoboDK now due to the ongoing pandemic, maybe I should have specified that! I’ve tried looking some other scripts online, but they all give considerable errors in relation to the expected joint-angles. However, thanks again for the help
Hi there, i have one. it only works for the UR5 and its slow but it works fine. I use a gradient descent method to find the local minima from a error function to find solution (the gradient is calculated in another script so u cant reuse it for other robots but maybe it helps).
Dont be afraid by all the sin and cos that is just the gradient
function [q]=CI_universal(pd,dir,q,maxerror)
%
%%
%Verificacion de pertenencia del punto sobre el espacio de trabajo
%???
%%
%%Gradiente por descenso
nd=dir(:,1)/norm(dir(:,1));
od=dir(:,2)/norm(dir(:,2));
ad=dir(:,3)/norm(dir(:,3));
pdx=pd(1);
pdy=pd(2);
pdz=pd(3);
ndx=nd(1);
ndy=nd(2);
ndz=nd(3);
odx=od(1);
ody=od(2);
odz=od(3);
adx=ad(1);
ady=ad(2);
adz=ad(3);
%Escalamiento de paso
alfa=0.1;
%Vector semilla
q1=q(1);
q2=q(2);
q3=q(3);
q4=q(4);
q5=q(5);
q6=q(6);
maxiter=2800;
posweight=1;
%Actualizacion iterativa
for i=0:maxiter
%Gradiente
E =(cos(q2 + q3 + q4)*sin(q6) - ndz + sin(q2 + q3 + q4)*cos(q5)cos(q6))^2 + (ndx - cos(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) + sin(q2 + q3 + q4)*cos(q1)sin(q6))^2 + (odx + sin(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) + sin(q2 + q3 + q4)*cos(q1)cos(q6))^2 + (ndy + cos(q6)(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) + sin(q2 + q3 + q4)*sin(q1)sin(q6))^2 + (ody - sin(q6)(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) + sin(q2 + q3 + q4)*cos(q6)*sin(q1))^2 + (adx - cos(q5)*sin(q1) + cos(q2 + q3 + q4)*cos(q1)*sin(q5))^2 + (ady + cos(q1)*cos(q5) + cos(q2 + q3 + q4)sin(q1)sin(q5))^2 + (adz + sin(q2 + q3 + q4)sin(q5))^2 + posweight((pdy + (2183cos(q1))/20000 + (823cos(q1)cos(q5))/10000 + (17cos(q2)sin(q1))/40 - (1961sin(q1)*sin(q2)sin(q3))/5000 + (823cos(q2 + q3 + q4)*sin(q1)sin(q5))/10000 - (1893cos(q2 + q3)*sin(q1)sin(q4))/20000 - (1893sin(q2 + q3)*cos(q4)sin(q1))/20000 + (1961cos(q2)cos(q3)sin(q1))/5000)^2 + ((2183sin(q1))/20000 - pdx - (17cos(q1)cos(q2))/40 + (823cos(q5)sin(q1))/10000 - (823cos(q2 + q3 + q4)*cos(q1)sin(q5))/10000 + (1893cos(q2 + q3)*cos(q1)sin(q4))/20000 + (1893sin(q2 + q3)cos(q1)cos(q4))/20000 - (1961cos(q1)cos(q2)cos(q3))/5000 + (1961cos(q1)sin(q2)sin(q3))/5000)^2 + (pdz + (1961sin(q2 + q3))/5000 + (17sin(q2))/40 + sin(q5)((823cos(q2 + q3)sin(q4))/10000 + (823sin(q2 + q3)cos(q4))/10000) + (1893cos(q2 + q3)cos(q4))/20000 - (1893sin(q2 + q3)*sin(q4))/20000 - 6424583026827617/72057594037927936)^2) + (odz - cos(q2 + q3 + q4)*cos(q6) + sin(q2 + q3 + q4)*cos(q5)*sin(q6))^2;
Errpos =abs(pdy + (2183cos(q1))/20000 + (823cos(q1)cos(q5))/10000 + (17cos(q2)sin(q1))/40 - (1961sin(q1)*sin(q2)sin(q3))/5000 + (823cos(q2 + q3 + q4)*sin(q1)sin(q5))/10000 - (1893cos(q2 + q3)*sin(q1)sin(q4))/20000 - (1893sin(q2 + q3)*cos(q4)sin(q1))/20000 + (1961cos(q2)cos(q3)sin(q1))/5000) + abs((2183sin(q1))/20000 - pdx - (17cos(q1)cos(q2))/40 + (823cos(q5)sin(q1))/10000 - (823cos(q2 + q3 + q4)*cos(q1)sin(q5))/10000 + (1893cos(q2 + q3)*cos(q1)sin(q4))/20000 + (1893sin(q2 + q3)cos(q1)cos(q4))/20000 - (1961cos(q1)cos(q2)cos(q3))/5000 + (1961cos(q1)sin(q2)sin(q3))/5000) + abs(pdz + (1961sin(q2 + q3))/5000 + (17sin(q2))/40 + sin(q5)((823cos(q2 + q3)sin(q4))/10000 + (823sin(q2 + q3)cos(q4))/10000) + (1893cos(q2 + q3)cos(q4))/20000 - (1893sin(q2 + q3)*sin(q4))/20000 - 6424583026827617/72057594037927936);
DEq1 =2*(sin(q6)*(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) - sin(q2 + q3 + q4)cos(q6)sin(q1))(odx + sin(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) + sin(q2 + q3 + q4)*cos(q1)cos(q6)) - 2(cos(q5)*sin(q1) - cos(q2 + q3 + q4)cos(q1)sin(q5))(ady + cos(q1)cos(q5) + cos(q2 + q3 + q4)sin(q1)sin(q5)) - posweight(2((2183sin(q1))/20000 - (17cos(q1)cos(q2))/40 + (823cos(q5)sin(q1))/10000 - (823cos(q2 + q3 + q4)*cos(q1)sin(q5))/10000 + (1893cos(q2 + q3)*cos(q1)sin(q4))/20000 + (1893sin(q2 + q3)*cos(q1)cos(q4))/20000 - (1961cos(q1)cos(q2)cos(q3))/5000 + (1961cos(q1)sin(q2)sin(q3))/5000)(pdy + (2183cos(q1))/20000 + (823cos(q1)cos(q5))/10000 + (17cos(q2)sin(q1))/40 - (1961sin(q1)*sin(q2)sin(q3))/5000 + (823cos(q2 + q3 + q4)*sin(q1)sin(q5))/10000 - (1893cos(q2 + q3)*sin(q1)sin(q4))/20000 - (1893sin(q2 + q3)cos(q4)sin(q1))/20000 + (1961cos(q2)cos(q3)sin(q1))/5000) - 2((2183cos(q1))/20000 + (823cos(q1)cos(q5))/10000 + (17cos(q2)sin(q1))/40 - (1961sin(q1)*sin(q2)sin(q3))/5000 + (823cos(q2 + q3 + q4)*sin(q1)sin(q5))/10000 - (1893cos(q2 + q3)*sin(q1)sin(q4))/20000 - (1893sin(q2 + q3)cos(q4)sin(q1))/20000 + (1961cos(q2)cos(q3)sin(q1))/5000)((2183sin(q1))/20000 - pdx - (17cos(q1)cos(q2))/40 + (823cos(q5)sin(q1))/10000 - (823cos(q2 + q3 + q4)*cos(q1)sin(q5))/10000 + (1893cos(q2 + q3)*cos(q1)sin(q4))/20000 + (1893sin(q2 + q3)*cos(q1)cos(q4))/20000 - (1961cos(q1)*cos(q2)cos(q3))/5000 + (1961cos(q1)sin(q2)sin(q3))/5000)) - 2(cos(q6)(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) + sin(q2 + q3 + q4)sin(q1)sin(q6))(ndx - cos(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) + sin(q2 + q3 + q4)cos(q1)sin(q6)) - 2(cos(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) - sin(q2 + q3 + q4)cos(q1)sin(q6))(ndy + cos(q6)(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) + sin(q2 + q3 + q4)*sin(q1)sin(q6)) - 2(cos(q1)*cos(q5) + cos(q2 + q3 + q4)*sin(q1)sin(q5))(adx - cos(q5)*sin(q1) + cos(q2 + q3 + q4)cos(q1)sin(q5)) + 2(sin(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) + sin(q2 + q3 + q4)cos(q1)cos(q6))(ody - sin(q6)(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) + sin(q2 + q3 + q4)*cos(q6)*sin(q1));
DEq2 =2*(cos(q2 + q3 + q4)*sin(q1)*sin(q6) + sin(q2 + q3 + q4)*cos(q5)cos(q6)sin(q1))(ndy + cos(q6)(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) + sin(q2 + q3 + q4)*sin(q1)sin(q6)) + 2(cos(q2 + q3 + q4)*cos(q6)*sin(q1) - sin(q2 + q3 + q4)*cos(q5)sin(q1)sin(q6))(ody - sin(q6)(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) + sin(q2 + q3 + q4)*cos(q6)sin(q1)) + 2(sin(q2 + q3 + q4)*cos(q6) + cos(q2 + q3 + q4)*cos(q5)sin(q6))(odz - cos(q2 + q3 + q4)*cos(q6) + sin(q2 + q3 + q4)*cos(q5)sin(q6)) + 2(cos(q2 + q3 + q4)*cos(q1)*sin(q6) + sin(q2 + q3 + q4)*cos(q1)cos(q5)cos(q6))(ndx - cos(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)cos(q1)cos(q5)) + sin(q2 + q3 + q4)cos(q1)sin(q6)) + 2(cos(q2 + q3 + q4)cos(q1)cos(q6) - sin(q2 + q3 + q4)cos(q1)cos(q5)sin(q6))(odx + sin(q6)(sin(q1)sin(q5) + cos(q2 + q3 + q4)cos(q1)cos(q5)) + sin(q2 + q3 + q4)cos(q1)cos(q6)) + posweight(2((1961cos(q2 + q3))/5000 + (17cos(q2))/40 + sin(q5)((823cos(q2 + q3)cos(q4))/10000 - (823sin(q2 + q3)sin(q4))/10000) - (1893cos(q2 + q3)sin(q4))/20000 - (1893sin(q2 + q3)cos(q4))/20000)(pdz + (1961sin(q2 + q3))/5000 + (17sin(q2))/40 + sin(q5)((823cos(q2 + q3)sin(q4))/10000 + (823sin(q2 + q3)cos(q4))/10000) + (1893cos(q2 + q3)cos(q4))/20000 - (1893sin(q2 + q3)sin(q4))/20000 - 6424583026827617/72057594037927936) + 2((17cos(q1)sin(q2))/40 + (823sin(q2 + q3 + q4)*cos(q1)sin(q5))/10000 + (1893cos(q2 + q3)*cos(q1)cos(q4))/20000 - (1893sin(q2 + q3)*cos(q1)sin(q4))/20000 + (1961cos(q1)cos(q2)sin(q3))/5000 + (1961cos(q1)cos(q3)sin(q2))/5000)((2183sin(q1))/20000 - pdx - (17cos(q1)cos(q2))/40 + (823cos(q5)sin(q1))/10000 - (823cos(q2 + q3 + q4)*cos(q1)sin(q5))/10000 + (1893cos(q2 + q3)*cos(q1)sin(q4))/20000 + (1893sin(q2 + q3)*cos(q1)cos(q4))/20000 - (1961cos(q1)*cos(q2)cos(q3))/5000 + (1961cos(q1)sin(q2)sin(q3))/5000) - 2((17sin(q1)sin(q2))/40 + (823sin(q2 + q3 + q4)*sin(q1)sin(q5))/10000 + (1893cos(q2 + q3)*cos(q4)sin(q1))/20000 - (1893sin(q2 + q3)*sin(q1)sin(q4))/20000 + (1961cos(q2)sin(q1)sin(q3))/5000 + (1961cos(q3)sin(q1)sin(q2))/5000)(pdy + (2183cos(q1))/20000 + (823cos(q1)cos(q5))/10000 + (17cos(q2)sin(q1))/40 - (1961sin(q1)*sin(q2)sin(q3))/5000 + (823cos(q2 + q3 + q4)*sin(q1)sin(q5))/10000 - (1893cos(q2 + q3)*sin(q1)sin(q4))/20000 - (1893sin(q2 + q3)*cos(q4)sin(q1))/20000 + (1961cos(q2)*cos(q3)sin(q1))/5000)) - 2(sin(q2 + q3 + q4)*sin(q6) - cos(q2 + q3 + q4)*cos(q5)cos(q6))(cos(q2 + q3 + q4)*sin(q6) - ndz + sin(q2 + q3 + q4)*cos(q5)cos(q6)) + 2cos(q2 + q3 + q4)sin(q5)(adz + sin(q2 + q3 + q4)sin(q5)) - 2sin(q2 + q3 + q4)*cos(q1)sin(q5)(adx - cos(q5)*sin(q1) + cos(q2 + q3 + q4)*cos(q1)sin(q5)) - 2sin(q2 + q3 + q4)*sin(q1)sin(q5)(ady + cos(q1)*cos(q5) + cos(q2 + q3 + q4)*sin(q1)*sin(q5));
DEq3 =2*(cos(q2 + q3 + q4)*sin(q1)*sin(q6) + sin(q2 + q3 + q4)*cos(q5)cos(q6)sin(q1))(ndy + cos(q6)(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)sin(q1)) + sin(q2 + q3 + q4)sin(q1)sin(q6)) + 2(cos(q2 + q3 + q4)cos(q6)sin(q1) - sin(q2 + q3 + q4)cos(q5)sin(q1)sin(q6))(ody - sin(q6)(cos(q1)sin(q5) - cos(q2 + q3 + q4)cos(q5)sin(q1)) + sin(q2 + q3 + q4)cos(q6)sin(q1)) + posweight(2((1961cos(q2 + q3))/5000 + sin(q5)((823cos(q2 + q3)cos(q4))/10000 - (823sin(q2 + q3)sin(q4))/10000) - (1893cos(q2 + q3)sin(q4))/20000 - (1893sin(q2 + q3)cos(q4))/20000)(pdz + (1961sin(q2 + q3))/5000 + (17sin(q2))/40 + sin(q5)((823cos(q2 + q3)sin(q4))/10000 + (823sin(q2 + q3)cos(q4))/10000) + (1893cos(q2 + q3)cos(q4))/20000 - (1893sin(q2 + q3)sin(q4))/20000 - 6424583026827617/72057594037927936) + 2((823sin(q2 + q3 + q4)*cos(q1)sin(q5))/10000 + (1893cos(q2 + q3)*cos(q1)cos(q4))/20000 - (1893sin(q2 + q3)*cos(q1)sin(q4))/20000 + (1961cos(q1)cos(q2)sin(q3))/5000 + (1961cos(q1)cos(q3)sin(q2))/5000)((2183sin(q1))/20000 - pdx - (17cos(q1)cos(q2))/40 + (823cos(q5)sin(q1))/10000 - (823cos(q2 + q3 + q4)*cos(q1)sin(q5))/10000 + (1893cos(q2 + q3)*cos(q1)sin(q4))/20000 + (1893sin(q2 + q3)*cos(q1)cos(q4))/20000 - (1961cos(q1)*cos(q2)cos(q3))/5000 + (1961cos(q1)sin(q2)sin(q3))/5000) - 2((823sin(q2 + q3 + q4)*sin(q1)sin(q5))/10000 + (1893cos(q2 + q3)*cos(q4)sin(q1))/20000 - (1893sin(q2 + q3)*sin(q1)sin(q4))/20000 + (1961cos(q2)sin(q1)sin(q3))/5000 + (1961cos(q3)sin(q1)sin(q2))/5000)(pdy + (2183cos(q1))/20000 + (823cos(q1)cos(q5))/10000 + (17cos(q2)sin(q1))/40 - (1961sin(q1)*sin(q2)sin(q3))/5000 + (823cos(q2 + q3 + q4)*sin(q1)sin(q5))/10000 - (1893cos(q2 + q3)*sin(q1)sin(q4))/20000 - (1893sin(q2 + q3)*cos(q4)sin(q1))/20000 + (1961cos(q2)*cos(q3)sin(q1))/5000)) + 2(sin(q2 + q3 + q4)*cos(q6) + cos(q2 + q3 + q4)*cos(q5)sin(q6))(odz - cos(q2 + q3 + q4)*cos(q6) + sin(q2 + q3 + q4)*cos(q5)sin(q6)) + 2(cos(q2 + q3 + q4)*cos(q1)*sin(q6) + sin(q2 + q3 + q4)*cos(q1)cos(q5)cos(q6))(ndx - cos(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) + sin(q2 + q3 + q4)*cos(q1)sin(q6)) + 2(cos(q2 + q3 + q4)*cos(q1)*cos(q6) - sin(q2 + q3 + q4)*cos(q1)cos(q5)sin(q6))(odx + sin(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) + sin(q2 + q3 + q4)*cos(q1)cos(q6)) - 2(sin(q2 + q3 + q4)*sin(q6) - cos(q2 + q3 + q4)*cos(q5)cos(q6))(cos(q2 + q3 + q4)*sin(q6) - ndz + sin(q2 + q3 + q4)*cos(q5)cos(q6)) + 2cos(q2 + q3 + q4)sin(q5)(adz + sin(q2 + q3 + q4)sin(q5)) - 2sin(q2 + q3 + q4)*cos(q1)sin(q5)(adx - cos(q5)*sin(q1) + cos(q2 + q3 + q4)*cos(q1)sin(q5)) - 2sin(q2 + q3 + q4)*sin(q1)sin(q5)(ady + cos(q1)*cos(q5) + cos(q2 + q3 + q4)*sin(q1)*sin(q5));
DEq4 =2*(cos(q2 + q3 + q4)*sin(q1)*sin(q6) + sin(q2 + q3 + q4)*cos(q5)cos(q6)sin(q1))(ndy + cos(q6)(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) + sin(q2 + q3 + q4)sin(q1)sin(q6)) + 2(cos(q2 + q3 + q4)cos(q6)sin(q1) - sin(q2 + q3 + q4)cos(q5)sin(q1)sin(q6))(ody - sin(q6)(cos(q1)sin(q5) - cos(q2 + q3 + q4)cos(q5)sin(q1)) + sin(q2 + q3 + q4)cos(q6)sin(q1)) - posweight(2((1893cos(q2 + q3)sin(q4))/20000 - sin(q5)((823cos(q2 + q3)cos(q4))/10000 - (823sin(q2 + q3)sin(q4))/10000) + (1893sin(q2 + q3)cos(q4))/20000)(pdz + (1961sin(q2 + q3))/5000 + (17sin(q2))/40 + sin(q5)((823cos(q2 + q3)sin(q4))/10000 + (823sin(q2 + q3)cos(q4))/10000) + (1893cos(q2 + q3)cos(q4))/20000 - (1893sin(q2 + q3)sin(q4))/20000 - 6424583026827617/72057594037927936) - 2((823sin(q2 + q3 + q4)*cos(q1)sin(q5))/10000 + (1893cos(q2 + q3)cos(q1)cos(q4))/20000 - (1893sin(q2 + q3)cos(q1)sin(q4))/20000)((2183sin(q1))/20000 - pdx - (17cos(q1)cos(q2))/40 + (823cos(q5)sin(q1))/10000 - (823cos(q2 + q3 + q4)*cos(q1)sin(q5))/10000 + (1893cos(q2 + q3)*cos(q1)sin(q4))/20000 + (1893sin(q2 + q3)*cos(q1)cos(q4))/20000 - (1961cos(q1)*cos(q2)cos(q3))/5000 + (1961cos(q1)sin(q2)sin(q3))/5000) + 2((823sin(q2 + q3 + q4)*sin(q1)sin(q5))/10000 + (1893cos(q2 + q3)cos(q4)sin(q1))/20000 - (1893sin(q2 + q3)sin(q1)sin(q4))/20000)(pdy + (2183cos(q1))/20000 + (823cos(q1)cos(q5))/10000 + (17cos(q2)sin(q1))/40 - (1961sin(q1)*sin(q2)sin(q3))/5000 + (823cos(q2 + q3 + q4)*sin(q1)sin(q5))/10000 - (1893cos(q2 + q3)*sin(q1)sin(q4))/20000 - (1893sin(q2 + q3)*cos(q4)sin(q1))/20000 + (1961cos(q2)*cos(q3)sin(q1))/5000)) + 2(sin(q2 + q3 + q4)*cos(q6) + cos(q2 + q3 + q4)*cos(q5)sin(q6))(odz - cos(q2 + q3 + q4)*cos(q6) + sin(q2 + q3 + q4)*cos(q5)sin(q6)) + 2(cos(q2 + q3 + q4)*cos(q1)*sin(q6) + sin(q2 + q3 + q4)*cos(q1)cos(q5)cos(q6))(ndx - cos(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) + sin(q2 + q3 + q4)*cos(q1)sin(q6)) + 2(cos(q2 + q3 + q4)*cos(q1)*cos(q6) - sin(q2 + q3 + q4)*cos(q1)cos(q5)sin(q6))(odx + sin(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) + sin(q2 + q3 + q4)*cos(q1)cos(q6)) - 2(sin(q2 + q3 + q4)*sin(q6) - cos(q2 + q3 + q4)*cos(q5)cos(q6))(cos(q2 + q3 + q4)*sin(q6) - ndz + sin(q2 + q3 + q4)*cos(q5)cos(q6)) + 2cos(q2 + q3 + q4)sin(q5)(adz + sin(q2 + q3 + q4)sin(q5)) - 2sin(q2 + q3 + q4)*cos(q1)sin(q5)(adx - cos(q5)*sin(q1) + cos(q2 + q3 + q4)*cos(q1)sin(q5)) - 2sin(q2 + q3 + q4)*sin(q1)sin(q5)(ady + cos(q1)*cos(q5) + cos(q2 + q3 + q4)*sin(q1)*sin(q5));
DEq5 =2*(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)cos(q5))(adx - cos(q5)*sin(q1) + cos(q2 + q3 + q4)*cos(q1)sin(q5)) - 2(cos(q1)*sin(q5) - cos(q2 + q3 + q4)cos(q5)sin(q1))(ady + cos(q1)cos(q5) + cos(q2 + q3 + q4)sin(q1)sin(q5)) - posweight(2((823sin(q1)sin(q5))/10000 + (823cos(q2 + q3 + q4)cos(q1)cos(q5))/10000)((2183sin(q1))/20000 - pdx - (17cos(q1)cos(q2))/40 + (823cos(q5)sin(q1))/10000 - (823cos(q2 + q3 + q4)*cos(q1)sin(q5))/10000 + (1893cos(q2 + q3)cos(q1)sin(q4))/20000 + (1893sin(q2 + q3)cos(q1)cos(q4))/20000 - (1961cos(q1)cos(q2)cos(q3))/5000 + (1961cos(q1)sin(q2)sin(q3))/5000) + 2((823cos(q1)sin(q5))/10000 - (823cos(q2 + q3 + q4)cos(q5)sin(q1))/10000)(pdy + (2183cos(q1))/20000 + (823cos(q1)cos(q5))/10000 + (17cos(q2)sin(q1))/40 - (1961sin(q1)sin(q2)sin(q3))/5000 + (823cos(q2 + q3 + q4)sin(q1)sin(q5))/10000 - (1893cos(q2 + q3)sin(q1)sin(q4))/20000 - (1893sin(q2 + q3)cos(q4)sin(q1))/20000 + (1961cos(q2)cos(q3)sin(q1))/5000) - 2cos(q5)((823cos(q2 + q3)sin(q4))/10000 + (823sin(q2 + q3)cos(q4))/10000)(pdz + (1961sin(q2 + q3))/5000 + (17sin(q2))/40 + sin(q5)((823cos(q2 + q3)sin(q4))/10000 + (823sin(q2 + q3)cos(q4))/10000) + (1893cos(q2 + q3)cos(q4))/20000 - (1893sin(q2 + q3)sin(q4))/20000 - 6424583026827617/72057594037927936)) + 2sin(q2 + q3 + q4)cos(q5)(adz + sin(q2 + q3 + q4)sin(q5)) - 2cos(q6)(cos(q5)*sin(q1) - cos(q2 + q3 + q4)cos(q1)sin(q5))(ndx - cos(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) + sin(q2 + q3 + q4)cos(q1)sin(q6)) + 2cos(q6)(cos(q1)*cos(q5) + cos(q2 + q3 + q4)sin(q1)sin(q5))(ndy + cos(q6)(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) + sin(q2 + q3 + q4)sin(q1)sin(q6)) + 2sin(q6)(cos(q5)*sin(q1) - cos(q2 + q3 + q4)cos(q1)sin(q5))(odx + sin(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) + sin(q2 + q3 + q4)cos(q1)cos(q6)) - 2sin(q6)(cos(q1)*cos(q5) + cos(q2 + q3 + q4)sin(q1)sin(q5))(ody - sin(q6)(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) + sin(q2 + q3 + q4)*cos(q6)sin(q1)) - 2sin(q2 + q3 + q4)*sin(q5)sin(q6)(odz - cos(q2 + q3 + q4)*cos(q6) + sin(q2 + q3 + q4)*cos(q5)sin(q6)) - 2sin(q2 + q3 + q4)*cos(q6)sin(q5)(cos(q2 + q3 + q4)*sin(q6) - ndz + sin(q2 + q3 + q4)*cos(q5)*cos(q6));
DEq6 =2*(cos(q2 + q3 + q4)*sin(q6) + sin(q2 + q3 + q4)*cos(q5)cos(q6))(odz - cos(q2 + q3 + q4)*cos(q6) + sin(q2 + q3 + q4)cos(q5)sin(q6)) + 2(sin(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) + sin(q2 + q3 + q4)cos(q1)cos(q6))(ndx - cos(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) + sin(q2 + q3 + q4)cos(q1)sin(q6)) + 2(cos(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) - sin(q2 + q3 + q4)cos(q1)sin(q6))(odx + sin(q6)(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) + sin(q2 + q3 + q4)cos(q1)cos(q6)) - 2(sin(q6)(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) - sin(q2 + q3 + q4)cos(q6)sin(q1))(ndy + cos(q6)(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) + sin(q2 + q3 + q4)sin(q1)sin(q6)) - 2(cos(q6)(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) + sin(q2 + q3 + q4)sin(q1)sin(q6))(ody - sin(q6)(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) + sin(q2 + q3 + q4)*cos(q6)sin(q1)) + 2(cos(q2 + q3 + q4)*cos(q6) - sin(q2 + q3 + q4)*cos(q5)sin(q6))(cos(q2 + q3 + q4)*sin(q6) - ndz + sin(q2 + q3 + q4)*cos(q5)*cos(q6));
q1 = q1 - alfa*DEq1;
q2 = q2 - alfa*DEq2;
q3 = q3 - alfa*DEq3;
q4 = q4 - alfa*DEq4;
q5 = q5 - alfa*DEq5;
q6 = q6 - alfa*DEq6;
E =(cos(q2 + q3 + q4)*sin(q6) - ndz + sin(q2 + q3 + q4)*cos(q5)*cos(q6))^2 + (ndx - cos(q6)*(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) + sin(q2 + q3 + q4)*cos(q1)*sin(q6))^2 + (odx + sin(q6)*(sin(q1)*sin(q5) + cos(q2 + q3 + q4)*cos(q1)*cos(q5)) + sin(q2 + q3 + q4)*cos(q1)*cos(q6))^2 + (ndy + cos(q6)*(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) + sin(q2 + q3 + q4)*sin(q1)*sin(q6))^2 + (pdy + (2621*cos(q1))/20000 + (921*cos(q1)*cos(q5))/10000 + (4871*cos(q2)*sin(q1))/20000 - (533*sin(q1)*sin(q2)*sin(q3))/2500 + (921*cos(q2 + q3 + q4)*sin(q1)*sin(q5))/10000 - (1707*cos(q2 + q3)*sin(q1)*sin(q4))/20000 - (1707*sin(q2 + q3)*cos(q4)*sin(q1))/20000 + (533*cos(q2)*cos(q3)*sin(q1))/2500)^2 + (ody - sin(q6)*(cos(q1)*sin(q5) - cos(q2 + q3 + q4)*cos(q5)*sin(q1)) + sin(q2 + q3 + q4)*cos(q6)*sin(q1))^2 + (adx - cos(q5)*sin(q1) + cos(q2 + q3 + q4)*cos(q1)*sin(q5))^2 + (ady + cos(q1)*cos(q5) + cos(q2 + q3 + q4)*sin(q1)*sin(q5))^2 + ((2621*sin(q1))/20000 - pdx - (4871*cos(q1)*cos(q2))/20000 + (921*cos(q5)*sin(q1))/10000 - (921*cos(q2 + q3 + q4)*cos(q1)*sin(q5))/10000 + (1707*cos(q2 + q3)*cos(q1)*sin(q4))/20000 + (1707*sin(q2 + q3)*cos(q1)*cos(q4))/20000 - (533*cos(q1)*cos(q2)*cos(q3))/2500 + (533*cos(q1)*sin(q2)*sin(q3))/2500)^2 + (adz + sin(q2 + q3 + q4)*sin(q5))^2 + (odz - cos(q2 + q3 + q4)*cos(q6) + sin(q2 + q3 + q4)*cos(q5)*sin(q6))^2 + (pdz + (533*sin(q2 + q3))/2500 + (4871*sin(q2))/20000 + sin(q5)*((921*cos(q2 + q3)*sin(q4))/10000 + (921*sin(q2 + q3)*cos(q4))/10000) + (1707*cos(q2 + q3)*cos(q4))/20000 - (1707*sin(q2 + q3)*sin(q4))/20000 - 3037/20000)^2;
if E<= maxerror
break;
end
if i==maxiter-1
'Se supero el maximo de iteraciones, el valor del error es:'
E
%'Error de posicionamiento:'
%Errpos
end
end
q=[q1,q2,q3,q4,q5,q6];
end
if u really want something useful in MATLAB for any kind of robots i recommend u to use the Peter Corke Toolbox. Is has a lot of functions an u can visualize your robot as well as create your own robot using DH parameters
Hello, I am looking for the same Matlab script so i would be very thankful if you share with me your script
Thank you