/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd and Vincent Hayward.	    *
 *									    *
 *	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:	jacobT(), jacobD(), jacobI(), jacobIT() -- RCCL routines
 *		to perform jacobian calculations.
 *
 * SYNOPSIS:	#include <robot/rccl.h>
 *		#include <robot/kine.h>
 *		
 *		jacobT (t, f, sncs)
 *		  JNTS *t;
 *		  FORCE *f;
 *		  SNCS *sncs;
 *		
 *		jacobD (d, j, sncs)
 *		  DIFF *d;
 *		  JNTS *j;
 *		  SNCS *sncs;
 *		
 *		code = jacobI (j, d, sncs, epsilon)
 *		  int code;
 *		  JNTS *j;
 *		  DIFF *d;
 *		  SNCS *sncs;
 *		  float epsilon;
 *
 *		code = jacobIT (f, t, sncs, epsilon)
 *		  int code;
 *		  FORCE *f;
 *		  JNTS *t;
 *		  SNCS *sncs;
 *		  float epsilon;
 *
 * DESCRIPTION: Jacobian computations in link 6, each performed by mapping 
 *		the arguments to and from link 4 and computing the jacobian
 *		there.
 *		
 *		jacobT maps a force 'f' in link 6 onto the corresponding
 *		 joint torques 'j':
 *
 *			       T T6
 *			j  =  J    f
 *		
 *		jacobD computes the differential change 'd' in cartesian 
 *		 space produced in link 6 by a differential change in joint 
 *		 angles 'q':
 *
 *		      T6
 *			d   = J q
 *		
 *		jacobI computes the inverse jacobian in link 6. 
 *		 The inverse jacobian maps a differential change 'd' in link 
 *		 6 to a corresponding differential change 'q' in joint angles:
 *			       -1 T6
 *			q  =  J     d   
 *		 
 *		jacobIT computes the transpose of inverse jacobian in link 6. 
 *		 The transpose of the inverse jacobian maps torques onto
 *		 the corresponding forces in link 6:
 *		
 *		      T6       -1 T
 *			f  =  J     t
 *		 
 *		Each routine takes a pointer to a data structure of type 
 *		SNCS (defined in "../h/kine.h"), which contains the sines and
 *		cosines of all the joint angles, as well as various
 *		precomputed terms related to the Jacobian. The reason for
 *		having the SNCS data structure 	is computational efficiency:
 *		the quanities in SNCS only need to be computed once. This is
 *		done using the routines update_sincos(), and update_jacobian_terms():
 *
 *			update_sincos (sncs, angles);
 *			update_jacobian_terms (sncs, angles);
 *
 *		where 'angles' is an array of 6 doubles describing the
 *		joint angles in radians (NOT range coordinates).
 *
 *		SINGULARITIES:
 *
 *		 The inverse Jacobian routines each take an argument
 *		 `epsilon', which is used to indicate the size of the
 *		 `forbidden' regions surrounding the manipulator
 *		 singularites. In the computation process, a singularity
 *		 is manifested as a divison by zero. `epsilon' describes,
 *		 approximately, the minimum ratio of denominator to numerator
 *		 that should be tolerated before the routine considers
 *		 the arm to be at a singularity. 
 *
 *		 `epsilon' is made available as a parameter because the
 *		 desired value depends on the application. If the input data
 *		 has a large error associated with it, then it is desirable to
 *		 make epsilon relatively large; otherwise, large errors will
 *		 be introduced near singularities.
 *
 *		 The inverse routines return a bit mask when the manipulator
 *		 is within epsilon of a signularity. The bits are described 
 *		 in  ../h/kine.h:
 *
 *				WRIST_DEG:	wrist degeneracy
 *			        ELBOW_DEG:	elbow is straight out
 *				ALIGN_DEG:	t6 lies along axis 1
 *
 *		 Detecting singularities is easy; handling them is something
 *		 else. A singularity implies a loss of the ability to observe
 *		 one or more degrees of freedom. When we are right at the 
 *		 singular point, we deal with this by setting the unobservable
 *		 quantity to zero. If we do this over the entire region
 *		 associated with the singularity, however, we will get a 
 *		 discontinuity in the function as the denomintor passes
 *		 through zero. This is the hack we do instead:
 *
 *		 Suppose the quantity being computed is y = m / x. We have
 *		 a singularity at x = 0. For a given epsilon, the singular
 *		 region is -epsilon * |m| < x < epsilon * |m|. Then,
 *		 WITHIN the singular region, we compute 
 *							 2
 *				y = m * x / (epsilon * m)
 *		
 *		 This is actually not so weird as it seems: It means that
 *		 as we pass through the singularity, our observed value cuts
 *		 a straight line between the points on either side of it.
 *		 Hence we can tolerate a large singular zone without
 *		 discontinuties.
 *		
 *		The parameter 'sncs' provides precomputed terms for part of
 *		the jacobian, sines, cosines, and the U5 transform.
 *
 * ARGUMENTS:	The units described below are the ones we use. The units 
 *		can be anything, as long as they are consistant.
 *
 *		JNTS *j --	describes small changes in joint angle values
 *				(radians).
 *
 *		DIFF *d --	Small changes in Cartesian coordinates, link 6
 *				(radians and millimeters).
 *
 *		FORCE *f --	Cartesian forces in link 6 (Newtons and
 *				Newton millimeters).
 *
 *		JNTS *t --	joint torques (Newton millimeters).
 *
 *		SNCS *sncs --	pointer to SNCS data structure constaining
 *				parameters used in computing the Jacobian.
 *
 *		float epsilon - tolerence describing size of singular region.
 *
 * RETURN VAL:	jacobD() and jacobT() return nothing. The inverse functions
 *		a code describing any singular regions the manipulator is
 *		within.
 *
 * EXTERNALS:	None.
 *
 * NOTES:	The computations are performed by mapping into the Cartesian
 *		coordinate frame in link 4, where things are simpler, and 
 *		working there.
 *
 *		Some day, we should write the inverse functions so that they
 *		return a minimum norm solution as they approach a singularity.
 *		Sigh. But not now.
 *
 * AUTHOR/DATE:	Vincent Hayward, Purdue University 1983.
 *		Functions rewritten by John Lloyd, McGill University, Fall 86
 *
 * UPDATES:	
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include "rccl.h"
#include "kine.h"
#include "pumadata.h"
#include "pumaforce.h"
#include "macros.h"

