(* inverse kinematic code *) 
 (* 
      usage
computeJointPositions; 
computeJacobian; 
setUp; 
delta  =  
        LinearSolve[A, 
          b]; 
step; 
drawArm;
*) 

<< 
      LinearAlgebra`GaussianElimination`

 (* initial conditions *) 
base  = {0, 0};
  
oldMove  =  {0.1, 0.1, 0.1};
threshold  =  0.1;
oldDistance  =  1000.0;
newDistance  =  0.0;
  
lengths =  {5, 5, 5};
goal = {5, 5};
computeJointPositions[
        x_]  :=  (
a  =  
          x *Pi/180;  
p  =  {
{base[[1]], base[[2]]}, 
{base[[1]] + 
                lengths[[1]]*Cos[a[[1]]], 
              base[[2]] + 
                lengths[[1]]*
                  Sin[a[[1]]]}, 
{base[[1]] + 
                lengths[[1]]*Cos[a[[1]]] + 
                lengths[[2]]*
                  Cos[a[[1]] + 
                      a[[2]]], 
base[[2]] + 
                lengths[[1]]*Sin[a[[1]]] + 
                lengths[[2]]*
                  Sin[a[[1]] + a[[2]]]}, 
{N[
                base[[1]] + lengths[[1]]*Cos[a[[1]]] + 
                  lengths[[2]]*Cos[a[[1]] + a[[2]]]  + 
                  lengths[[3]]*
                    Cos[a[[1]] + a[[2]] + 
                        a[[3]]]], 
N[
                base[[2]] + lengths[[1]]*Sin[a[[1]]] + 
                  lengths[[2]]*Sin[a[[1]] + a[[2]]] + 
                  lengths[[3]]*
                    Sin[a[[1]] + a[[2]] + 
                        
                        a[[3]]]]}
}; 
v  =  
          goal  -  
            p[[4]]
)
computeJacobian  :=  (
w = 
          p[[4]] - p[[1]]  // 
            N; 
w1 = {-w[[2]], 
            w[[1]]}; 
w = 
          p[[4]] - p[[2]]  //  
            N; 
w2 = {-w[[2]], 
            w[[1]]}; 
w = 
          p[[4]] - p[[3]]  //  
            N; 
w3 = {-w[[2]], 
            w[[1]]}; 
JT  =  {w1, w2, 
            w3}; 
J  =  
          Transpose[JT]
)
setUp  :=  (
b = JT . v; 
A = 
          JT . J ;
)
drawArm  := (
g0 = 
          Graphics[
            Rectangle[{-5, -10}, {0, 10}]]; 
g01 = 
          Graphics[Line[{p[[1]], p[[2]]}]]; 
g12 = 
          Graphics[Line[{p[[2]], p[[3]]}]]; 
g23 = 
          Graphics[
            Line[{p[[3]], p[[4]]}]]; 
ggoal = 
          Graphics[{PointSize[0.05], Point[goal]}]; 
Show[
          ggoal, g0, g01, g12, g23, 
          PlotRange -> {{-15, 15}, {-15, 
                15}}]
)
step  := ( anglesTemp  =  
            angles  +  delta; )
norm[
        v_] := (
For[i = 1; s = 0, 
          i <= Length[v],  i++, 
          s += v[[i]]*v[[i]]]; 
Sqrt[
          s]
)