Code covered by the BSD License

# Input-Output Linearization of planar 3-link manipulator

### Vikash Gupta (view profile)

[theta_i, theta_dot_i]=invkine(Xi,Zi,phi_i,cartesianVelo_initial)
```%% Inverse Kinematics

function [theta_i, theta_dot_i]=invkine(Xi,Zi,phi_i,cartesianVelo_initial)

x=Xi;
z=Zi;
phi=phi_i;
global a;

theta_i(2)= acos(((x-a(3)*cos(phi))^2 + (z - a(3)*sin(phi))^2 - a(1)^2 - a(2)^2)/(2*a(1)*a(2)));

k=a(1)+a(2)*cos(theta_i(2));
X=x-a(3)*cos(phi);
Z=z-a(3)*sin(phi);

theta_i(1)=atan2((k*Z-a(2)*sin(theta_i(2))*X),(k*X+a(2)*sin(theta_i(2))*Z));

% theta_i(1)=acos(1/sqrt(1+k^2));
theta_i(3)=phi-theta_i(1)-theta_i(2);

invJ =[
cos(theta_i(1) + theta_i(2))/(a(1)*sin(theta_i(2))),                           sin(theta_i(1) + theta_i(2))/(a(1)*sin(theta_i(2))),                                    (a(3)*sin(theta_i(3)))/(a(1)*sin(theta_i(2)));
-(a(2)*cos(theta_i(1) + theta_i(2)) + a(1)*cos(theta_i(1)))/(a(1)*a(2)*sin(theta_i(2))), -(a(2)*sin(theta_i(1) + theta_i(2)) + a(1)*sin(theta_i(1)))/(a(1)*a(2)*sin(theta_i(2))), -(a(3)*(a(1)*sin(theta_i(2) + theta_i(3)) + a(2)*sin(theta_i(3))))/(a(1)*a(2)*sin(theta_i(2)));
cos(theta_i(1))/(a(2)*sin(theta_i(2))),                                    sin(theta_i(1))/(a(2)*sin(theta_i(2))),          (a(3)*sin(theta_i(2) + theta_i(3)) + a(2)*sin(theta_i(2)))/(a(2)*sin(theta_i(2)))];
theta_dot_i=invJ*cartesianVelo_initial;
%  theta_i=theta_i*180/pi;
end

```