/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/*=~=~=~=~ McGill-Computer-Vision-and-Robotics-Laboratory--C-Module ~=~=~=~=
 *		
 * NAME:	testJacobian
 *
 * USAGE:	testJacobian <robotName> [-i]
 *
 * DESCRIPTION: performs a test of the jacobian functions for the arm.
 *		The tests are done by playing the functions off against
 *		each other: a function is applied to a set of data,
 *		then the inverse is applied, and the final output is
 *		compared to the original input.
 * 
 *		fwdJacob() is played off against invJacob(), and
 *		fwdJacobT() is played off against invJacobT().
 *
 *		The program performs about 2000 tests. These include 
 *		cases away from, close to, and at singularities. The results
 *		printed out describe the maximum observed percent error
 *		between the input and output values.
 *
 *		The testing AT SINGULARITIES is a bit crude, but the best
 *		I could do quickly: The functions are run through once in
 *		in an attempt to zero the values associated with degrees
 *		of freedom which have been lost; running the data through
 *		again should produce meaningfull results.
 *
 * ARGUMENTS:	The -i option allows the user to interactively run 
 *		individual test on specific sets of joints angles.
 *	
 * NOTES:	I wrote this because I was always playing with the kinematic
 *		functions, but was never sure I hadn't screwed them up in the
 *		process.
 *
 * AUTHOR/DATE: John Lloyd, Spring 87
 *
 * UPDATES:
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include <stdio.h>
#include <math.h>

#include "../h/cdefs.h"
#include "../h/robot_jls.h"
#include "../h/robotMath.h"
#include "../h/scara_kynvar.h"
#include "../h/vector.h"
#include "../h/robotTypes.h"

#define EPSILONF	2.0e-6

#define	COMMAND(s)	(strcmp (key_buf, s) == 0)
#define	COMMANDS(s)	(strncmp (key_buf, s, strlen(s)) == 0)
char key_buf [256];

KYN *kyn;
void *var;
JLS *jls;

/* List of singularities to test for */

unsigned long singSet[] = { 
	SCARA_ELBOW_DEG,
	SCARA_WRIST_DEG,
};
int numSingCombos = (1<<ARRAY_SIZE(singSet));

unsigned long getSingCode(i)
int i;
{
	unsigned long code = 0;
	unsigned int mask;
	int k;

	for (mask=i,k=0; mask; mask>>=1,k++)
	 { if (mask & 0x1)
	    { code |= singSet[k];
	    }
	 }
	return (code);	
}

