close all;

clc;

clear;

$\%$ Define global variables

global l$1$ l$2$ l$3$ l$4$ l$5$ l$6$

l$1=3\mathrm{.}0$; $\%$ Length of link BC

l$2=4\mathrm{.}0$; $\%$ Length of link AD

l$3=4\mathrm{.}0$; $\%$ Length of link DE

l$4=3\mathrm{.}0$; $\%$ Length of link CE

l$5=2\mathrm{.}0$; $\%$ Length of link EP

l$6=5\mathrm{.}0$; $\%$ Length of link AB

$\%$ Example usage for inverse kinematics

Xp $=-0\mathrm{.}48384$;

Yp $=9\mathrm{.}703$;

$\%$ Call the solve$\_$angles function

$[$theta$1\_$deg, theta$2\_$deg$]=$ solve$\_$angles$($Xp$,$ Yp$)$;

$\%$ Display the results

disp$([\text{'}$Theta$1$ in degrees: $\text{'},$ num$2$str$($theta$1\_$deg$)])$

disp$([\text{'}$Theta$2$ in degrees: $\text{'},$ num$2$str$($theta$2\_$deg$)])$

function $[$theta$1\_$deg, theta$2\_$deg$]=$ solve$\_$angles$($Xp$,$ Yp$)$

global l$1$ l$2$ l$3$ l$4$ l$5$ l$6$

initial$\_$guess $=[$pi$,$ pi$]$;

options $=$ optimoptions$(\text{'}$lsqnonlin$\text{'},$ 'Display', 'iter', 'MaxFunctionEvaluations', $1000,$ 'MaxIterations', $1000)$;

solution $=$ lsqnonlin$($@equations, initial$\_$guess, $[],[],$ options, Xp$,$ Yp$)$;

theta$1=$ solution$(1)$;

theta$2=$ solution$(2)$;

$\%$ Convert radians to degrees

theta$1\_$deg $=$ rad$2$deg$($theta$1)$;

theta$2\_$deg $=$ rad$2$deg$($theta$2)$;

end

function F $=$ equations$($vars$,$ Xp$,$ Yp$)$

global l$1$ l$2$ l$3$ l$4$ l$5$ l$6$

theta$1=$ vars$(1)$;

theta$2=$ vars$(2)$;

$\%$ Calculate positions of joints

A $=[0,0]$;

B $=[$l$6,0]$;

D $=[$l$2*$ cos$($theta$2),$ l$2*$ sin$($theta$2)]$;

C $=[$l$6+$ l$1*$ cos$($theta$1),$ l$1*$ sin$($theta$1)]$;

$\%$ Calculate angle th$4$ using vector loop closure

R$1=[$l$6$; $0]+$ l$1*[$cos$($theta$1)$; sin$($theta$1)]$;

R$2=$ l$2*[$cos$($theta$2)$; sin$($theta$2)]$;

R$3=$ l$3*[$cos$($pi $-$ theta$2)$; sin$($pi $-$ theta$2)]$;

E $=$ R$2+$ R$3$;

$\%$ Calculate the position of the end$-$effector P

P $=$ E $+$ l$5*[$cos$($pi $-$ theta$2)$; sin$($pi $-$ theta$2)]$;

eq$1=$ P$(1)-$ Xp;

eq$2=$ P$(2)-$ Yp;

F $=[$eq$1$; eq$2]$;

end

need to fix with theta$1$ pls$,$ problem from initial$\_$guess