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

#include "../h/scara_kynvar.h"

#define CP30	skyn->cp30
#define CP50	skyn->cp50

#define S5	svar->s[4]

scaraGravload(l, j6, kyn)
float *l;
float *j6;
KYN *kyn;
{
	SCARA_VAR var;

	scaraUpdateVar (&var, j6, kyn);
	scaraGravloadVar (l, &var, kyn);
}

scaraGravloadVar(l, var, kyn)
register float *l;
void *var;
KYN *kyn;
{
	SCARA_KYN *skyn = (SCARA_KYN*)kyn->ext;
	SCARA_VAR *svar = (SCARA_VAR*)var;

	l[0]  = 0.0;
	l[1]  = 0.0;
	l[2]  = CP30;
	l[3]  = 0.0;
	l[4]  = CP50*S5;
	l[5]  = 0.;
}
