Universal Robots Forum

Inverse Kinematics of the UR5

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! :slight_smile:

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.

1 Like

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 :slight_smile:

https://robodk.com/doc/en/Robots-Universal-Robots-Copy-Update-Robot-Kinematics.html

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