main(argc, argv)
int argc;
char **argv;
{
	char *robotName = NULL;
	int interactive = 0;

	argc--; argv++;
	while (argc)
	 { if (ARGUMENT ("-i"))
	    { interactive = 1;
	    }
	   else if (robotName == NULL)
	    { robotName = *argv;
	    }
	   else
	    { robotName = NULL;
	    }
	   argc--; argv++;
	 }
	if (robotName == NULL)
	 { usageMsg (-1, "testJacobian <robotName> [-i]\n");
	 }

	if ((jls = getJls(robotName)) == NULL)
	 { printErrors();
	   exit (-1);
	 }
	if ((kyn = getKyn(robotName, NULL)) == NULL)
	 { printErrors();
	   exit (-1);
	 }
	var = (void*)malloc(getVarSize(robotName));

	if (interactive)
	 { char *keytree, *tree_match_parse(), *tree_match_err_at();
	   float epsilon;

	   float angles[SCARAJ];

	   keytree = tree_match_parse("					\
(	   quit								\
	   set 								\
	    (angles							\
	     epsilon							\
	      (%f", &epsilon, ",0))					\
	   show								\
	   fwdJacobT							\
	   fwdJacob							\
	   invJacobT							\
	   invJacob							\
)",0);

	   if (keytree == 0)
	    { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
	      exit (-1);
	    }

	   scaraUpdateVar (var, angles, kyn);

	   do
	    { tree_match (keytree, "TEST JACOBIAN> ", key_buf);

	      if (COMMAND ("set angles"))
	       { 
		 printf ("6 angles, in degrees > ");
		 if (scanVf (angles, 6) != 1)
		  { printf ("ERROR reading values\n");
		    continue;
		  }
		 scaleVf (angles, DEGTORAD, angles, 6);

		 scaraUpdateVar (var, angles, kyn);
	       }
	      else if (COMMAND ("show"))
	       { printf ("epsilon = %g\n", epsilon);
		 printVf ("JOINT ANGLES: %8.3f\n", angles, 6);
	       }
	      else if (COMMAND ("fwdJacob"))
	       { float dtheta1[SCARAJ], dtheta2[SCARAJ];
		 DIFF dc1, dc2;
		 int code;

		 printf ("6 delta theta values > ");
		 if (scanVf (dtheta1, 6) != 1)
		  { printf ("ERROR reading values\n");
		    continue;
		  }
		 scaraFwdJacobVar (&dc1, dtheta1, var, kyn);

		 printf ("Cartesian deltas from fwdJacob():\n");
		 printVf ("\t%10.5f\n", (float*)&dc1, 6);

		 code = scaraInvJacobVar(dtheta2, &dc1, var, kyn, epsilon);

		 printf ("Joint deltas from invJacob(), status %x:\n", code);
		 printVf ("\t%10.5f\n", dtheta2, 6);

		 scaraFwdJacobVar (&dc2, dtheta2, var, kyn);

		 printf ("Cartesian deltas from fwdJacob():\n");
		 printVf ("\t%10.5f\n", (float*)&dc2, 6);
	       }		 
	      else if (COMMAND ("invJacob"))
	       { float dtheta1[SCARAJ], dtheta2[SCARAJ];
		 DIFF dc1, dc2;
		 int code;

		 printf ("6 Cartesian delta values > ");
		 if (scanVf ((float*)&dc1, 6) != 1)
		  { printf ("ERROR reading values\n");
		    continue;
		  }
		 code = scaraInvJacobVar(dtheta1, &dc1, var, kyn, epsilon);

		 printf ("Joint deltas from invJacob(), status %x:\n", code);
		 printVf ("\t%10.5f\n", dtheta1, 6);

		 scaraFwdJacobVar (&dc2, dtheta1, var, kyn);

		 printf ("Cartesian deltas from fwdJacob():\n");
		 printVf ("\t%10.5f\n", (float*)&dc2, 6);

		 code = scaraInvJacobVar(dtheta2, &dc2, var, kyn, epsilon);

		 printf ("Joint deltas from invJacob(), status %x:\n", code);
		 printVf ("\t%10.5f\n", dtheta2, 6);
	       }		 
	      else if (COMMAND ("fwdJacobT"))
	       { float torque1[SCARAJ], torque2[SCARAJ];
		 FORCE f1, f2;
		 int code;

		 printf ("6 force values > ");
		 if (scanVf ((float*)&f1, 6) != 1)
		  { printf ("ERROR reading values\n");
		    continue;
		  }
		 scaraFwdJacobTVar (torque1, &f1, var, kyn);

		 printf ("Torque values from fwdJacobT():\n");
		 printVf ("\t%10.5f\n", torque1, 6);

		 code = scaraInvJacobTVar(&f2, torque1, var, kyn, epsilon);

		 printf ("Force values from invJacobT(), status %x:\n", code);
		 printVf ("\t%10.5f\n", (float*)&f2, 6);

		 scaraFwdJacobTVar (torque2, &f2, var, kyn);

		 printf ("Torque values from fwdJacobT():\n");
		 printVf ("\t%10.5f\n", torque2, 6);
	       }		 
	      else if (COMMAND ("invJacobT"))
	       { float torque1[SCARAJ], torque2[SCARAJ];
		 FORCE f1, f2;
		 int code;

		 printf ("6 torque values > ");
		 if (scanVf (torque1, 6) != 1)
		  { printf ("ERROR reading values\n");
		    continue;
		  }
		 code = scaraInvJacobTVar(&f1, torque1, var, kyn, epsilon);

		 printf ("Force values from invJacobT(), status %x:\n", code);
		 printVf ("\t%10.5f\n", (float*)&f1, 6);

		 scaraFwdJacobTVar (torque2, &f1, var, kyn);

		 printf ("Torque values from fwdJacobT():\n");
		 printVf ("\t%10.5f\n", torque2, 6);

		 code = scaraInvJacobTVar(&f2, torque2, var, kyn, epsilon);

		 printf ("Force values from invJacobT(), status %x:\n", code);
		 printVf ("\t%10.5f\n", (float*)&f2, 6);
	       }		 
	    }
	   while (!COMMAND ("quit"));
	 }
	else	/* run automatric checking sequence */
	 { 
	   int numchecks, i, code;
	   float angles1[6];
	   float angles2[6];
	   float maxAnglesD[6];
	   float maxAnglesT[6];
	   float normD, normT, maxnormD, maxnormT;
	   int numsing;

#define END		-1.0e30

	    { static float testset[] = 
		{  0.0, 120.0, 200.0, END,
		 -30.0,  60.0, 120.0, END,
		 -50.0,-100.0, END,
		   0.0, 120.0, END,
		  45.0,   0.0, -45.0, END,
		 120.0,-120.0, END };

	      numchecks = testAnglesInit (testset);
printf ("\nTesting %d positions far from singularity ...\n\n", 
	numchecks);
	      maxnormD = maxnormT = -1.0;
	      numsing = 0;
	      while (testAnglesGet (angles1))
	       { 
		 if (code = scaraSingularityDetect(angles1, 5.0*DEGTORAD, kyn))
	          { scaraSingularitySet (angles1, angles1, 5.0*DEGTORAD, 
							  code, kyn, jls);
		  }
		
		 if (code = test_jacobian (&normD, &normT, angles1, EPSILONF))
		  { numsing++;
		  }

		 if (normD > maxnormD)
	          { maxnormD = normD;
		    copyVf (maxAnglesD, angles1, 6);
		  }
		 if (normT > maxnormT)
	          { maxnormT = normT;
		    copyVf (maxAnglesT, angles1, 6);
		  }
	       }
/*printf ("%d singular positions detected\n", numsing);*/
printf ("Maximum %% error for fwdJacob() <-> invJacob() is %g at angles \n", 
	maxnormD);
printa (" ",maxAnglesD);
printf ("Maximum %% error for fwdJacobT() <-> invJacobT() is %g at angles \n", 
	maxnormT);
printa (" ",maxAnglesT);
	    }

	    { static float testset[] = 
		{  0.0, 120.0, END,
		  60.0, -60.0, END,
		 -50.0,-100.0, END,
	  	  20.0, 120.0, END,
	 	 -45.0,  45.0, END,
		 120.0,-120.0, END };
	      int i;
	      float distance;

	      numchecks = testAnglesInit (testset);
printf ("\nTesting %d positions within 1 degree of singularity ...\n\n", 
	numchecks*numSingCombos*2);
	      maxnormD = maxnormT = -1.0;
	      numsing = 0;
	      while (testAnglesGet (angles1))
	       { 
		 for (i=0; i<numSingCombos*2; i++)
		  { 
		    distance = (i & 0x1) ? -DEGTORAD*.1 : DEGTORAD*.1;
		    scaraSingularitySet (angles2, angles1, distance, 
						 getSingCode(i>>1), kyn, jls);

		    if (code = test_jacobian (&normD, &normT, 
					      angles2, EPSILONF))
		     { numsing++;
		     }
		    if (normD > maxnormD)
	             { maxnormD = normD;
		       copyVf (maxAnglesD, angles2, 6);
		     }
		    if (normT > maxnormT)
	             { maxnormT = normT;
		       copyVf (maxAnglesT, angles2, 6);
		     }
		  }
	       }
/*printf ("%d singular positions detected\n", numsing);*/
printf ("Maximum %% error for fwdJacob() <-> invJacob() is %g at angles \n", 
	maxnormD);
printa (" ",maxAnglesD);
printf ("Maximum %% error for fwdJacobT() <-> invJacobT() is %g at angles \n", 
	maxnormT);
printa (" ",maxAnglesT);
	    }

	    { static float testset[] = 
		{  0.0, 120.0, END,
		  60.0, -60.0, END,
		 -50.0,-100.0, END,
	  	  20.0, 120.0, END,
	 	 -45.0,  45.0, END,
		 120.0,-120.0, END };
	      int i;

	      numchecks = testAnglesInit (testset);
	      printf ("\nTesting %d singular positions ...\n\n", 
		      (numSingCombos-1)*numchecks);
	      maxnormD = maxnormT = -1.0;
	      numsing = 0;
	      while (testAnglesGet (angles1))
	       {
		 for (i=1; i<numSingCombos; i++)
		  { 
		    scaraSingularitySet (angles2, angles1, 0.0, 
						 getSingCode(i), kyn, jls);

		    if (code = test_jacobian (&normD, &normT, 
					      angles2, EPSILONF))
		     { numsing++;
		     }
		    if (code != getSingCode(i))
		     { 
printf ("Jacobians detected singularities %x instead of %x at angles\n", 
	code, i);
printa (" ", angles2);
		     }
		    if (normD > maxnormD)
	             { maxnormD = normD;
		       copyVf (maxAnglesD, angles2, 6);
		     }
		    if (normT > maxnormT)
	             { maxnormT = normT;
		       copyVf (maxAnglesT, angles2, 6);
		     }
		  }
	       }
/*printf ("%d singular positions detected\n", numsing);*/
printf ("Maximum %% error for fwdJacob() <-> invJacob() is %g at angles \n", 
	maxnormD);
printa (" ",maxAnglesD);
printf ("Maximum %% error for fwdJacobT() <-> invJacobT() is %g at angles \n", 
	maxnormT);
printa (" ",maxAnglesT);
	    }

	 }
}