#define EPSILONF 2.0e-6

jacobT(j, f, sncs) /*::*/
JNTS_PTR j;
FORCE_PTR f;
register SNCS *sncs;
{
	FORCE fl4;

	fl4.f.x = sncs->u5.n.x * f->f.x + sncs->u5.o.x * f->f.y + 
		  sncs->u5.a.x * f->f.z;

	fl4.f.y = sncs->u5.n.y * f->f.x + sncs->u5.o.y * f->f.y +
		  sncs->u5.a.y * f->f.z;

	fl4.f.z = sncs->u5.n.z * f->f.x + sncs->u5.o.z * f->f.y;

	fl4.m.x = sncs->u5.n.x * f->m.x + sncs->u5.o.x * f->m.y + 
		  sncs->u5.a.x * f->m.z;

	fl4.m.y = sncs->u5.n.y * f->m.x + sncs->u5.o.y * f->m.y + 
		  sncs->u5.a.y * f->m.z;

	fl4.m.z = sncs->u5.n.z * f->m.x + sncs->u5.o.z * f->m.y;

	j->th1 = sncs->d1x * fl4.f.x +
		 sncs->d1y * fl4.f.y +
		 sncs->d1z * fl4.f.z +
		 sncs->r1x * fl4.m.x +
		 sncs->c23 * fl4.m.y +
		 sncs->r1z * fl4.m.z;

	j->th2 = sncs->d2x * fl4.f.x +
		 sncs->d2z * fl4.f.z +
		 sncs->d2y * fl4.f.y;

	j->th3 = sncs->d3x * fl4.f.x +
		 A3 * fl4.f.y +
		 sncs->d3z * fl4.f.z -
		 sncs->s4  * fl4.m.x +
		 sncs->c4  * fl4.m.z;

	j->th2 += j->th3;

	j->th4 =            fl4.m.y;

	j->th5 =            fl4.m.z;

	j->th6 = - sncs->s5  * fl4.m.x +
		   sncs->c5  * fl4.m.y;

}

