/*-=-=-=-=-=-=-=-=-=-=-=- 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:	traject.c
 *
 * DESCRIPTION:	Trajectory computation routines for the RCCL trajectory
 *		generator.
 * 
 * 		The best way to understand what is happening here is to read
 *		the paper 
 *
 *			``Trajectory Generation in Multi-RCCL''
 *
 *		which is included in the /doc directory on the distribution
 *		tape.
 *
 * AUTHOR/DATE:	John Lloyd, Fall 1986
 *
 *--------------------------------------------------------------------------*/

#include <math.h>

#include "../h/fastmath.h"
#include "../h/cdefs.h"
#include "../h/robotTypes.h"
#include "../h/robotMath.h"
#include "../h/rciTask.h"
#include "../h/robot_kynvar.h"
#include "../h/errorCodes.h"

#include "../rccl/cartBlend.h"
#include "../rccl/jointBlend.h"
#include "../rccl/setpoint.h"
#include "../rccl/solveEqn.h"
#include "../rccl/kynMacros.h"

#define TRY_JACOB_KYN	0
#define NEWSCALING	0
#define NEWDRIVE	0
#define SCALEINTERP	1

/* The 3 main classes of motion target */
#define TARGET_KNOWN 	1
#define TARGET_B     	2
#define TARGET_STOP	4

/* What the inverse kinematic routine should check for */
#define KYN_CHECK	(KYN_JOINT_LIMIT|KYN_CANT_REACH)

/* Compile in singularity avoidance hack for joint 5 */
#define SINGTEST	1

/* A few abbreviations ... */
#define T6	(&m->t6)
#define J6	(&m->j6)

double cartesianTravelTime();
double jointTravelTime();
DRIVEP *trsfToDrive();

