Puma Jacobian functions from RCCL / RCI, (C) 1987, 1991 J Lloyd and V Hayward

pumaJacobian.c

/*-=-=-=-=-=-=-=-=-=-=-=- 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 ~=~=~=~=
 *		
 * MODULE NAME:	pumaJacob.c
 *
 * DESCRIPTION:	Jacobian computations for the PUMA robots.
 *		For description, see 'man pumaJacob'
 * 
 * AUTHOR/DATE:	John Lloyd, Spring 1987, after Vincent Hayward, Purdue, 1983
 *		Rewrote routines to interface with new RCI, winter 1988.
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include <math.h>

#include "../h/fastmath.h"
#include "../h/cdefs.h"
#include "../h/robot_jls.h"
#include "../h/robotMath.h"
#include "../h/puma_jls.h"
#include "../h/puma_kynvar.h"
#include "../h/robotTypes.h"

#define EPSILONF 2.0e-6

#define J1	0
#define J2	1
#define J3	2
#define J4	3
#define J5	4
#define J6	5

/* ---- Definition of link parameters to make code more readable ---- */

#define A2		gen->dh[1].a		/* basic A matrix parameters */
#define D3		gen->dh[2].d
#define D4		gen->dh[3].d
#define A3		gen->dh[2].a
#define D32		pkyn->d32
#define A22A32D42	pkyn->a22a32d42		/* A2*A2 - A3*A3 - D4*D4 */
#define ATANA3D4	pkyn->atana3d4		/* atan2(A3, D4) */
#define	EPSILON_A2D4	pkyn->epsilon_a2d4	/* Precision of (A2+D4)/2 */
#define EPSILON_D32	pkyn->epsilon_d32	/* Precision of D32 */

/* ---- Definition of var structure parameters ---- */

#define S1	pvar->s[J1]
#define C1	pvar->c[J1]
#define S2	pvar->s[J2]
#define C2	pvar->c[J2]
#define S23	pvar->s23
#define C23	pvar->c23
#define S3	pvar->s[J3]
#define C3	pvar->c[J3]
#define S4	pvar->s[J4]
#define C4	pvar->c[J4]
#define S5	pvar->s[J5]
#define C5	pvar->c[J5]
#define S6	pvar->s[J6]
#define C6	pvar->c[J6]

#define U5	pvar->u5

pumaFwdJacobT(t, f, j6, kyn)
float *t;
FORCE *f;
float *j6;
KYN *kyn;
{
	PUMA_VAR var;

	pumaUpdateVar (&var, j6, kyn);
	return (pumaFwdJacobTVar (t, f, &var, kyn));
}

pumaFwdJacob(dc, dj, j6, kyn)
DIFF *dc;
float *dj;
float *j6;
KYN *kyn;
{
	PUMA_VAR var;

	pumaUpdateVar (&var, j6, kyn);
	return (pumaFwdJacobVar (dc, dj, &var, kyn));
}

pumaInvJacobT(f, t, j6, kyn, epsilon)
FORCE *f;
float *t;
float *j6;
KYN *kyn;
double epsilon;
{
	PUMA_VAR var;

	pumaUpdateVar (&var, j6, kyn);
	return (pumaInvJacobTVar (f, t, &var, kyn, epsilon));
}

pumaInvJacob(dj, dc, j6, kyn, epsilon)
float *dj;
DIFF *dc;
float *j6;
KYN *kyn;
double epsilon;
{
	PUMA_VAR var;

	pumaUpdateVar (&var, j6, kyn);
	return (pumaInvJacobVar (dj, dc, &var, kyn, epsilon));
}