jacobD(d, q, sncs) /*::*/
register DIFF_PTR d;
register JNTS_PTR q;
register SNCS *sncs;
{
	DIFF dl4;

	float q23 = q->th2 + q->th3;

	dl4.t.x = sncs->d1x * q->th1 +
		  sncs->d2x * q->th2 +
		  sncs->d3x * q23;

	dl4.t.y = sncs->d1y * q->th1 +
		  sncs->d2y * q->th2 +
		  A3 * q23;

	dl4.t.z = sncs->d1z * q->th1 +
		  sncs->d2z * q->th2 +
		  sncs->d3z * q23;

	dl4.r.x = sncs->r1x * q->th1 -
		  sncs->s4  * q23    -
		  sncs->s5  * q->th6;

	dl4.r.y=  sncs->c23 * q->th1 +
			    q->th4 +
		  sncs->c5  * q->th6;

	dl4.r.z = sncs->r1z * q->th1 +
		  sncs->c4  * q23    +
			      q->th5;

	d->t.x = sncs->u5.n.x * dl4.t.x + sncs->u5.n.y * dl4.t.y + 
		 sncs->u5.n.z * dl4.t.z;

	d->t.y = sncs->u5.o.x * dl4.t.x + sncs->u5.o.y * dl4.t.y + 
		 sncs->u5.o.z * dl4.t.z;

	d->t.z = sncs->u5.a.x * dl4.t.x + sncs->u5.a.y * dl4.t.y;

	d->r.x = sncs->u5.n.x * dl4.r.x + sncs->u5.n.y * dl4.r.y + 
		 sncs->u5.n.z * dl4.r.z;

	d->r.y = sncs->u5.o.x * dl4.r.x + sncs->u5.o.y * dl4.r.y + 
		 sncs->u5.o.z * dl4.r.z;

	d->r.z = sncs->u5.a.x * dl4.r.x + sncs->u5.a.y * dl4.r.y;
	
}

jacobI(q, d, sncs, epsilon) /*::*/
register JNTS_PTR q;
register DIFF_PTR d;
register SNCS *sncs;
float epsilon;
{
	DIFF dl4;
	int code = 0;
	register float threshold;
	float q23, vx;

	dl4.t.x = sncs->u5.n.x * d->t.x + sncs->u5.o.x * d->t.y + 
		  sncs->u5.a.x * d->t.z;

	dl4.t.y = sncs->u5.n.y * d->t.x + sncs->u5.o.y * d->t.y + 
		  sncs->u5.a.y * d->t.z;

	dl4.t.z = sncs->u5.n.z * d->t.x + sncs->u5.o.z * d->t.y;

	dl4.r.x = sncs->u5.n.x * d->r.x + sncs->u5.o.x * d->r.y + 
		  sncs->u5.a.x * d->r.z;

	dl4.r.y = sncs->u5.n.y * d->r.x + sncs->u5.o.y * d->r.y + 
		  sncs->u5.a.y * d->r.z;

	dl4.r.z = sncs->u5.n.z * d->r.x + sncs->u5.o.z * d->r.y;

	/* Massage epsilon into something reasonable */

	if (epsilon < EPSILONF)
	 { epsilon = EPSILONF;
	 }

	/* theta1 */

#define H1MAX	(A2+D4)

	threshold = epsilon * H1MAX;
	if (FABS(sncs->h1) < threshold) 
	 { code |= ALIGN_DEG;
	   q->th1 = (sncs->s4 * dl4.t.x - sncs->c4 * dl4.t.z) * sncs->h1 /
		    (threshold * threshold);
	 }
	else
	 { q->th1 = (sncs->s4 * dl4.t.x - sncs->c4 * dl4.t.z) / sncs->h1;
	 }

	/* theta2 */

	vx = sncs->c4 * dl4.t.x + sncs->s4 * dl4.t.z;

#define H3MAX	(A3+D4)

	threshold = epsilon * H3MAX;
	if (FABS(sncs->h3) < threshold)
	 { code |= ELBOW_DEG;
	   q->th2 =   (D4 * dl4.t.y + A3 * vx - q->th1 * D3 * sncs->h2 )
		    * (sncs->h3) / (A2 * threshold * threshold);
	 }
	else
	 { q->th2 =   (D4 * dl4.t.y + A3 * vx - q->th1 * D3 * sncs->h2 )
		    / (A2 * sncs->h3);
	 }

	/* theta3 */

	q23 = ( -vx + A2 * sncs->s3 * q->th2 + D3 * sncs->c23 * q->th1 ) / D4;
	q->th3 = q23 - q->th2;

	/* theta6 */

	threshold = epsilon;
	if (FABS(sncs->s5) < threshold)
	 { code |= WRIST_DEG;
	   q->th6 = (- dl4.r.x + sncs->s23 * sncs->c4 * q->th1 -
		       sncs->s4 * q23) * sncs->s5 / (threshold * threshold);
	 }
	else
	 { q->th6 = (- dl4.r.x + sncs->s23 * sncs->c4 * q->th1 -
		       sncs->s4 * q23) / sncs->s5;
	 }

	/* theta4 */

	q->th4 = dl4.r.y - sncs->c23 * q->th1 - sncs->c5 * q->th6;

	/* theta5 */

	q->th5 = dl4.r.z - sncs->s23 * sncs->s4 * q->th1 - sncs->c4 * q23;

	return (code);
}


