[ return ]

hand.c




/*
 * file : hand.c
 *
 * [DEFINE DESCRIPTION = Hand dynamics simulation]
 *
 *  Name                Date        Description
 *  --------------      --------    -------------------------------------
 *  Andrew H. Fagg      09/05/94    Original
 *
 *
 *   Hand simulation
 *
 */
/*
*
*     Copyright (C) 1995 Andrew H. Fagg (af0a@robotics.usc.edu)
*     
*     This program is free software; you can redistribute it and/or
*     modify it under the terms of the GNU General Public License
*     as published by the Free Software Foundation; either version 2
*     of the License, or any later version.
*     
*     This program is distributed in the hope that it will be useful,
*     but WITHOUT ANY WARRANTY; without even the implied warranty of
*     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*     GNU General Public License for more details.
*     
*     You should have received a copy of the GNU General Public License
*     along with this program; if not, write to the Free Software
*     Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*
*/

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


/*
  * void hand_dynamics(nsl_vector& finger_positions, nsl_vector& input_finger_forces,        //OVERLOAD CALL: hand_dynamics: hand.c(?), hand.orig.c(?)
  *
  *  Compute the next state of the hand.
  *
  */

void hand_dynamics(nsl_vector& finger_positions,
		   nsl_vector& input_finger_desired,
		   nsl_vector& input_finger_degree,
		   nsl_vector& finger_stiffness,
		   nsl_data& position_gain, nsl_data& degree2stiffness,
		   nsl_vector& max_flexion,
		   nsl_vector& min_flexion,
		   nsl_vector& pad_forces,
		   nsl_vector& finger_vel,
		   nsl_data& finger_vel_gain,
		   nsl_data& degree2vel_gain,
		   nsl_data& delta,
		   nsl_vector& input_finger_forces)

{
  int i;

				/* Rotational accel */

				/* Position component*/
  input_finger_forces = ((finger_stiffness +
			 degree2stiffness * input_finger_degree)
    ^ (input_finger_desired - finger_positions))

				/* Velocity component */
      - ((finger_vel_gain +
	 degree2vel_gain * input_finger_degree) ^ finger_vel);

				/* Rotational velocity */
  finger_vel = finger_vel + (delta.elem()/1000.0) * input_finger_forces;

				/* Joint position */
    				/* Make sure within legal range */
  finger_positions = NSLsat(finger_positions +
			    (delta.elem()/1000.0) * finger_vel,
			    -20, 100, -20, 100);
  pad_forces = 0;
				/* Check for collisions */
  for(i = 0; i < HAND_JOINTS; ++i)
    {
      if(finger_positions.elem(i) >= max_flexion.elem(i)
	 || finger_positions.elem(i) <= min_flexion.elem(i))
	{
				/* Collision- reset the position */
	  if(finger_positions.elem(i) >= max_flexion.elem(i))
	    finger_positions.elem(i) = max_flexion.elem(i);
	  else
	    finger_positions.elem(i) = min_flexion.elem(i);
	  finger_vel.elem(i) = 0;

				/* Set pad force levels - assume tip collision */
	  if(i == J_T1)
	    {
	      pad_forces.elem(PAD_T0) = 0.5;
	    }
	  else if(i >= J_I0 && i <= J_I2)
	    {
	      pad_forces.elem(PAD_I2) = 0.5;
	    }
	  else if(i >= J_M0 && i <= J_M2)
	    {
	      pad_forces.elem(PAD_M2) = 0.5;
	    }
	  else if(i >= J_R0 && i <= J_R2)
	    {
	      pad_forces.elem(PAD_R2) = 0.5;
	    }
	  else if(i >= J_L0 && i <= J_L2)
	    {
	      pad_forces.elem(PAD_L2) = 0.5;
	    }
	}
    };
    
}


/*
  * 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;

				/* Put in finger positions */

				/* Index finger */
  hand_display.elem((int) (1+8*finger_positions.elem(J_I0)/100.0),14) = 1.4;

				/* Middle finger */
  hand_display.elem((int) (1+8*finger_positions.elem(J_M0)/100.0),11) = 1.4;

				/* Ring finger */
  hand_display.elem((int) (1+8*finger_positions.elem(J_R0)/100.0),9) = 1.4;

				/* Little finger */
  hand_display.elem((int) (1+8*finger_positions.elem(J_L0)/100.0),7) = 1.4;

				/* Thumb Flexion*/
  hand_display.elem(16, (int) (18-8*finger_positions.elem(J_T1)/100.0)) = 1.4;
				/* Thumb Abduction*/
  hand_display.elem(17, (int) (18-8*finger_positions.elem(J_T0)/100.0)) = 1.4;
}




[ return ]