test_jacobian (normD, normT, angles, epsilon)
float *normD, *normT;
float *angles;
float epsilon;
{
	int singular, codeD, codeT;

	DIFF dc, dc_ref;
	FORCE f, f_ref;
	float dtheta[SCARAJ], dtheta_ref[SCARAJ];
	float torque[SCARAJ], torque_ref[SCARAJ];

	static DIFF dc_test = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0 };
	static FORCE f_test = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0 };
	static float dtheta_test[SCARAJ] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0 };
	static float torque_test[SCARAJ] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0 };

	singular = scaraSingularityDetect (angles, DEGTORAD*0.01, kyn);

	scaraUpdateVar (var, angles, kyn);

	if (singular)
	 { scaraFwdJacobVar (&dc, dtheta_test, var, kyn);
	   scaraInvJacobVar (dtheta_ref, &dc, var, kyn, epsilon);
	 }
	else
	 { copyVf (dtheta_ref, dtheta_test, 6);
	 }
	scaraFwdJacobVar (&dc, dtheta_ref, var, kyn);
	codeD = scaraInvJacobVar (dtheta, &dc, var, kyn, epsilon);

	if (normVf (dtheta, 6) > normVf(dtheta_ref, 6)*1.01 && 
	    !singular)
	 { 
	   printf ("Input and reconstructed joint deltas differ greatly,\n");
	   printf ("(norm %g vs. %g), at angles\n",
		   normVf(dtheta, 6), normVf(dtheta_ref,6));
	   printa (" ", angles);
	 }

	subtractVf (dtheta, dtheta, dtheta_ref, 6);
	*normD = normVf (dtheta, 6) / normVf (dtheta_ref, 6);

	if (singular)
	 { scaraInvJacobTVar (&f, torque_test, var, kyn, epsilon);
	   scaraFwdJacobTVar (torque_ref, &f, var, kyn);
	 }
	else
  	 { copyVf (torque_ref, torque_test, 6);
	 }
	codeT = scaraInvJacobTVar (&f, torque_ref, var, kyn, epsilon);
	scaraFwdJacobTVar (torque, &f, var, kyn);

	if (normVf(torque, 6) > normVf(torque_ref, 6)*1.01 && 
	    !singular)
	 { 
	   printf ("Input and reconstructed torque values differ greatly,\n");
	   printf ("(norm %g vs. %g), at angles\n",
		   normVf(torque,6), normVf(torque_ref,6));
	   printa (" ", angles);
	 }

	subtractVf (torque, torque, torque_ref, 6);
	*normT = normVf (torque, 6) / normVf (torque_ref, 6);

	if (codeD != codeT)
	 { 
printf ("invJacob() and invJacobT() find singularites %x and %x, at angles\n",
	codeD, codeT);
printa (" ", angles);
	 }
	return (codeD | codeT);
}

	 
printa (s, fp)
char *s;
float *fp;
{
	char buf[256];
	float a[6];

	strcpy (buf, s);
	strcat (buf, " %10.5lf\n");
	scaleVf (a, RADTODEG, fp, 6);
	printVf (buf, a, 6);
}