jacobIT(f, t, sncs, epsilon) /*::*/
register FORCE *f;
register JNTS *t;
register SNCS *sncs;
float epsilon;
{
	FORCE fl4;
	int code = 0;
	float tor2, threshold, vx, vz, wx, wy;

	/* Set threshold to something reasonable */

	if (epsilon < EPSILONF)
	 { epsilon = EPSILONF;
	 }

	tor2 = t->th2 - t->th3;

	fl4.m.z = t->th5;

	fl4.m.y = t->th4;

	threshold = epsilon;
	if (FABS(sncs->s5) < threshold)
  	 { code |= WRIST_DEG;
	   fl4.m.x = (sncs->c5 * t->th4 - t->th6) * sncs->s5 / 
		     (threshold * threshold);
	 }
 	else
  	 { fl4.m.x = (sncs->c5 * t->th4 - t->th6) / sncs->s5;
	 }

	vx = t->th1 - (sncs->c23 * fl4.m.y + 
		       sncs->s23 * (sncs->c4 * fl4.m.x + sncs->s4 * fl4.m.z));
	vz = t->th3 + fl4.m.x * sncs->s4 - fl4.m.z * sncs->c4;

#define H3MAX	(A3+D4)

	threshold = epsilon * H3MAX;
	if (FABS(sncs->h3) < threshold)
  	 { code |= ELBOW_DEG;
	   fl4.f.y = (tor2 * D4 + A2 * sncs->s3 * vz) 
	   * (sncs->h3) / (A2 * threshold * threshold);
	 }
	else
  	 { fl4.f.y = (tor2 * D4 + A2 * sncs->s3 * vz) / (A2 * sncs->h3);
	 }

	wx = (A3 * fl4.f.y - vz) / D4;
	
#define H1MAX	(A2+D4)

	threshold = epsilon * H1MAX;
	if (FABS(sncs->h1) < threshold)
  	 { code |= ALIGN_DEG;
  	   wy =  (D3 * (sncs->s23 * fl4.f.y - sncs->c23 * wx) + vx)
		* sncs->h1 / (threshold * threshold);
	 }
 	else
  	 { wy =  (D3 * (sncs->s23 * fl4.f.y - sncs->c23 * wx) + vx)
		/ sncs->h1;
	 }

	fl4.f.x = sncs->c4 * wx + sncs->s4 * wy;

	fl4.f.z = sncs->s4 * wx - sncs->c4 * wy;


	f->m.x = sncs->u5.n.x * fl4.m.x + sncs->u5.n.y * fl4.m.y + 
		 sncs->u5.n.z * fl4.m.z;

	f->m.y = sncs->u5.o.x * fl4.m.x + sncs->u5.o.y * fl4.m.y + 
		 sncs->u5.o.z * fl4.m.z;

	f->m.z = sncs->u5.a.x * fl4.m.x + sncs->u5.a.y * fl4.m.y;

	f->f.x = sncs->u5.n.x * fl4.f.x + sncs->u5.n.y * fl4.f.y + 
		 sncs->u5.n.z * fl4.f.z;

	f->f.y = sncs->u5.o.x * fl4.f.x + sncs->u5.o.y * fl4.f.y + 
		 sncs->u5.o.z * fl4.f.z;

	f->f.z = sncs->u5.a.x * fl4.f.x + sncs->u5.a.y * fl4.f.y;

	return (code);
}



