[ return ]

hand.orig.c



/*
 * file : hand.c
 *
 * [DEFINE DESCRIPTION = Hand dynamics simulation]
 *
 *  Name                Date        Description
 *  --------------      --------    -------------------------------------
 *  Andrew H. Fagg      09/05/94    Original
 *
 *
 *   Hand simulation
 *
 *  Each finger is represented by a single state variable: flexion
 * (0 = full flexion, 1 = full extension).
 *  Objects are represented by
 *     - the flexion values at which contact will be made.
 *     - whether the surface is curved or cornered (square).
 */

#include "nsl_include.h"
#include "alib.h"
#include "gen.h"
#include "net.h"
#include "hand.h"
#include "math.h"

/*
  * nsl_vector compute_object_forces(nsl_vector& finger_positions,
  *
  *  Computes the backdriving force of each finger due to contact
  * with the object.  For each finger, a flexion level is provided at
  * which contact with the object is made.  If this level is reached,
  * then we model the force produced by the object as a spring.
  * If the object does not contact the hand for a particular finger,
  * then the width is set to -1.
  *
  */

nsl_vector compute_object_forces(nsl_vector& finger_positions,
				 nsl_vector& object_parms,
				 int grasp_type,
				 nsl_data force_igain,
				 int hand_enclosed_flag)

{
  nsl_vector forces(HAND_JOINTS);
  forces = 0;
  if(!hand_enclosed_flag)
    return(forces);

				/* Thumb - palm opposition  */

  if(finger_positions.elem(JOINTS_TH_FL) < object_parms.elem(OBJECT_W0)
     && (grasp_type & GRASP_PALM) && !(grasp_type & GRASP_SIDE))
    {
      forces.elem(JOINTS_TH_FL) =
	NSLsat(object_parms.elem(OBJECT_W0)
	       - finger_positions.elem(JOINTS_TH_FL),
	       0, force_igain.elem(), 0, 1);
      
    }

				/* Thumb - side opposition  */

  if(finger_positions.elem(JOINTS_TH_FL) < object_parms.elem(OBJECT_W0)
     && (grasp_type & GRASP_SIDE))
    {
      forces.elem(JOINTS_TH_FL) =
	NSLsat(object_parms.elem(OBJECT_W0)
	       - finger_positions.elem(JOINTS_TH_FL),
	       0, force_igain.elem(), 0, 1);
      
    }



				/* Index finger. - palm opposition  */
  if(finger_positions.elem(JOINTS_IN_FL) < object_parms.elem(OBJECT_W1)
     && grasp_type & GRASP_PALM)
    {
      forces.elem(JOINTS_IN_FL) =
	NSLsat(object_parms.elem(OBJECT_W1)
	       - finger_positions.elem(JOINTS_IN_FL),
	       0, force_igain.elem(), 0, 1);
      
    }
				/* Middle finger - palm opposition  */
  if(finger_positions.elem(JOINTS_MD_FL) < object_parms.elem(OBJECT_W2))
    {
      forces.elem(JOINTS_MD_FL) =
	NSLsat(object_parms.elem(OBJECT_W2)
	       - finger_positions.elem(JOINTS_MD_FL),
	       0, force_igain.elem(), 0, 1);
      
    }
				/* Ring finger  - palm opposition */
  if(finger_positions.elem(JOINTS_RG_FL) < object_parms.elem(OBJECT_W3))
    {
      forces.elem(JOINTS_RG_FL) =
	NSLsat(object_parms.elem(OBJECT_W3)
	       - finger_positions.elem(JOINTS_RG_FL),
	       0, force_igain.elem(), 0, 1);
      
    }
				/* Little finger - palm opposition */
  if(finger_positions.elem(JOINTS_LI_FL) < object_parms.elem(OBJECT_W4))
    {
      forces.elem(JOINTS_LI_FL) =
	NSLsat(object_parms.elem(OBJECT_W4)
	       - finger_positions.elem(JOINTS_LI_FL),
	       0, force_igain.elem(), 0, 1);
      
    }

  return(forces);
}

/*
  * void compute_finger_pad_forces(nsl_vector& object_forces,
  *
  *  Translates finger backlash forces and the object cross-section
  * shape into a pattern of activity in <pad_forces>.
  *
  */

