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