estimateTargetAndTime (m)
register MOTION_PARAMETER_BLOCK *m;
{
	/* Called once at the beginning of a path segment, during the
	   same cycle as, and before, computeInitialSepoints().

	   This routine 

		1. Estimates where the point to which the previous
		   motion was headed. If the target for the previous 
		   motion was constant, and we have not aborted the 
		   motion early, then this is easy ... we simply use the
		   value of the previous motion target. Otherwise, we
		   use the current velocity to create an ``estimate'' of 
		   the target point by extrapolating from where we are
		   now to where we would be at the transition midpoint.

		2. Decides, for motion requests where the time duration 
		   has not been given explicitly, how long the motion 
		   should take. This is done by taking an estimate of the
		   distance the motion will have to cover and dividing by
		   the desired velocities.

		   This code should probably be integrated 
		   more tightly with the code that estimates the acceleration
		   time; there is definitely some redundancy between the two.


	   I say "estimate" target position because there is a bit of a
	   chicken/egg  problem here: to know EXACTLY where the motion target 
	   is (this cycle), it would be necessary to evaluate all the
	   associated transform functions. But some of them may depend on the
	   motion scale, and I don't know the motion scale until I know the
	   motion time ... etc. So I don't actually solve for the target
	   exactly until computeInitialSetpoints(). In this routine, I simply 
	   go through and multiply together the position equation transforms
	   with whatever value happens to currently be in them. The same
	   problem occurs in the 'pendingMotion' routines. The effect of this
	   is that the transition and motion segment time estimates can be 
	   thrown off if a transform function introduces a large change 
	   to its transform during the 

	   What I should do is modify the programmer interface a little to
	   properly handle transforms that depend on the  motion scale ...
	*/		

	char *fxnName = "estimateTargetAndTime()";

	RCI_RBT *rbt = m->rbt;

	/* Compute estimate of Cartesian velocity */

	multLiTrsf (&m->t6del0, T6, &m->oldT6);
	trsfToDspl (&m->t6v0, &m->t6del0);
	scaleDspl (&m->t6v0, -1.0, &m->t6v0);

	switch (m->interMode)
	 { case CARTESIAN:
	    { 
	      m->t60 = *T6;

	      if (m->completedOnSchedule && m->lastTargetFixed)
	       { switch (m->lastInterMode)
		  { case CARTESIAN:
		     { m->tB_est = m->lastTC0;
		       break;
		     }
		    case JOINT:
		     { FWD_KINEMATICS (&m->tB_est, NULL, m->lastJC0.v, NULL);
		       break;
		     }
		  }
	       }

	      else
	       { /* estmate by extrapolation */
		 if (m->tau)
		  { TRSF TMP;

		    extrapTrsfByDspl (&TMP, 2.0*m->tau*m->tauSkew+1.0, 
					    &m->t6v0);
		    multTrsf (&m->tB_est, &m->t60, &TMP);
		  }
		 else
		  { m->tB_est = *T6;
		  }
	       }

#if NEWSCALING == 0
	      if (m->newMotion && m->motionHasTimeLimit == UNDEF)
	       { TRSF disp;
		 int c;
 
		 /* Estimate the distance from the target and combine
		    this with the desired Cartesian velocities to get a 
		    motion time.
		  */

		 if (m->relativePosition)
		  { multTrsf (&m->tC0, &m->tB_est, &m->shiftTransform);
		  }

		 multLiTrsf (&disp, &m->tB_est, &m->tC0);
		 c = cartesianSegmentTime (&disp, m);

		 m->ci.sigma = c;
		 m->motionHasTimeLimit = YES;
	       }
#endif
	      break;
	    }
	   case JOINT:
	    { 
	      if (m->completedOnSchedule && m->lastTargetFixed)
	       { switch (m->lastInterMode)
		  { case CARTESIAN:
		     { m->tB_est = m->lastTC0;
		       INV_KINEMATICS(m->jB_est.v, &m->lastTC0, 
				       m->lastConfigTarget, J6->v, NULL);
		       break;
		     }
		    case JOINT:
		     { m->jB_est = m->lastJC0;
		       FWD_KINEMATICS(&m->tB_est, NULL, m->lastJC0.v,NULL);
		       break;
		     }
		  }
	       }
	      else
	       { /* estimate by extrapolation */
		 if (m->tau)
		  { extrapJnts (&m->jB_est, J6,
				&m->j6vel, 2.0*m->tau*m->tauSkew+1.0);
		  }
		 else
		  { m->jB_est = *J6;
		  }
		 FWD_KINEMATICS (&m->tB_est, NULL, m->jB_est.v, NULL);
	       }
#if NEWSCALING == 0
	      if (m->newMotion && m->motionHasTimeLimit == UNDEF)
	       { JNTS jdistance;
		 int c;

		 /* Estimate the distance from the target and combine
		    this with the desired joint velocities to get a 
		    motion time.
		  */

		 if (m->goalPos)	/* moving to a Cartesian position */
		  { 
		    if (m->relativePosition)
		     { multTrsf (&m->tC0,&m->tB_est,&m->shiftTransform);
		       c = INV_KINEMATICS(m->jC0.v, &m->tC0, 
				   m->configTarget, J6->v, NULL);
		       if (c & KYN_CHECK)
			{ solutionAbort (m, fxnName, c, &m->jC0);
			  return (-1);
			}
		     }
		  }

		 if (m->goalPos)
		  { subtractJnts (&jdistance, &m->jC0, J6);
#if 0
/* jbias already added on in pendingMotion.c */
		    if (m->jbiasEnable)
		     { addJnts (&jdistance, &jdistance, &m->jbias);
		     }
#endif
		  }
		 else			/* just moving to joint angles */
		  { subtractJnts (&jdistance, &m->jointPosition, J6);
		    if (m->jbiasEnable)
		     { addJnts (&jdistance, &jdistance, &m->jbias);
		     }
		  }
		 c = jointSegmentTime (&jdistance, m);
		 m->ci.sigma = c;
		 m->motionHasTimeLimit = YES;
	       }
#endif
	      break;
	    }
	 }
	return (1);
}