void compute_finger_pad_forces(nsl_vector& object_forces,
			       nsl_vector& object_parms,
			       int grasp_type,
			       nsl_vector& pad_forces,
			       nsl_data& corner_offset,
			       nsl_data& corner_gain,
			       nsl_data& width_gain,
			       nsl_data& corner_force_const)
{
  int i;
  float tmp;

  pad_forces = 0;
				/* FINGERS */
				/* Round  object - even distribution. */
  if(object_parms.elem(OBJECT_CROSS_SHAPE) == OBJECT_CS_ROUND)
    {
				/* Index finger */
      if(object_forces.elem(JOINTS_IN_FL) > 0.0)
	{
	  for(i = IN_START; i < IN_START+IN_LEN; ++i)
	      pad_forces.elem(i) = object_forces.elem(JOINTS_IN_FL);
	}
      
				/* Middle finger */
      if(object_forces.elem(JOINTS_MD_FL) > 0.0)
	{
	  for(i = MD_START; i < MD_START+MD_LEN; ++i)
	      pad_forces.elem(i) = object_forces.elem(JOINTS_MD_FL);
	}
      
				/* Ring finger */
      if(object_forces.elem(JOINTS_RG_FL) > 0.0)
	{
	  for(i = RG_START; i < RG_START+IN_LEN; ++i)
	      pad_forces.elem(i) = object_forces.elem(JOINTS_RG_FL);
	}
      
				/* Little finger */
      if(object_forces.elem(JOINTS_LI_FL) > 0.0)
	{
	  for(i = LI_START; i < LI_START+IN_LEN; ++i)
	      pad_forces.elem(i) = object_forces.elem(JOINTS_LI_FL);
	}
    }
				/* Square cross-section - other distribution. */
  else if(object_parms.elem(OBJECT_CROSS_SHAPE) == OBJECT_CS_CUBE)
    {
				/* Index finger */
      if(object_forces.elem(JOINTS_IN_FL) > 0.0)
	{
	  for(i = IN_START; i < IN_START+IN_LEN; ++i)
	      pad_forces.elem(i) = object_forces.elem(JOINTS_IN_FL)
		* (1 + corner_force_const.elem() +
		   cos((i-IN_START+corner_offset.elem())
		       * (1-object_parms.elem(OBJECT_W1))
		       * corner_gain.elem()))/(2+corner_force_const.elem());
	}
      
				/* Middle finger */
      if(object_forces.elem(JOINTS_MD_FL) > 0.0)
	{
	  for(i = MD_START; i < MD_START+MD_LEN; ++i)
	      pad_forces.elem(i) = object_forces.elem(JOINTS_MD_FL)
		* (1 + corner_force_const.elem() +
		   cos((i-MD_START+corner_offset.elem())
		       * (1-object_parms.elem(OBJECT_W2))
		       * corner_gain.elem()))/(2+corner_force_const.elem());
	}
      
      
				/* Ring finger */
      if(object_forces.elem(JOINTS_RG_FL) > 0.0)
	{
	  for(i = RG_START; i < RG_START+MD_LEN; ++i)
	      pad_forces.elem(i) = object_forces.elem(JOINTS_RG_FL)
		* (1 + corner_force_const.elem() +
		   cos((i-RG_START+corner_offset.elem())
		       * (1-object_parms.elem(OBJECT_W3))
		       * corner_gain.elem()))/(2+corner_force_const.elem());
	}
      
				/* Little finger */
      if(object_forces.elem(JOINTS_LI_FL) > 0.0)
	{
	  for(i = LI_START; i < LI_START+MD_LEN; ++i)
	      pad_forces.elem(i) = object_forces.elem(JOINTS_LI_FL)
		* (1 + corner_force_const.elem() +
		   cos((i-LI_START+corner_offset.elem())
		       * (1-object_parms.elem(OBJECT_W4))
		       * corner_gain.elem()))/(2+corner_force_const.elem());
	}
    } /* Assume that for flat objects, there are no forces */

				/* Thumb and side of index finger. */
				/* Thumb pad */
  pad_forces.elem(TH_START) = object_forces.elem(JOINTS_TH_FL);

				/* Side of index finger. */
  if((grasp_type & GRASP_SIDE) && !(grasp_type & GRASP_PALM))
    {

				/* Side of index finger. */
				/* Width of force field proportional to */
				/*  width of object. */
      for(i = IN_SIDE_START; i < IN_SIDE_START+IN_SIDE_LEN; ++i)
	{
	  tmp = i-IN_SIDE_START-IN_SIDE_LEN/2; /* offset from center of finger */

	  if(Abs(tmp) < width_gain.elem() * object_parms.elem(OBJECT_W1))
	      pad_forces.elem(i) = object_forces.elem(JOINTS_TH_FL);
	}
    }
}

void update_finger_positions(nsl_vector& finger_positions, nsl_vector forces,
			     nsl_data& position_gain)
{
  finger_positions = NSLsat(finger_positions + position_gain * forces);
}



