(*  Mathematica Implementation of the null space projection
    algorithm.
    
    Bill Goodwine, October 12, 1998.
*)

Clear[t1, t2, t3];
l1 = 1.0;
l2 = 1.0;
l3 = 1.0;
tfinal = 4.0
J[t1_,t2_,t3_] := {{-l1 Sin[t1] - l2 Sin[t1 + t2] - l3 Sin[t1 + t2 + t3], -l2 Sin[t1 + t2] - l3 Sin[t1 + t2 + t3], -l3 Sin[t1 + t2 + t3]}, {l1 Cos[t1] + l2 Cos[t1 + t2] + l3 Cos[t1 + t2 + t3], l2 Cos[t1 + t2] + l3 Cos[t1 + t2 + t3], l3 Cos[t1 + t2 + t3]}};

Jo[t1_,t2_,t3_] := {{-l1 Sin[t1] - l2 Sin[t1 + t2], -l2 Sin[t1 + t2],0}, {l1 Cos[t1] + l2 Cos[t1 + t2], l2 Cos[t1 + t2],0}};

xd = -1.0;
yd = -0.25;

xod = 0.0;
yod = 1.0;

PI[J_] := Evaluate[PseudoInverse[J]];

eqn[t1_,t2_,t3_] := (PI[J[t1,t2,t3]] . {xd, yd}) + (IdentityMatrix[3] - PI[J[t1,t2,t3]] . J[t1,t2,t3]) . PI[Jo[t1,t2,t3] . (IdentityMatrix[3] - PI[J[t1,t2,t3]] . J[t1,t2,t3])] . ({xod, yod} - Jo[t1,t2,t3] . PI[J[t1,t2,t3]] . {xd, yd});  

g = {};
tt1 = {};
tt2 = {};
tt3 = {};
eqq = {};
count = 100;
dt = 0.01;
{t1,t2,t3} = {0.0,Pi/2,-Pi/2};
<<Graphics`Arrow`;
Do[
   {t1,t2,t3} += eqn[t1,t2,t3]*dt;
   eqq = Append[eqq,eqn[t1,t2,t3]];
	orig = {0,0};
	p1 = {l1 Cos[t1], l1 Sin[t1]};
	p2 = {l1 Cos[t1] + l2 Cos[t1 + t2], l1 Sin[t1]  + l2 Sin[t1 + t2]};
	p3 = {l1 Cos[t1] + l2 Cos[t1 + t2] + l3 Cos[t1 + t2 + t3], 
      l1 Sin[t1] + l2 Sin[t1 + t2] + l3 Sin[t1 + t2 + t3]};
	arr = p2 + {xod,yod};
	If[count >= 10,
	g = Append[g, Graphics[{RGBColor[1,0,0], Line[{orig,p1,p2,p3}], RGBColor[0,1,0], Disk[{1,-1},1],RGBColor[0,0,1], Arrow[p2,arr]}]];
		count = 0];
	tt1 = Append[tt1, t1];
	tt2 = Append[tt2, t2];
	tt3 = Append[tt3, t3];
	Print[t, {t1,t2,t3}];
	count++,
	{t, 0, tfinal, dt}];

<<Graphics`Animation`;
ShowAnimation[g, PlotRange->{{-3,3},{-3,3}}, Axes->True];