computeInitialSetpoints(m)
register MOTION_PARAMETER_BLOCK *m;
{
	/* Called once during the first cycle of every path segment. 
	   Computes the output joint angles for the first cycle, and
	   also does some of the computation of the transitioning
	   blend parameters (if the transition time is non-zero)
	   and some of the computation of the DRIVE parameters (if
	   there is a DRIVE associated with the motion).
	*/

	int c;
	register RCI_RBT *rbt = m->rbt;

	switch (m->interMode)
	 { case CARTESIAN:
	    { TRSF *comp;
	      JNTS j6;
   
	      switch (m->targetType)
	       { case TARGET_KNOWN:
		  { if (m->doDrive)
		     { if (m->newMotion)
			{ SOLVE_COORDi_X_TOOL (&m->DRIVE1,&m->tB_est,m);
#if NEWSCALING
			  if (motionHasTimeLimit == UNDEF)
			   { m->ci.sigma = 
				cartesianSegmentTime (&m->DRIVE1, m);
			     m->motionHasTimeLimit = YES;
			   }
#endif
			}
		       SOLVE_COORD_TOOLi (&m->tC, m);
		     }
		    else
		     { if (m->newMotion)
			{ identTrsf (&m->DRIVE);
			  scaleDrive (&m->drivePar, 0.0, &m->drivePar);
			}
		       SOLVE_COORD_X_TOOLi (&m->tC, &m->DRIVE, m);
		     }
		    break;
		  }
		 case TARGET_STOP:
		  { float scale;

		    if (m->expiringCi.sigma == UNDEF)
		     { scale = 0.0;
		     }
		    else if (m->tau)
		     { scale = (m->expiringCi.c +
				2.0*m->tau*m->tauSkew)/
				(float)m->expiringCi.sigma;
		     }
		    else
		     { scale = m->expiringCi.c/
				(float)m->expiringCi.sigma;
		     }
		    if (scale > 1.0)
		     { scale = 1.0;
		     }

		    if (m->lastInterMode == CARTESIAN)
		     { extrapTrsfByDrive (&m->DRIVE, 1.0-scale,
						       &m->drivePar);
		     }
		    else
		     { rciAbort (EInternal, "mode change when stopping\n");
		     }
		    SOLVE_COORD_X_TOOLi (&m->tC, &m->DRIVE, m);
		    break;
		  }
		 case TARGET_B:
		  { m->tC = m->tB_est;
		    m->jC = m->jB_est;
		    identTrsf (&m->DRIVE);
		    scaleDrive (&m->drivePar, 0.0, &m->drivePar);
		    break;
		  }
	       }
   
	      if (m->tau > 0)
	       { TRSF TMP;

		 if (GET_COORD(m))
		  { m->COORD1 = *m->coord;
		  }
		 else
		  { identTrsf (&m->COORD1);
		  }
		 if (GET_TOOL(m))
		  { invertTrsf (&m->TOOL1_I, m->tool);
		  }
		 else
		  { identTrsf (&m->TOOL1_I);
		  }

		 extrapTrsfByDspl (&TMP, 1.0, &m->t6v0);
		 multTrsf (&m->t61, &m->t60, &TMP);
		 extrapTrsfByDspl (&TMP, 2.0, &m->t6v0);
		 multTrsf (&m->t62, &m->t60, &TMP);

   		 *T6 = m->t61;
	       }
	      else 
	       { 
		 if (m->doDrive)
		  { TRSF TARGET;

		    if (m->newMotion)
		     { trsfToDrive (&m->drivePar, &m->DRIVE1);
		     }
		    extrapTrsfByDrive (&m->DRIVE, 1.0-m->ci.s,
						    &m->drivePar);
		    SOLVE_COORD_X_TOOLi (T6, &m->DRIVE, m);
		  }
		 else
		  { *T6 = m->tC;
		  }
	       }

	      m->tC0 = m->tC;

	      if (rbt)
	       { 
#if TRY_JACOB_KYN
		 TRSF x;
		 DIFF diff;
		 JNTS dj;

		 multLiTrsf (&x, &m->oldT6, T6);
		 trsfToDiff (&diff, &x);
		 invJacobVar (&dj, &diff, rbt->var, rbt->kyn, 0.001);
		 addJnts (&j6, J6, &dj);
#else
		 c = INV_KINEMATICS (j6.v,T6,m->configTarget,J6->v,rbt->var);
		 if ((c & KYN_CHECK) && !(m->mode & RANGE_LIMITING))
		  { char cbuf[4];
		    solutionAbort (m, "initialSetpoints(), mode 'c'",c,&j6);
		    rciPrintTrsf("%m\n", T6);
		    rciPrintf ("config=%x (%s)\n", m->configTarget,
				configToStr(cbuf,m->configTarget,rbt->kyn));
		    return (-1);
		  }
#endif
		 *J6 = j6;
		 m->config = m->configTarget;
		 setManipCondition (m, c);
		 if (c & KYN_WRIST_SING)
		  { solveConf (&m->config, J6->v, rbt->kyn);
		  }
	       }
	      break;
	    }
	   case JOINT:
	    { int tCoff = 0;
	      /* Determine the target */
   
	      switch (m->targetType)
	       { case TARGET_KNOWN:
		  {
		    if (m->goalPos)
		     { SOLVE_COORD_TOOLi (&m->tC, m);
		       c = INV_KINEMATICS (m->jC.v, &m->tC,
			   		   m->configTarget, J6->v, NULL);
		       if (c & KYN_CHECK)
			{ solutionAbort (m, "initialSetpoints(), mode 'j'",
					 c, &m->jC);
			  return (-1);
			}
		     }
		    else
		     { m->jC = m->jointPosition;
		       tCoff = 1;
		     }
		    if (!m->doDrive)
		     { if (m->newMotion)
			{ zeroJnts (&m->jdrive);
			  zeroJnts (&m->driveJnts);
			}
		       else
			{ addJnts (&m->jC, &m->jC, &m->jdrive);
			}
		     }
		    m->jCp0 = m->jC;
		    if (m->jbiasEnable)
		     { addJnts (&m->jC, &m->jC, &m->jbias);
		       tCoff = 1;
		     }
		    if (m->doDrive)
		     { if (m->newMotion)
			{ subtractJnts (&m->jdrive1, &m->jB_est,&m->jC);
#if NEWSCALING
			  if (motionHasTimeLimit == UNDEF)
			   { m->ci.sigma = 
				jointSegmentTime (&m->jdrive1, m);
			     m->motionHasTimeLimit = YES;
			   }
#endif			
			}
		     }
		    break;
		  }
		 case TARGET_STOP:
		  { float scale;

		    if (m->expiringCi.sigma == UNDEF)
		     { scale = 0.0;
		     }
		    else if (m->tau)
		     { scale = (m->expiringCi.c+
				2.0*m->tau*m->tauSkew)/
				(float)m->expiringCi.sigma;
		     }
		    else
		     { scale = m->expiringCi.c/
			       (float)m->expiringCi.sigma;
		     }
		    if (scale > 1.0)
		     { 
/*		       rciPrintf ("SCALE = %g\n", scale); */
		       scale = 1.0;
		     }
		    if (m->lastInterMode == JOINT)
		     { scaleJnts(&m->jdrive, 1.0-scale, &m->driveJnts);
		     }
		    else
		     { rciAbort (EInternal, "mode change when stopping\n");
		       return;
		     }

		    /* Compute jC because we need it anyway */

		    if (m->goalPos)
		     { SOLVE_COORD_TOOLi (&m->tC, m);
		       c = INV_KINEMATICS (m->jC.v, &m->tC,
					   m->configTarget, J6->v, NULL);
		       if (c & KYN_CHECK)
			{ solutionAbort (m, "initialSetpoints(), mode 'j'",
					 c, &m->jC);
			  return (-1);
			}
		     }
		    else
		     { m->jC = m->jointPosition;
		       tCoff = 1;
		     }
		    addJnts (&m->jC, &m->jC, &m->jdrive);
		    m->jCp0 = m->jC;
		    if (m->jbiasEnable)
		     { addJnts (&m->jC, &m->jC, &m->jbias);
		       tCoff = 1;
		     }
		    break;
		  }
		 case TARGET_B:
		  { m->tC = m->tB_est;
		    m->jC = m->jB_est;
		    m->jCp0 = m->jC;
		    zeroJnts (&m->jdrive);
		    zeroJnts (&m->driveJnts);
		    break;
		  }
	       }
   
	      if (m->tau > 0)
	       { extrapJnts (&m->j61, J6, &m->j6vel, 1.0);
		 extrapJnts (&m->j62, J6, &m->j6vel, 2.0);
		 *J6 = m->j61;
	       }
	      else
	       { 
		 if (m->doDrive)
		  { if (m->newMotion)
		     { copyJnts (&m->driveJnts, &m->jdrive1);
		     }
		    scaleJnts (&m->jdrive, 1.0-m->ci.s, &m->driveJnts);
		    addJnts (J6, &m->jC, &m->jdrive);
		  }
		 else
		  { *J6 = m->jC;
		  }
	       }
	      FWD_KINEMATICS (T6, &m->config, J6->v, rbt->var);
	      if (tCoff)
	       { FWD_KINEMATICS (&m->tC, NULL, m->jC.v, NULL);
	       }
	      m->jC0 = m->jC;	/* store jC0 for later */
	      break;
	    }
	 }
	return (1);
}

