/*-=-=-=-=-=-=-=-=-=-=-=- 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:	pumaGravload.c
 *
 * DESCRIPTION:	Gravity loading computations for the PUMA robots.
 *		For description, see 'man pumaGravload'.
 * 
 * AUTHOR/DATE:	Vincent Hayward, Purdue, 1983
 *		John Lloyd interfaced with new RCI, winter 1988.
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include "../h/puma_kynvar.h"

#define CP30	pkyn->cp30
#define CP50	pkyn->cp50
#define CP20	pkyn->cp20
#define CP21	pkyn->cp21
#define CP22	pkyn->cp22

#define S2	pvar->s[1]
#define C2	pvar->c[1]
#define S23	pvar->s23
#define C23	pvar->c23
#define S4	pvar->s[3]
#define C4	pvar->c[3]
#define S5	pvar->s[4]
#define C5	pvar->c[4]

pumaGravload(l, j6, kyn)
float *l;
float *j6;
KYN *kyn;
{
	PUMA_VAR var;

	pumaUpdateVar (&var, j6, kyn);
	pumaGravloadVar (l, &var, kyn);
}

pumaGravloadVar(l, var, kyn)
register float *l;
void *var;
KYN *kyn;
{
	PUMA_KYN *pkyn = (PUMA_KYN*)kyn->ext;
	PUMA_VAR *pvar = (PUMA_VAR*)var;

	l[0]  = 0.;
	l[2]  = CP30 * S23 + CP22 * C23 +
		CP50 * (C23 * C4 * S5 + C5 * S23);
	if (pkyn->oldCoordDefs)
	 { l[1]  = l[2] - C2 * CP21 - S2 * CP20;
	 }
	else
	 { l[1]  = l[2] + C2 * CP21 + S2 * CP20;
	 }
	l[3]  = -(CP50 * S23 * S4 * S5);
	l[4]  = CP50 * (S23 * C4 * C5 + C23 * S5);
	l[5]  = 0.;
}
