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