computeSecondarySetpoints(m)
register MOTION_PARAMETER_BLOCK *m;
{
	/* Called once during the second control cycle of every path
	   segment. Computes the output joint angles for the trajectory.
	   If a DRIVE computation is associated with the motion, then
	   the drive transform is computed here. If the motion has a 
	   non-zero transition time, then the transitioning blend parameters 
	   are also computed here.
	 */

	register RCI_RBT *rbt = m->rbt;	
	int c;

	switch (m->interMode)
	 { case CARTESIAN:
	    { 
	      JNTS j6;
	      if (m->tau == 0)
	       { if (m->doDrive)	
		  { 
#if NEWDRIVE
		    TRSF DRIVE;

		    SOLVE_COORDi_X_TOOL (&DRIVE, &m->tB_est, m);
		    trsfToDrive (&m->drivePar, &DRIVE);
		    if (sigma floating)
		     { m->ci.sigma = straightSegmentCount (&m->drivePar, m);
		       m->ci.s = m->ci.c/(float)(m->ci.sigma - m->ci.c + 1);
		     }
#endif
		    extrapTrsfByDrive (&m->DRIVE, 1.0-m->ci.s, &m->drivePar);
		    if (!m->goalPosFixed)
		     { SOLVE_COORD_TOOLi (&m->tC, m);
		     }
		    SOLVE_COORD_X_TOOLi (T6, &m->DRIVE, m);
		  }
		 else
		  { if (!m->goalPosFixed)
		     { SOLVE_COORD_X_TOOLi (&m->tC, &m->DRIVE, m);
		     }
		    *T6 = m->tC;
		  }
	       }
	      else
	       { TRSF DRIVE1;

		 if (m->doDrive)
		  {
#if NEWDRIVE
		    TRSF DRIVE;
		    float s;

		    DRIVE1 = m->DRIVE;
		    SOLVE_COORDi_X_TOOL (&DRIVE, T6, m);
		    trsfToDrive (&m->drivePar, &DRIVE);
		    if (sigma floating)
		     { m->ci.sigma = straightSegmentCount (&m->drivePar, m);
		       m->ci.s = m->ci.c/(float)(m->ci.sigma - m->ci.c + 1);
		     }
		    extrapTrsfByDrive (&m->DRIVE, 1.0-m->ci.s, &m->drivePar);
#else
		    if (m->newMotion)
		     { /* estimate drive parameters from tB_est to tC(b) */

		       TRSF DRIVEB;
		       TRSF DRIVE2;
		       DIFF DV;
		       
		       SOLVE_COORDi_X_TOOL (&DRIVE2, &m->tB_est, m);
		       trsfToVel (&DV, &m->DRIVE1, &DRIVE2);
		       velToTrsf (&DRIVEB,&m->DRIVE1,
				  2.0*m->tau*m->tauSkew,&DV);
		       trsfToDrive (&m->drivePar, &DRIVEB);
		     }
#if SCALEINTERP == 0
		    if (m->ci.sigma == UNDEF)
		     { /* then scale is always 1.0 */
		       driveToTrsf (&m->DRIVE, &m->drivePar);
		       DRIVE1 = m->DRIVE;
		     }
		    else
#endif
		     { float scale;

		       scale = 1.0 - m->ci.s;
		       extrapTrsfByDrive (&m->DRIVE, scale, &m->drivePar);
		       scale += m->ci.ds;
		       extrapTrsfByDrive (&DRIVE1, scale, &m->drivePar);
		     }
#endif
		    if (!m->goalPosFixed)
		     { SOLVE_COORD_TOOLi (&m->tC, m);
		     }
		  }
		 else
		  { DRIVE1 = m->DRIVE;
		    if (!m->goalPosFixed)
		     { SOLVE_COORD_X_TOOLi (&m->tC, &m->DRIVE, m);
		     }
		  }

		 if (m->blendInT6)
		  { TRSF S2_1; /* S1_0 = T61 */
		    TRSF S2_2; /* S1_1 = T62 */
		    float time;

		    time = 1.0/(float)(m->tau+m->tau);

		    if (m->doDrive)
		     { multTrsf (&S2_1, &m->COORD1, &DRIVE1);
		       multTrsf (&S2_1, &S2_1, &m->TOOL1_I);
		       SOLVE_COORD_X_TOOLi (&S2_2, &m->DRIVE, m);
		     }
		    else
		     { S2_1 = m->tC0;
		       S2_2 = m->tC;
		     }
		    setCartesianBlend1 (&m->cbp, &m->t61, &S2_1);
		    setCartesianBlend2 (&m->cbp, &m->t62, &S2_2,time);

		    applyCartesianBlend (T6, &m->cbp, &S2_2, time);
		  }
		 else
		  { /* DO NOT USE THIS OPTION. Code has not been 
		       tested, and there are some bugs with the idea of 
		       not blending in the T6 frame.
		     */
		    TRSF S2_1, S1_1;
		    TRSF S2_2, S1_2;
		    TRSF T;
		    float time;

		    time = 1.0/(float)(m->tau+m->tau);

		    multRiTrsf (&S1_1, &m->t61, &m->TOOL1_I);
		    multTrsf (&S2_1, &m->COORD1, &DRIVE1);

		    setCartesianBlend1 (&m->cbp, &S1_1, &S2_1);

		    SOLVE_X_TOOL (&S1_2, &m->t62, m);
		    SOLVE_COORD_X (&S2_2, &m->DRIVE, m);

		    setCartesianBlend2 (&m->cbp, &S1_2, &S2_2, time);

		    applyCartesianBlend (&T, &m->cbp, &S2_2, time);
		    SOLVE_X_TOOLi (T6, &T, m);
		  }
	       }

	      if (rbt)
	       { 
#if TRY_JACOB_KYN
		 TRSF x;
		 DIFF diff;
		 JNTS dj;

		 multLiTrsf (&x, &m->oldT6, T6);
		 trsfToDiff (&diff, &x);
		 invJacobVar (&dj, &diff, rbt->var, rbt->kyn, 0.001);
		 addJnts (&j6, J6, &dj);
#else
		 c = INV_KINEMATICS (j6.v,T6,m->configTarget,J6->v,rbt->var);
		 if ((c & KYN_CHECK) && !(m->mode & RANGE_LIMITING))
		  { solutionAbort (m, "secondarySetpoints(), mode 'c'", c, &j6);
		    return (-1);
		  }
#endif
		 *J6 = j6;
		 m->config = m->configTarget;
		 setManipCondition (m, c);
		 if (c & KYN_WRIST_SING)
		  { solveConf (&m->config, J6->v, rbt->kyn);
		  }
	       }
	      break;
	    }
	   case JOINT:
	    { int tCoff = 0;
	      
	      if (m->goalPos && !m->goalPosFixed)	
	       { /* track motion of target position */
		 SOLVE_COORD_TOOLi (&m->tC, m);
		 c = INV_KINEMATICS (m->jC.v,&m->tC,m->configTarget,
			 	     J6->v, NULL);
		 if (c & KYN_CHECK)
		  { solutionAbort (m, "secondarySetpoints(), mode 'j'", 
				   c, &m->jC);
		    return (-1);
		  }
		 if (!m->doDrive)
		  { addJnts (&m->jC, &m->jC, &m->jdrive);
		    tCoff = 1;
		  }
	       }
	      else
	       { m->jC = m->jCp0;
	       }
	      if (m->jbiasEnable)
	       { addJnts (&m->jC, &m->jC, &m->jbias);
		 tCoff = 1;
	       }
		 
	      if (m->tau == 0)
	       { 
		 if (m->doDrive)
		  { 
#if NEWDRIVE
		    subtractJnts (&m->driveJnts, &m->jB_est, &m->jC);
		    if (sigma floating)
		     { m->ci.sigma = straightJointCount (&jdrive, m);
		       m->ci.s = m->ci.c/(float)(m->ci.sigma - m->ci.c + 1);
		     }
#endif
		    scaleJnts (&m->jdrive, 1.0-m->ci.s, &m->driveJnts);
		    addJnts (J6, &m->jC, &m->jdrive);
		  }
		 else
		  { *J6 = m->jC;
		  }
	       }
	      else
	       { JNTS jtarget2;
		 JNTS jtarget1;
		 float time;

		 time = 1.0 / (double)(m->tau+m->tau);

		 if (m->doDrive)
		  { 
#if NEWDRIVE
		    JNTS jdrive;
		    float s;

		    subtractJnts (&jdrive, J6, &m->jC);
		    if (sigma floating)
		     { int c;
		       c = straightJointCount (&jdrive, m);
		       m->ci.sigma = MAX(c+m->ci.c+1, 2);
		     }
		    s = (m->ci.sigma == UNDEF ? 0.0 : 1.0-1/(m->ci.sigma-m->ci.c);
		    scaleJnts (&m->jdrive, s, &jdrive);

		    if (m->ci.sigma == UNDEF)
		     { /* then scale is always 1.0 */
		       m->jdrive = m->driveJnts;
		       addJnts (&jtarget2, &m->jC, &m->jdrive);
		       addJnts (&jtarget1, &m->jC0, &m->jdrive);
		     }
		    else
		     { float scale = 1.0 - m->ci.s;
		       scaleJnts (&m->jdrive, scale, &m->driveJnts);
		       addJnts (&jtarget2, &m->jC, &m->jdrive);
		       scale += 1/(float)m->ci.sigma;
		       extrapJnts (&jtarget1, &m->jC0,
				   &m->driveJnts, scale);
		     }
#else
		    if (m->newMotion)
		     { JNTS deltaJC2;
		       JNTS deltaJCvel;

		       subtractJnts (&deltaJC2, &m->jB_est, &m->jC);
		       subtractJnts (&deltaJCvel, &deltaJC2, &m->jdrive1);
		       extrapJnts (&m->driveJnts, &m->jdrive1,
				   &deltaJCvel, 2.0*m->tau*m->tauSkew);
		     }
#if SCALEINTERP == 0
		    if (m->ci.sigma == UNDEF)
		     { /* then scale is always 1.0 */
		       m->jdrive = m->driveJnts;
		       addJnts (&jtarget2, &m->jC, &m->jdrive);
		       addJnts (&jtarget1, &m->jC0, &m->jdrive);
		     }
		    else
#endif
		     { float scale;

		       scale = 1.0 - m->ci.s;
		       scaleJnts (&m->jdrive, scale, &m->driveJnts);
		       addJnts (&jtarget2, &m->jC, &m->jdrive);
		       scale += m->ci.ds;
		       extrapJnts (&jtarget1, &m->jC0,
				   &m->driveJnts, scale);
		     }
#endif
		  }
		 else
		  { jtarget2 = m->jC;
		    jtarget1 = m->jC0;
		  }

		 setJointBlend1 (&m->jbp, &m->j61, &jtarget1);
		 setJointBlend2 (&m->jbp, &m->j62, &jtarget2, time);
		 applyJointBlend (J6, &m->jbp, &jtarget2, time);
	       }
	      FWD_KINEMATICS (T6, &m->config, J6->v, rbt->var);
	      if (tCoff)
	       { FWD_KINEMATICS (&m->tC, NULL, m->jC.v, NULL);
	       }
	      break;
	    }
	 }
	return (1);
}

computeNominalSetpoints(m)
register MOTION_PARAMETER_BLOCK *m;
{
	/* Called once for every control cycle of path segment after
	   the second, to compute the output joint angles
	   for the trajectory.
	 */

	register RCI_RBT *rbt = m->rbt;
	int c;

	switch (m->interMode)
	 { case CARTESIAN:
	      { JNTS j6;

		if (m->doDrive)
		 { 
#if NEWDRIVE
		   TRSF DRIVE;

		   SOLVE_COORDi_X_TOOL (&DRIVE, &m->tB_est, m);
		   trsfToDrive (&m->drivePar, &DRIVE);
		   if (sigma floating)
		    { m->ci.sigma = straightSegmentCount (&m->drivePar, m);
		      m->ci.s = m->ci.c/(float)(m->ci.sigma - m->ci.c + 1);
		    }
#endif
		   extrapTrsfByDrive (&m->DRIVE, 1.0-m->ci.s, &m->drivePar);
		   if (!m->goalPosFixed)
		    { SOLVE_COORD_TOOLi (&m->tC, m);
		    }
		   SOLVE_COORD_X_TOOLi (T6, &m->DRIVE, m);
		 }
		else
		 { if (!m->goalPosFixed)
		    { SOLVE_COORD_X_TOOLi (&m->tC, &m->DRIVE, m);
		    }
		   *T6 = m->tC;
		 }
		if (m->cycle < 2*m->tau)
		 { double time = m->cycle / (double)(2*m->tau);

		   if (m->blendInT6)
		    { applyCartesianBlend (T6, &m->cbp, T6, time);
		    }
		   else
		    { /* DO NOT USE THIS OPTION. Code has not been 
			 tested, and there are some bugs with the idea of 
			 not blending in the T6 frame.
		      */
		      TRSF T;
		      SOLVE_X_TOOL (&T, T6, m);
		      applyCartesianBlend (&T, &m->cbp, &T, time);
		      SOLVE_X_TOOLi (T6, &T, m);
		    }
		 }
		if (rbt)
		 { 
#if TRY_JACOB_KYN
		   TRSF x;
		   DIFF diff;
		   JNTS dj;

		   multLiTrsf (&x, &m->oldT6, T6);
		   trsfToDiff (&diff, &x);
		   invJacobVar (&dj, &diff, rbt->var, rbt->kyn, 0.001);
		   addJnts (&j6, J6, &dj);
#else
		   c = INV_KINEMATICS(j6.v,T6,m->configTarget,J6->v,rbt->var);
		   if ((c & KYN_CHECK) && !(m->mode & RANGE_LIMITING))
		    { JNTS deg;

		      solutionAbort (m, "nominalSetpoints(), mode 'c'", c, &j6);
		      return (-1);
		    }
#endif
		   *J6 = j6;
		   m->config = m->configTarget;
		   setManipCondition (m, c);
		   if (c & KYN_WRIST_SING)
		    { solveConf (&m->config, J6->v, rbt->kyn);
		    }
		 }
		break;
	      }
	   case JOINT:
	      { int tCoff = 0;

		if (m->goalPos && !m->goalPosFixed)
		 { /* track motion of target position */
		   SOLVE_COORD_TOOLi (&m->tC, m);
		   c = INV_KINEMATICS (m->jC.v, &m->tC, m->configTarget,
				       J6->v, NULL);
		   if (c & KYN_CHECK)
		    { solutionAbort (m, "nominalSetpoints(), mode 'j'", 
				     c, &m->jC);
		      return (-1);
		    }
		   if (!m->doDrive)
		    { addJnts (&m->jC, &m->jC, &m->jdrive);
		      tCoff = 1;
		    }
		 }
		else
		 { m->jC = m->jCp0;
		 }
		if (m->jbiasEnable)
		 { addJnts (&m->jC, &m->jC, &m->jbias);
		   tCoff = 1;
		 }
	 	if (m->doDrive)
		 { 
#if NEWDRIVE
		   subtractJnts (&m->driveJnts, &m->jB_est, &m->jC);
		   if (sigma floating)
		    { m->ci.sigma = straightJointCount(&jdrive, m);
		      m->ci.s = m->ci.c/(float)(m->ci.sigma - m->ci.c + 1);
		    }
#endif
		   scaleJnts (&m->jdrive, 1.0-m->ci.s, &m->driveJnts);
		   addJnts (J6, &m->jC, &m->jdrive);
		 }
		else
		 { *J6 = m->jC;
		 }
		if (m->cycle < 2*m->tau)
		 { double time = m->cycle / (double)(2*m->tau);
	           applyJointBlend (J6, &m->jbp, J6, time);
		 }
		FWD_KINEMATICS (T6, &m->config, J6->v, rbt->var);
		if (tCoff)
		 { FWD_KINEMATICS (&m->tC, NULL, m->jC.v, NULL);
		 }
		break;
	      }
	 }
	return (1);
}

solutionAbort (m, s, c, j6)
MOTION_PARAMETER_BLOCK *m;
char *s;
int c;
JNTS *j6;
{
	/* A bit of a hack routine for printing out diagnostic messages
	   and aborting the program when a solution limit is hit by
	   the inverse kinematics function.
	*/

	if (c & KYN_CANT_REACH)
	 { rciAbort (ESingular, "%s -- target out of reach (code=0x%x)\n",s,c);
	 }
	else if (c & KYN_JOINT_LIMIT)
	 { extern char *jointDescStr();
	   char buf[128];
	   JNTS deg;


	   radianToDegree (deg.v, j6->v, m->rbt->jls);
	   rciPrintVf ("wanted to go to: %8.3f\n", deg.v, 6);
	   rciAbort (ELimit, "%s -- joint limit hit on %s\n",
		     s, jointDescStr(buf, c & KYN_JOINT_LIMIT));
	 }
	else if (c & KYN_SINGULAR)
	 { rciAbort (ESingular, "%s -- singularity (code=0x%x)\n", s, c);
	 }
	else
	 { rciAbort (EInternal, "%s -- unknown limit code (0x%x)\n", s, c);
	 }
}

cartesianSegmentTime (disp, m)
TRSF *disp;
register MOTION_PARAMETER_BLOCK *m;
{
	MOTION_CONTROL_BLOCK *mcb;

	float accelTime;
	DSPL dspl;
	float d_t;
	float d_r;
	float timeTrans;
	float timeRot;
	int segCount;
	int new;

	mcb = m->mcb;
	new = (mcb->pathsegIndex&1)^1;

	accelTime = m->tau * rcclGetInterval() / 500.0;
	trsfToDspl (&dspl, disp);
	d_t =   SQRT(dspl.p.x*dspl.p.x + dspl.p.y*dspl.p.y + dspl.p.z*dspl.p.z)
	      - mcb->activeTransVel[new] * accelTime;
	if (d_t > 0.0)
	 { timeTrans = accelTime + d_t / mcb->activeTransVel[new];
	 }
	else
	 { timeTrans = accelTime;
	 }
	d_r = FABS(dspl.a) - mcb->activeRotVel[new] * accelTime;
	if (d_r > 0.0)
	 { timeRot = accelTime + d_r / mcb->activeRotVel[new];
	 }
	else
	 { timeRot = accelTime;
	 }
	segCount = NINT(MAX(timeRot, timeTrans)*1000.0/(float)rcclGetInterval());
	return (MAX(segCount, 2));
}

jointSegmentTime (disp, m)
JNTS *disp;
register MOTION_PARAMETER_BLOCK *m;
{
	MOTION_CONTROL_BLOCK *mcb;

	float *jvel;
	float accelTime;
	float d_j;
	float time;
	float time_max;
	int segCount;
	int new;
	int k;

	mcb = m->mcb;
	new = (mcb->pathsegIndex&1)^1;

	accelTime = m->tau * rcclGetInterval() / 500.0;
	time_max = 0.0;
	jvel = mcb->activeJointVel[new].v;
	for (k=0; k<NUM_JNTS; k++)
	 { d_j = FABS(disp->v[k]) - jvel[k] * accelTime;
	   if (d_j > 0.0)
	    { time = d_j / jvel[k];
	      if (time > time_max)
	       { time_max = time;
	       }
	    }
	 }
	segCount = NINT((time_max+accelTime)*1000.0/(float)rcclGetInterval());
	return (MAX(segCount, 2));
}
