	(*******************************************************************)
	(** program to simulate kinematics of redundancy resolution based **)
	(** on the Jacobian pseudo-inverse				  **)
	(*******************************************************************)

	(* Link Lengths of the 3R robot *)
l1 = 1.0; l2 = 1.0; l3 = 1.0

	(* forward kinematics functions *)
x[t1_,t2_,t3_]:= l1 Cos[t1] + l2 Cos[t1+t2] + l3 Cos[t1+t2+t3]
y[t1_,t2_,t3_]:= l1 Sin[t1] + l2 Sin[t1+t2] + l3 Sin[t1+t2+t3]

	(* the Jacobian matrix *)
JacMat[t1_,t2_,t3_]:=Block[{J11,J12,J13,J21,J22,J23,s1,c1,s12,c12,s123,c123},
    s1 = Sin[t1]; s12 = Sin[t1+t2]; s123 = Sin[t1+t2+t3];
    c1 = Cos[t1]; c12 = Cos[t1+t2]; c123 = Cos[t1+t2+t3];
    J13 = - l3 s123;     J12 = J13 - (l2 s12);     J11 = J12 - (l1 s1);
    J23 = l3 c123;       J22 = J23 + l2 c12;       J21 = J22 + l1 c1;
    Return[{{J11, J12, J13},{J21,J22,J23}}]; ]

	(* returns the Jacobian pseudo-inverse *)
JPseudo[t1_,t2_,t3_]:= Block[{jac},
   jac = JacMat[t1,t2,t3];
   Return[PseudoInverse[jac]]; ]

	(* equations for desired circular path *)
R=0.5 						(* radius of circle *)
OMEGA = Pi/2					(* speed along path *)
xd[t_]:= 1.5 - R Sin[OMEGA t]			(* desired x-position *)
yd[t_]:=     - R Cos[OMEGA t]			(* desired y-position *)
xddot[t_]:= -R OMEGA Cos[OMEGA t]		(* desired x velocity *)
yddot[t_]:=  R OMEGA Sin[OMEGA t]		(* desired y velocity *)
pdot[t_]:= List[xddot[t],yddot[t]]

	(* constant basis vectors *)
XVECT = List[1,0,0]; YVECT = List[0,1,0]; ZVECT = List[0,0,1]

    (* use Mathematica's NDSolve capability to integrate the redundancy
       resolution differential equations.  The manipulator is initialized at
       configuration (97.1808,-97.1808,-97.1808) degrees. This function returns a vector of
       interpolating functions *)

Angles=NDSolve[{
   t1'[t] == XVECT . (JPseudo[t1[t],t2[t],t3[t]] . pdot[t]),
   t2'[t] == YVECT . (JPseudo[t1[t],t2[t],t3[t]] . pdot[t]),
   t3'[t] == ZVECT . (JPseudo[t1[t],t2[t],t3[t]] . pdot[t]),
   t1[0] == 97.1808*(Pi/180), t2[0]== -97.1808*(Pi/180), t3[0]== -97.1808*(Pi/180) },  {t1,t2,t3},{t,0,4.5}]

	(* plot the joint angles resulting from the NDSolve integration *)
PlotAngles = Plot[{Evaluate[t1[t] /. Angles], Evaluate[t2[t] /. Angles],
	           Evaluate[t3[t] /. Angles]}, {t,0,4.4}]

	(* Plot the associated end-effector coordinates *)
PlotXY = ParametricPlot[
  {x[Evaluate[t1[t] /. Angles][[1]], Evaluate[t2[t] /. Angles][[1]], 
     Evaluate[t3[t] /. Angles][[1]] ],
   y[Evaluate[t1[t] /. Angles][[1]], Evaluate[t2[t] /. Angles][[1]], 
     Evaluate[t3[t] /. Angles][[1]] ] },  {t,0.1,4.4}, AspectRatio->1.0]