/*---------McGill-Computer-Vision-and-Robotics-Laboratory--C-Module---------
 *		
 * NAME:	update_sincos(), update_jacobian_terms()
 *
 * USAGE:	#include <robot/kine.h>
 *		
 *		update_sincos(sncs, ang)
 *		  SNCS *sncs;
 *		  double *ang;
 *		
 *		update_jacobian_terms(sncs)
 *		  SNCS *sncs;
 *
 * DESCRIPTION: update_sincos() updates the sine and cosine terms of the 
 *		'sncs' struct to be equal to the sines and cosines of the
 *		angles 'ang', which are given in radians.
 *		
 *		update_jacobian_terms() updates the jacobian terms and U5
 *		transform of the struct 'sncs'. It is assumed that the sine
 *		and cosine terms are valid; this can be ensured by calling
 *		update_sincos() beforehand.
 *
 * AUTHOR/DATE: John Lloyd, fall 1984
 *
 * UPDATES:
 *
 *-------------McGill-Computer-Vision-and-Robotics-Laboratory--------------*/

/* Jacobian updating functions */

update_sincos(sncs, ang)
register SNCS *sncs;
register double *ang;
{
	double ang23 = ang[1] + ang[2];

 	sncs->s1 = sin (*ang);
 	sncs->c1 = cos (*ang++);

 	sncs->s2 = sin (*ang);
 	sncs->c2 = cos (*ang++);

 	sncs->s23 = sin (ang23);
 	sncs->c23 = cos (ang23);

 	sncs->s3 = sin (*ang);
 	sncs->c3 = cos (*ang++);

 	sncs->s4 = sin (*ang);
 	sncs->c4 = cos (*ang++);

 	sncs->s5 = sin (*ang);
 	sncs->c5 = cos (*ang++);

 	sncs->s6 = sin (*ang);
 	sncs->c6 = cos (*ang++);
}

update_jacobian_terms(sncs)
register SNCS *sncs;
{
 	/* update h values */

	sncs->h2 =   sncs->c23 * A3 +
		   - sncs->s23 * D4;
	sncs->h1 =   sncs->h2 + sncs->c2 * A2;
	sncs->h3 =   sncs->s3 * A3 + sncs->c3 * D4;

	/* update terms of the jacobian */

	sncs->d1x =  sncs->h1  * sncs->s4 +
		     D3 * sncs->c23 * sncs->c4;
	sncs->d1y = -sncs->s23 * D3;
	sncs->d1z = -sncs->h1  * sncs->c4 + D3 *
		     sncs->c23 * sncs->s4;
	sncs->r1x =  sncs->s23 * sncs->c4;
	sncs->r1z =  sncs->s23 * sncs->s4;
	sncs->d2x =  A2 * sncs->s3 * sncs->c4;
	sncs->d2y =  A2 * sncs->c3;
	sncs->d2z =  A2 * sncs->s3 * sncs->s4;
	sncs->d3x = -sncs->c4  * D4;
	sncs->d3z = -sncs->s4  * D4;

	/* update the U5 coefficients */

	sncs->u5.n.x =  sncs->c5 * sncs->c6;
	sncs->u5.n.y =  sncs->s5 * sncs->c6;
	sncs->u5.n.z = -sncs->s6;
	sncs->u5.o.x = -sncs->c5 * sncs->s6;
	sncs->u5.o.y = -sncs->s5 * sncs->s6;
	sncs->u5.o.z = -sncs->c6;
	sncs->u5.a.x = -sncs->s5;
	sncs->u5.a.y =  sncs->c5;
	sncs->u5.a.z =  0.;
}
