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

#include "../h/elbow_kynvar.h"

#define CP50	ekyn->cp50
#define CP40	ekyn->cp40
#define CP30	ekyn->cp30
#define CP20	ekyn->cp20

#define C2	evar->c[1]
#define C23	evar->c23
#define S234	evar->s234
#define C234	evar->c234
#define S5	evar->s[4]
#define C5	evar->c[4]

elbowGravload(l, j6, kyn)
float *l;
float *j6;
KYN *kyn;
{
	ELBOW_VAR var;

	elbowUpdateVar (&var, j6, kyn);
	elbowGravloadVar (l, &var, kyn);
}

elbowGravloadVar(l, var, kyn)
register float *l;
void *var;
KYN *kyn;
{
	register ELBOW_KYN *ekyn = (ELBOW_KYN*)kyn->ext;
	register ELBOW_VAR *evar = (ELBOW_VAR*)var;

	l[5] = 0.0;
	l[4] = -CP50*C5*S234;
	l[3] = -CP50*S5*C234 - CP40*C234;
	l[2] = l[3] - CP30*C23;
	l[1] = l[2] - CP20*C2;
	l[0] = 0.0;
}
