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

#include "../h/stan_kynvar.h"

#define CP20	skyn->cp20
#define CP30	skyn->cp30
#define CP31	skyn->cp31
#define CP40	skyn->cp40
#define CP50	skyn->cp50

#define C5	svar->c[4]
#define S5	svar->s[4]
#define C4	svar->c[3]
#define S4	svar->s[3]
#define C2	svar->c[1]
#define S2	svar->s[1]
#define D3	svar->d3

stanGravload(l, j6, kyn)
float *l;
float *j6;
KYN *kyn;
{
	STAN_VAR var;

	stanUpdateVar (&var, j6, kyn);
	stanGravloadVar (l, &var, kyn);
}

stanGravloadVar(l, var, kyn)
register float *l;
KYN *kyn;
void *var;
{
	register STAN_KYN *skyn = (STAN_KYN*)kyn->ext;
	register STAN_VAR *svar = (STAN_VAR*)var;

	l[5]  =  0.0;
	l[4]  =  CP50*(C2*S5 + C4*C5*S2);
	l[3]  = -CP50*(S2*S4*S5) - CP40*(C4*S2);
	l[2]  = -CP31*C2;
	l[1]  =  CP50*(C2*S5*C4 + C5*S2) - CP40*C2*S4 + CP31*D3*S2 + CP20*S2;
	l[0]  =  CP50*S5;
}