pumaFwdJacobTVar(j, f, var, kyn) /*::*/
float *j;
FORCE *f;
void *var;
KYN *kyn;
{
	register PUMA_KYN *pkyn =(PUMA_KYN*)kyn->ext;
	register PUMA_VAR *pvar = (PUMA_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	FORCE fl4;

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

	if (pkyn->oldCoordDefs)
	 { j[J1] = pvar->j11 * fl4.f.x + pvar->j21 * fl4.f.y +
		   pvar->j31 * fl4.f.z + pvar->j41 * fl4.m.x -
		   C23 * fl4.m.y + pvar->j61 * fl4.m.z;

	   j[J2] = pvar->j12 * fl4.f.x + pvar->j22 * fl4.f.y +
		   pvar->j32 * fl4.f.z;
   
	   j[J3] = pvar->j13 * fl4.f.x + A3 * fl4.f.y +
		   pvar->j33 * fl4.f.z + S4 * fl4.m.x + C4 * fl4.m.z;
   
	   j[J2] += j[J3];
   
	   j[J4] = - fl4.m.y;
   
	   j[J5] = fl4.m.z;
   
	   j[J6] = S5 * fl4.m.x - C5 * fl4.m.y;
	 }
	else
	 { j[J1] = pvar->j11 * fl4.f.x + pvar->j21 * fl4.f.y +
		   pvar->j31 * fl4.f.z + pvar->j41 * fl4.m.x +
		   C23 * fl4.m.y + pvar->j61 * fl4.m.z;

	   j[J2] = pvar->j12 * fl4.f.x + pvar->j32 * fl4.f.z +
		   pvar->j22 * fl4.f.y;
   
	   j[J3] = pvar->j13 * fl4.f.x + A3 * fl4.f.y +
		   pvar->j33 * fl4.f.z - S4  * fl4.m.x + C4 * fl4.m.z;
   
	   j[J2] += j[J3];
   
	   j[J4] = fl4.m.y;
   
	   j[J5] = fl4.m.z;
   
	   j[J6] = - S5 * fl4.m.x + C5 * fl4.m.y;
	 }
}

pumaFwdJacobVar(d, q, var, kyn) /*::*/
register DIFF_PTR d;
register float *q;
void *var;
KYN *kyn;
{
	register PUMA_KYN *pkyn =(PUMA_KYN*)kyn->ext;
	register PUMA_VAR *pvar = (PUMA_VAR*)var;
	GEN_KYN *gen = kyn->gen;
	
	DIFF dl4;

	float q23 = q[J2] + q[J3];

	if (pkyn->oldCoordDefs)
	 { dl4.t.x = pvar->j11 * q[J1] + pvar->j12 * q[J2] + pvar->j13 * q23;
	   dl4.t.y = pvar->j21 * q[J1] + pvar->j22 * q[J2] + A3 * q23;
	   dl4.t.z = pvar->j31 * q[J1] + pvar->j32 * q[J2] + pvar->j33 * q23;
	   dl4.r.x = pvar->j41 * q[J1] + S4 * q23 + S5 * q[J6];
	   dl4.r.y = -C23 * q[J1] - q[J4] - C5 * q[J6];
	   dl4.r.z = pvar->j61 * q[J1] + C4 * q23 + q[J5];
	 }
	else
	 { dl4.t.x = pvar->j11 * q[J1] + pvar->j12 * q[J2] + pvar->j13 * q23;
	   dl4.t.y = pvar->j21 * q[J1] + pvar->j22 * q[J2] + A3 * q23;
	   dl4.t.z = pvar->j31 * q[J1] + pvar->j32 * q[J2] + pvar->j33 * q23;
	   dl4.r.x = pvar->j41 * q[J1] - S4 * q23 - S5 * q[J6];
	   dl4.r.y = C23 * q[J1] + q[J4] + C5 * q[J6];
	   dl4.r.z = pvar->j61 * q[J1] + C4 * q23 + q[J5];
	 }

	d->t.x = U5.n.x * dl4.t.x + U5.n.y * dl4.t.y + U5.n.z * dl4.t.z;
	d->t.y = U5.o.x * dl4.t.x + U5.o.y * dl4.t.y + U5.o.z * dl4.t.z;
	d->t.z = U5.a.x * dl4.t.x + U5.a.y * dl4.t.y + U5.a.z * dl4.t.z;
	d->r.x = U5.n.x * dl4.r.x + U5.n.y * dl4.r.y + U5.n.z * dl4.r.z;
	d->r.y = U5.o.x * dl4.r.x + U5.o.y * dl4.r.y + U5.o.z * dl4.r.z;
	d->r.z = U5.a.x * dl4.r.x + U5.a.y * dl4.r.y + U5.a.z * dl4.r.z;
}

pumaInvJacobVar(q, d, var, kyn, epsilon) /*::*/
register float *q;
register DIFF_PTR d;
void *var;
KYN *kyn;
double epsilon;
{
	register PUMA_KYN *pkyn =(PUMA_KYN*)kyn->ext;
	register PUMA_VAR *pvar = (PUMA_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	DIFF dl4;
	int code = 0;
	register float threshold;
	float q23, vx;

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

	/* Massage epsilon into something reasonable */

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

	if (pkyn->oldCoordDefs)
	 { 
	   /* theta1 */
   
	   threshold = epsilon * pvar->h1_norm;
	   if (FABS(pvar->h1) < threshold) 
	    { code |= PUMA_ALIGN_DEG;
	      q[J1] = (S4 * dl4.t.x + C4 * dl4.t.z) * pvar->h1 /
		       (threshold * threshold);
	    }
	   else
	    { q[J1] = (S4 * dl4.t.x + C4 * dl4.t.z) / pvar->h1;
	    }
   
	   /* theta2 */
   
	   vx = C4 * dl4.t.x - S4 * dl4.t.z;
   
	   threshold = epsilon * pkyn->sqrt_a32d42;
	   if (FABS(pvar->h3) < threshold)
	    { code |= PUMA_ELBOW_DEG;
	      q[J2] =   (- D4 * dl4.t.y + A3 * vx + q[J1] * D3 * pvar->h2 )
		       * (pvar->h3) / (A2 * threshold * threshold);
	    }
	   else
	    { q[J2] =   (- D4 * dl4.t.y + A3 * vx + q[J1] * D3 * pvar->h2 )
		       / (A2 * pvar->h3);
	    }
   
	   /* theta3 */
   
	   q23 = ( vx - A2 * S3 * q[J2] + D3 * C23 * q[J1] ) / D4;
	   q[J3] = q23 - q[J2];
   
	   /* theta6 */
   
	   threshold = epsilon;
	   if (FABS(S5) < threshold)
	    { code |= PUMA_WRIST_DEG;
	      q[J6] = (dl4.r.x + S23 * C4 * q[J1] -
		       S4 * q23) * S5 / (threshold * threshold);
	    }
	   else
	    { q[J6] = (dl4.r.x + S23 * C4 * q[J1] -
			  S4 * q23) / S5;
	    }
   
	   /* theta4 */
   
	   q[J4] = - dl4.r.y - C23 * q[J1] - C5 * q[J6];
   
	   /* theta5 */
   
	   q[J5] = dl4.r.z - S23 * S4 * q[J1] - C4 * q23;
	 }
	else
	 { 
	   /* theta1 */
   
	   threshold = epsilon * pvar->h1_norm;
	   if (FABS(pvar->h1) < threshold) 
	    { code |= PUMA_ALIGN_DEG;
	      q[J1] = (S4 * dl4.t.x - C4 * dl4.t.z) * pvar->h1 /
		       (threshold * threshold);
	    }
	   else
	    { q[J1] = (S4 * dl4.t.x - C4 * dl4.t.z) / pvar->h1;
	    }
   
	   /* theta2 */
   
	   vx = C4 * dl4.t.x + S4 * dl4.t.z;
   
	   threshold = epsilon * pkyn->sqrt_a32d42;
	   if (FABS(pvar->h3) < threshold)
	    { code |= PUMA_ELBOW_DEG;
	      q[J2] =   (D4 * dl4.t.y + A3 * vx - q[J1] * D3 * pvar->h2 )
		       * (pvar->h3) / (A2 * threshold * threshold);
	    }
	   else
	    { q[J2] =   (D4 * dl4.t.y + A3 * vx - q[J1] * D3 * pvar->h2 )
		       / (A2 * pvar->h3);
	    }
   
	   /* theta3 */
   
	   q23 = ( -vx + A2 * S3 * q[J2] + D3 * C23 * q[J1] ) / D4;
	   q[J3] = q23 - q[J2];
   
	   /* theta6 */
   
	   threshold = epsilon;
	   if (FABS(S5) < threshold)
	    { code |= PUMA_WRIST_DEG;
	      q[J6] = (- dl4.r.x + S23 * C4 * q[J1] -
			  S4 * q23) * S5 / (threshold * threshold);
	    }
	   else
	    { q[J6] = (- dl4.r.x + S23 * C4 * q[J1] -
			  S4 * q23) / S5;
	    }
   
	   /* theta4 */
   
	   q[J4] = dl4.r.y - C23 * q[J1] - C5 * q[J6];
   
	   /* theta5 */
   
	   q[J5] = dl4.r.z - S23 * S4 * q[J1] - C4 * q23;
	 }

	return (code);
}

pumaInvJacobTVar(f, t, var, kyn, epsilon)
register FORCE *f;
register float *t;
void *var;
KYN *kyn;
double epsilon;
{
	register PUMA_KYN *pkyn =(PUMA_KYN*)kyn->ext;
	register PUMA_VAR *pvar = (PUMA_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	FORCE fl4;
	int code = 0;
	float tor2, threshold, vx, vz, wx, wy, wz;

	/* Set threshold to something reasonable */

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

	tor2 = t[J2] - t[J3];

	if (pkyn->oldCoordDefs)
	 { 
	   fl4.m.z = t[J5];
   
	   fl4.m.y = -t[J4];
   
	   threshold = epsilon;
	   if (FABS(S5) < threshold)
	    { code |= PUMA_WRIST_DEG;
	      fl4.m.x = (- C5 * t[J4] + t[J6]) * S5 / 
			(threshold * threshold);
	    }
	   else
	    { fl4.m.x = (- C5 * t[J4] + t[J6]) / S5;
	    }
   
	   vx = t[J1] + C23 * fl4.m.y + 
			S23 * (C4 * fl4.m.x - S4 * fl4.m.z);
	   vz = t[J3] - fl4.m.x * S4 - fl4.m.z * C4;
   
	   threshold = epsilon * pkyn->sqrt_a32d42;
	   if (FABS(pvar->h3) < threshold)
	    { code |= PUMA_ELBOW_DEG;
	      wz = (tor2 * D4 - A2 * S3 * vz) 
	       * (pvar->h3) / (A2 * threshold * threshold);
	    }
	   else
	    { wz = (tor2 * D4 - A2 * S3 * vz) / (A2 * pvar->h3);
	    }
   
	   wx = (A3 * wz + vz) / D4;
   
	   threshold = epsilon * pvar->h1_norm;
	   if (FABS(pvar->h1) < threshold)
	    { code |= PUMA_ALIGN_DEG;
	      wy =  (D3 * (S23 * wz + C23 * wx) + vx)
		     * pvar->h1 / (threshold * threshold);
	    }
	   else
	    { wy =  (D3 * (S23 * wz + C23 * wx) + vx)
		     / pvar->h1;
	    }
   
	   fl4.f.y = -wz;

	   fl4.f.x =   C4 * wx + S4 * wy;
   
	   fl4.f.z = - S4 * wx + C4 * wy;
	 }
	else
	 { fl4.m.z = t[J5];
   
	   fl4.m.y = t[J4];
   
	   threshold = epsilon;
	   if (FABS(S5) < threshold)
	    { code |= PUMA_WRIST_DEG;
	      fl4.m.x = (C5 * t[J4] - t[J6]) * S5 / 
			(threshold * threshold);
	    }
	   else
	    { fl4.m.x = (C5 * t[J4] - t[J6]) / S5;
	    }
   
	   vx = t[J1] - (C23 * fl4.m.y + 
			  S23 * (C4 * fl4.m.x + S4 * fl4.m.z));
	   vz = t[J3] + fl4.m.x * S4 - fl4.m.z * C4;
   
	   threshold = epsilon * pkyn->sqrt_a32d42;
	   if (FABS(pvar->h3) < threshold)
	    { code |= PUMA_ELBOW_DEG;
	      fl4.f.y = (tor2 * D4 + A2 * S3 * vz) 
	      * (pvar->h3) / (A2 * threshold * threshold);
	    }
	   else
	    { fl4.f.y = (tor2 * D4 + A2 * S3 * vz) / (A2 * pvar->h3);
	    }
   
	   wx = (A3 * fl4.f.y - vz) / D4;
   
	   threshold = epsilon * pvar->h1_norm;
	   if (FABS(pvar->h1) < threshold)
	    { code |= PUMA_ALIGN_DEG;
	      wy =  (D3 * (S23 * fl4.f.y - C23 * wx) + vx)
		   * pvar->h1 / (threshold * threshold);
	    }
	   else
	    { wy =  (D3 * (S23 * fl4.f.y - C23 * wx) + vx)
		   / pvar->h1;
	    }
   
	   fl4.f.x = C4 * wx + S4 * wy;
   
	   fl4.f.z = S4 * wx - C4 * wy;
	 }

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

	return (code);
}

/* Jacobian updating functions */

pumaUpdateVar (var, ang, kyn)
void *var;
register float *ang;
KYN *kyn;
{
	register PUMA_VAR *pvar = (PUMA_VAR*)var;

	float ang23 = ang[J2] + ang[J3];

	SINCOS (ang[J1], &S1, &C1);
	SINCOS (ang[J2], &S2, &C2);
	SINCOS (ang[J3], &S3, &C3);
	SINCOS (ang[J4], &S4, &C4);
	SINCOS (ang[J5], &S5, &C5);
	SINCOS (ang[J6], &S6, &C6);
	SINCOS (ang23, &S23, &C23);

	pumaUpdateVarXsincos (var, ang, kyn);
}

pumaUpdateVarXsincos (var, ang, kyn)
void *var;
register float *ang;
KYN *kyn;
{
	register PUMA_KYN *pkyn =(PUMA_KYN*)kyn->ext;
	register PUMA_VAR *pvar = (PUMA_VAR*)var;
	register GEN_KYN *gen = kyn->gen;

	if (pkyn->oldCoordDefs)
	 { 
	   /* update h values */
   
	   pvar->h2 =   C23 * A3 +
		       S23 * D4;
	   pvar->h1 =   pvar->h2 + C2 * A2;
	   pvar->h3 =   S3 * A3 - C3 * D4;
	   pvar->h1_norm = D4 + A2;
   
	   /* update terms of the jacobian */
   
	   pvar->j11 =  pvar->h1 * S4 -
		       D3 * C23 * C4;
	   pvar->j21 =  S23 * D3;
	   pvar->j31 =  pvar->h1 * C4 + D3 * C23 * S4;
	   pvar->j41 = -S23 * C4;
	   pvar->j61 =  S23 * S4;
	   pvar->j12 =  A2 * S3 * C4;
	   pvar->j22 =  A2 * C3;
	   pvar->j32 = -A2 * S3 * S4;
	   pvar->j13 =  C4 * D4;
	   pvar->j33 = -S4 * D4;
   
	   /* update the U5 coefficients */
   
	   U5.n.x =  C5 * C6;
	   U5.n.y =  S5 * C6;
	   U5.n.z =  S6;
	   U5.o.x = -C5 * S6;
	   U5.o.y = -S5 * S6;
	   U5.o.z =  C6;
	   U5.a.x =  S5;
	   U5.a.y = -C5;
	   U5.a.z =  0.;
	 }
	else
	 { /* update h values */
   
	   pvar->h2 =   C23 * A3 +
		      - S23 * D4;
	   pvar->h1 =   pvar->h2 + C2 * A2;
	   pvar->h3 =   S3 * A3 + C3 * D4;
	   pvar->h1_norm = sqrt (pkyn->d42a22a32 + 
			  2.0 * A2 * (A3 * C3 - D4 * S3));
   
	   /* update terms of the jacobian */
   
	   pvar->j11 =  pvar->h1  * S4 +
			D3 * C23 * C4;
	   pvar->j21 = -S23 * D3;
	   pvar->j31 = -pvar->h1  * C4 + D3 *
			C23 * S4;
	   pvar->j41 =  S23 * C4;
	   pvar->j61 =  S23 * S4;
	   pvar->j12 =  A2 * S3 * C4;
	   pvar->j22 =  A2 * C3;
	   pvar->j32 =  A2 * S3 * S4;
	   pvar->j13 = -C4  * D4;
	   pvar->j33 = -S4  * D4;
   
	   /* update the U5 coefficients */

	   U5.n.x =  C5 * C6;
	   U5.n.y =  S5 * C6;
	   U5.n.z = -S6;
	   U5.o.x = -C5 * S6;
	   U5.o.y = -S5 * S6;
	   U5.o.z = -C6;
	   U5.a.x = -S5;
	   U5.a.y =  C5;
	   U5.a.z =  0.;
	 }
}