/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	setJbias
 *
 * DESCRIPTION:	Routines to enable joint biasing, set values for the
 *		joint bias, and attach a joint biasing function.
 *
 *		Joint biasing is the analog of functional or variable
 *		transforms in joint space: a joint bias is a set
 *		of joint angles that is applied as an offset to the
 *		nominal setpoint.
 *
 * CAVEATS:	Joint biasing is still pretty "new".
 *	
 * AUTHOR/DATE:	John Lloyd, Spring 1989
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include "../h/cdefs.h"
#include "../h/rcclDefs.h"
#include "../h/mutex.h"
#include "../h/errorCodes.h"

#include "../rccl/motionRequest.h"
#include "../rccl/rcclAutoAbort.h"
#include "../rccl/rcclErrorMsgs.h"

extern constTrsf();
extern varbTrsf();

jbiasSelect(mnp, on)
MANIP *mnp;
int on;
{
	/* RCCL primitive to enable joint biasing on the next motion */

	char *fxnName = "jbiasSelect()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK*)mnp->mvb;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	mvb->newJbiasEnable = on;
	mvb->newJbiasSetting = YES;
}

setJbiasConst(mnp)
MANIP *mnp;
{
	/* RCCL primitive to declare the joint bias to be "constant" */

	char *fxnName = "setJbiasConst()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK*)mnp->mvb;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (mnp->objectFlag)
	 { errorMsg (ENotDefined, rcclVirtMnpMsg, fxnName);
	   RETURN_ERROR (-1);
	 }
	mvb->newJbiasFxn.fxn = constTrsf;
	mvb->newJbiasSetting = YES;
	RETURN_NORMAL(1);				
}

setJbiasVarb(mnp)
MANIP *mnp;
{
	/* RCCL primitive to declare the joint bias to be "variable" */

	char *fxnName = "setJbiasVarb()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK*)mnp->mvb;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (mnp->objectFlag)
	 { errorMsg (ENotDefined, rcclVirtMnpMsg, fxnName);
	   RETURN_ERROR (-1);
	 }
	mvb->newJbiasFxn.fxn = varbTrsf;
	mvb->newJbiasSetting = YES;
	RETURN_NORMAL(1);				
}

jbiasMotionEval (fxn, arg, mnp)
int (*fxn)();
int arg;
MANIP *mnp;
{
	/* RCCL primitive to bind the joint bias to a function
	   which will be called for all joint interpolated motions
	   on the manipulator 'mnp'.
	 */

	char *fxnName = "jbiasMotionEval()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK*)mnp->mvb;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (mnp->objectFlag)
	 { errorMsg (ENotDefined, rcclVirtMnpMsg, fxnName);
	   RETURN_ERROR (-1);
	 }
	mvb->newJbiasFxn.fxn = fxn;
	mvb->newJbiasFxn.arg = arg;
	mvb->newJbiasSetting = YES;
	RETURN_NORMAL(1);				
}
