/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 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:	defaultValues
 *
 * DESCRIPTION:	Routines to compute or estimate default acceleration
 *		and velocity values for a robot, based on information
 *		stored in the JLS structure.
 *
 * AUTHOR/DATE:	John Lloyd, Summer 1989
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include "../h/rcclDefs.h"
#include "../h/robotTypes.h"
#include "../h/rciTask.h"
#include "../h/robot_kynvar.h"
#include "../h/vector.h"

computeDefaultAccel (rbt, acc_j, acc_t, acc_r)
RCI_RBT *rbt;
JNTS *acc_j;
float *acc_t;
float *acc_r;
{
	/* Compute the default acceleration values for a robot.

	   This routine is typically called somewhere during rcclCreate().

	   Read in the joint accelerations and joint velocities directly from
	   its JLS structure. Then, use the manipulator Jacobian at the 
	   ``best'' angles point to determine the desired Cartesian 
	   accelertions. This is done by taking the average of the 
	   acceleration bounds introduced by each joint.

	   The accleration bounds depend on the type of joint:

	   revolute joints:
			translational acceleration is amplified by the
			Jacobian. rotational acceleration bounds stay
			the same.

	   prismatic joints:
			translational acceleration stays the same. No
			bounds are created for rotational acceleration.

	   */

	JLS *jls;
	void *var;
	int numRevoluteJnts, j;
	float taccel;
	float raccel;

	jls = rbt->jls;
	copyVf (acc_j->v, jls->nominalAcc, NUM_JNTS);

	var = (void*)malloc(rbt->varSize);
	updateVar (var, jls->bestAngles, rbt->kyn);
	     
	taccel = 0.0;
	raccel = 0.0;
	numRevoluteJnts = 0;
	for (j = 0; j<NUM_JNTS; j++)
	 { if (jls->jtype[j] == 'R')
	    { JNTS jdisp;
	      DIFF cdisp;

	      /* map that joint's accel into Cart. space */

	      zeroVf (jdisp.v, NUM_JNTS);
	      jdisp.v[j] = 1.0;
	      fwdJacobVar (&cdisp, jdisp.v, var, rbt->kyn);
	      taccel += acc_j->v[j]*normVf ((float*)(&cdisp.t), 3);
	      raccel += acc_j->v[j];
	      numRevoluteJnts++;
	    }
	   else if (jls->jtype[j] == 'P')
	    { taccel += acc_j->v[j];
	    }
	   else
	    { /* should make this an error code */
	      fatalMsg (-1, "%s bogus joint type (%d)\n", jls->jtype[j]);
	    }
	 }
	*acc_t = taccel / NUM_JNTS;
	*acc_r = raccel / numRevoluteJnts;
}

computeDefaultVel (rbt, vel_j, vel_t, vel_r)
RCI_RBT *rbt;
JNTS *vel_j;
float *vel_t;
float *vel_r;
{
	/* Compute the default velocity values for a robot.

	   This is routine typically called somewhere during rcclCreate().

	   At this point, all we do is copy the joint velocties
	   in from the nominal velocity field of the JLS structure,
	   and then use the DEFAULT_TRANS_VEL and DEFAULT_ROT_VEL
	   values for the Cartesian velocities.
	 */
	JLS *jls;

	jls = rbt->jls;
	copyVf (vel_j->v, jls->nominalVel, NUM_JNTS);
	*vel_t = DEFAULT_TRANS_VEL;
	*vel_r = DEFAULT_ROT_VEL;
}