/*
  * void hand_dynamics(nsl_vector& finger_positions, nsl_vector& input_finger_forces,        //OVERLOAD CALL: hand_dynamics: hand.c(?), hand.orig.c(?)
  *
  *   Given the position of the fingers, and the config of the object,
  * compute the forces on the hand, and on the pads on the fingers.
  * Then compute the next position of the fingers (given external and
  * object forces).
  *
  *   If <hand_enclosed_flag> is FALSE, this indicates that the hand
  * is not near the object, therefore no forces should be sensed.
  */

void hand_dynamics(nsl_vector& finger_positions, nsl_vector& input_finger_forces,
	      nsl_vector& object_forces,
	      nsl_vector& object_parms, int grasp_type, nsl_vector& pad_forces,
	      nsl_data& force_igain, nsl_data& corner_offset,
	      nsl_data& corner_gain, nsl_data& width_gain,
	      nsl_data& position_gain, nsl_data& corner_force_const,
		   int hand_enclosed_flag)


{
  object_forces = compute_object_forces(finger_positions,
					object_parms,
					grasp_type, force_igain,
					hand_enclosed_flag);
  object_forces = NSLsat(object_forces, -1, 1, -1, 1);

  compute_finger_pad_forces(object_forces, object_parms, grasp_type, pad_forces,
			    corner_offset, corner_gain,
			    width_gain, corner_force_const);

  update_finger_positions(finger_positions, input_finger_forces + object_forces,
			  position_gain);

}


/*
  * void compute_hand_display(nsl_vector& pad_forces, nsl_matrix& hand_display)        //OVERLOAD CALL: compute_hand_display: hand.c(?), hand.orig.c(?)
  *
  *  Convert tbe pad forces into a 2D layout for display.  Set background
  * to 2.
  *
  */

void compute_hand_display(nsl_vector& pad_forces, nsl_matrix& hand_display,
			  nsl_vector& finger_positions)
{
  int i;

  hand_display = 1.5;		/* All background elements are 2. */

				/* White space to complete palm. */
  for(i = 6; i < 15; ++i)
    {
      hand_display.elem(15, i) = 0;
      hand_display.elem(14, i) = 0;
      hand_display.elem(13, i) = 0;
    }
  hand_display.elem(15, 14) = 1.5;

				/* Thumb */
  hand_display.elem(14, 15) = pad_forces.elem(TH_START);
  hand_display.elem(14, 16) = pad_forces.elem(TH_START);
  hand_display.elem(13, 15) = pad_forces.elem(TH_START);
  hand_display.elem(13, 16) = pad_forces.elem(TH_START);

				/* Index finger - side. */
  for(i = 0; i < IN_SIDE_LEN; ++i)
    hand_display.elem(12-i, 13) = pad_forces.elem(i+IN_SIDE_START);

				/* Index finger - pads */
  for(i = 0; i < IN_LEN; ++i)
    hand_display.elem(12-i, 12) = pad_forces.elem(i+IN_START);
  for(i = 0; i < 3; ++i)
    hand_display.elem(12-i, 11) = pad_forces.elem(i+IN_START);

				/* Middle finger */
  for(i = 0; i < MD_LEN; ++i)
    hand_display.elem(12-i, 10) = pad_forces.elem(i+MD_START);
  for(i = 0; i < 3; ++i)
    hand_display.elem(12-i, 9) = pad_forces.elem(i+MD_START);

				/* Ring finger */
  for(i = 0; i < RG_LEN; ++i)
    hand_display.elem(12-i, 8) = pad_forces.elem(i+RG_START);
  for(i = 0; i < 3; ++i)
    hand_display.elem(12-i, 7) = pad_forces.elem(i+RG_START);

				/* Little finger */
  for(i = 0; i < LI_LEN; ++i)
    hand_display.elem(12-i, 6) = pad_forces.elem(i+LI_START);


				/* Put in finger positions */

				/* Index finger */
  hand_display.elem((int) (9-8*finger_positions.elem(JOINTS_IN_FL)),14) = 1.4;

				/* Middle finger */
  hand_display.elem((int) (9-8*finger_positions.elem(JOINTS_MD_FL)),11) = 1.4;

				/* Ring finger */
  hand_display.elem((int) (9-8*finger_positions.elem(JOINTS_RG_FL)),9) = 1.4;

				/* Little finger */
  hand_display.elem((int) (9-8*finger_positions.elem(JOINTS_LI_FL)),7) = 1.4;

				/* Thumb Flexion*/
  hand_display.elem(16, (int) (10+8*finger_positions.elem(JOINTS_TH_FL))) = 1.4;
				/* Thumb Abduction*/
  hand_display.elem(17, (int) (10+8*finger_positions.elem(JOINTS_TH_AB))) = 1.4;
}


[ return ]