import vrml.*; import vrml.node.*; import vrml.field.*; import java.math.*; import blueprint; /*** Name: Conrad Chu and Daniel Lim * Login: cs184-cc cs184-bu * PROJECT 1 */ public class project1 extends Script { /*** FIELDS ***/ private SFRotation shoulder_CurrentRotation; //The current rotation of the shoulder private SFRotation bicep_CurrentRotation; //The current rotation of the bicep private SFRotation forearm_CurrentRotation; //The current rotation of the forearm /*** EVENT OUTS ***/ private SFRotation shoulder_RotationChanged; //The rotation applied to the shoulder private SFRotation bicep_RotationChanged; //The rotation applied to the bicep private SFRotation forearm_RotationChanged; //The rotation applied to the forearm private SFRotation hand_RotationChanged; //The rotation applied to the hand /** EVENT INS ***/ private SFBool reset; //Reset system private SFBool map1; private SFBool map2; private SFBool map3; /** VARIABLES ***/ private blueprint map = new blueprint(); //Map configurations imported in private SFVec3f [] objects; //Array of object translations private SFVec3f [] objects_EventOuts; //Array of the evenouts for the objects private float shoulder_NewRotation; //The rotation float that is sent to shoulder_RotationChanged private float bicep_NewRotation; //The rotation float that is sent to bicep_RotationChanged private float forearm_NewRotation; //The rotation float that is sent to forearm_RotationChanged private float hand_NewRotation; //The rotation float that is sent to hand_RotationChanged private float object_NewTranslation; //The translation of the object private float shoulder_CurrentRotationKeyframe; //The rotation float for the current shoulder keyframe private float bicep_CurrentRotationKeyframe; //The rotation float for the current bicep keyframe private float forearm_CurrentRotationKeyframe; //The rotation float for the current forearm keyframe private float shoulder_NextRotationKeyframe; //The rotation float for the next shoulder keyframe private float bicep_NextRotationKeyframe; //The rotation float for the next bicep keyframe private float forearm_NextRotationKeyframe; //The rotation float for the next forearm keyframe private boolean inbetweening=true; //If robot is middle of interpolating private boolean isGrabbed=false; //Boolean if object is being grabbed or not private boolean getNextKeyframe=true; //Boolean of getting the next keyframe private int obj_count=0; //Number of object being handled, 0... 100 private float frame_num = 0.0f; //Current frame number in interpolation private float r; //distance from (0,0) to object private int map_num=0; //Map number /** CONSTANTS **/ private final float FRAME_MAX = 50.0f; //Resolution of interpolation private final float bicep_length = 3.1f; private final float forearm_length = 1.7f; public void initialize() { try { //Initalize variables objects = new SFVec3f[map.NUM_OBJECTS]; //Define array of object translations objects_EventOuts = new SFVec3f[map.NUM_OBJECTS]; //Initalize event outs shoulder_RotationChanged = (SFRotation)(getEventOut("shoulder_RotationChanged")); bicep_RotationChanged = (SFRotation)(getEventOut("bicep_RotationChanged")); forearm_RotationChanged = (SFRotation)(getEventOut("forearm_RotationChanged")); hand_RotationChanged = (SFRotation)(getEventOut("hand_RotationChanged")); for(int i=0; i<map.NUM_OBJECTS;i++){ objects_EventOuts[i] = (SFVec3f)(getEventOut("object" + i + "_translation")); //init eventouts objects[i] = (SFVec3f)(getField("object" + i)); //init fields } //Initalize fields shoulder_CurrentRotation = (SFRotation)(getField("shoulder_CurrentRotation")); bicep_CurrentRotation = (SFRotation)(getField("bicep_CurrentRotation")); forearm_CurrentRotation = (SFRotation)(getField("forearm_CurrentRotation")); //Initialize rotations float temp[] = new float[4]; shoulder_CurrentRotation.getValue(temp); //Set the current rotation of the shoulder to shoulder_CurrentRotationKeyframe shoulder_CurrentRotationKeyframe = temp[3]; bicep_CurrentRotation.getValue(temp); //Set the current rotation of the bicep to bicep_CurrentRotationKeyframe bicep_CurrentRotationKeyframe = temp[3]; forearm_CurrentRotation.getValue(temp); //Set the current rotation of the forearm to forearm_CurrentRotationKeyframe forearm_CurrentRotationKeyframe = temp[3]; } catch(InvalidFieldException e) { System.out.println("invalid initialization"); System.exit(1); } } public void shoulder_SetRotation(float vals[]) { shoulder_CurrentRotation.setValue(vals); shoulder_RotationChanged.setValue(vals); } public void bicep_SetRotation(float vals[]) { bicep_CurrentRotation.setValue(vals); bicep_RotationChanged.setValue(vals); } public void forearm_SetRotation(float vals[]) { forearm_CurrentRotation.setValue(vals); forearm_RotationChanged.setValue(vals); } private void kinematics(float set_fraction){ Interpolate(); //Interpolate new rotations for shoulder, bicep, and forearm shoulder_RotationChanged.setValue(0.0f, 1.0f, 0.0f, shoulder_NewRotation); bicep_RotationChanged.setValue(0.0f, 1.0f, 0.0f, -bicep_NewRotation); forearm_RotationChanged.setValue(0.0f, 1.0f, 0.0f, -forearm_NewRotation); hand_RotationChanged.setValue(0.0f, 1.0f, 0.0f, hand_NewRotation); if(isGrabbed){ float [] temp = new float[3]; temp = getEndEffector(); objects_EventOuts[obj_count].setValue(getEndEffector()); } } private void reset(boolean bval) { //Reset all values to initial conditions if(bval){ for(int i=0; i<map.NUM_OBJECTS;i++){ objects_EventOuts[i].setValue(objects[i]); } getNextKeyframe = true; obj_count = 0; isGrabbed = false; frame_num = 0.0f; inbetweening=true; //If robot is middle of interpolating } } private void map1(boolean bval) { //Use the first map configuration if(bval){ reset(bval); map_num = 0; } } private void map2(boolean bval) { //Use the second map configuration if(bval){ reset(bval); map_num = 1; } } private void map3(boolean bval) { //Use the third map configuration if(bval){ reset(bval); map_num = 2; } } /** Postcondition: Set the new rotations for all the joints */ private void Interpolate(){ if(inbetweening){ if(getNextKeyframe){ //If need the next keyframe shoulder_NextRotationKeyframe = getShoulderRotation(); //Assign a new shoulder rotation keyframe bicep_NextRotationKeyframe = getBicepRotation(); //Assign a new bicep rotation keyframe forearm_NextRotationKeyframe = getForearmRotation(); //Assign a new forearm rotation keyframe getNextKeyframe = false; } //Shoulder interpolation shoulder_NewRotation = shoulder_CurrentRotationKeyframe + (frame_num/FRAME_MAX) * (shoulder_NextRotationKeyframe - shoulder_CurrentRotationKeyframe); //Forearm interpolation bicep_NewRotation = bicep_CurrentRotationKeyframe + (frame_num/FRAME_MAX) * (bicep_NextRotationKeyframe - bicep_CurrentRotationKeyframe); //Bicep interpolation forearm_NewRotation = forearm_CurrentRotationKeyframe + (frame_num/FRAME_MAX) * (forearm_NextRotationKeyframe - forearm_CurrentRotationKeyframe); if((frame_num > FRAME_MAX && isGrabbed) || (frame_num > FRAME_MAX-1 && !isGrabbed)) { //Robot has reached destination inbetweening = false; //Reached destination, so stop interpolating joint movement until grab/ungrab is complete frame_num = 0.0f; //Reset frame number getNextKeyframe = true; //Next time, compute the next keyframe isGrabbed = !isGrabbed; inbetweening = false; if(!isGrabbed){ obj_count++; //Proceed to the next object } //Set current rotation keyframe to the next shoulder_CurrentRotationKeyframe = shoulder_NextRotationKeyframe; bicep_CurrentRotationKeyframe = bicep_NextRotationKeyframe; forearm_CurrentRotationKeyframe = forearm_NextRotationKeyframe; } else frame_num++; //Increment frame number } else{ if(frame_num > FRAME_MAX-40.0f){ //If reached destination, hold there for a while and then continue interpolating frame_num = 0.0f; inbetweening = true; }else frame_num++; } } /** Postcondition: Return the rotation float of the next shoulder keyframe **/ public float getShoulderRotation(){ float x,z; if(!isGrabbed){ //If joint has not reached object and in not grab mode yet... x = objects[obj_count].getX(); z = objects[obj_count].getZ(); } else { //Use map.destination_Translation's x value x = map.destination_Translation[map_num][obj_count][0]; z = map.destination_Translation[map_num][obj_count][2]; } return -1.0f * (float)Math.atan2((double)z, (double)x); } /** Postcondition: Return the rotation float of the next bicep keyframe **/ public float getBicepRotation(){ float x,y,z; if(!isGrabbed){ //If joint has not reached destination and in grab mode yet... x = objects[obj_count].getX(); y = objects[obj_count].getY(); z = objects[obj_count].getZ(); } else{ //In grab mode, bring object to a destination] x = map.destination_Translation[map_num][obj_count][0]; y = map.destination_Translation[map_num][obj_count][1]; z = map.destination_Translation[map_num][obj_count][2]; } r = (float)Math.sqrt((double)(x*x + y*y + z*z)); //Compute distance r double phi = Math.atan2((double)y,Math.sqrt((double)(x*x + z*z))); return (float)(phi + Math.acos(( (double)(bicep_length*bicep_length) - (double)(forearm_length*forearm_length) + (double)(r*r))/(2.0*bicep_length*r))); } /** Postcondition: Return the rotation float of the next forearm keyframe **/ public float getForearmRotation(){ float x,y,z; if(!isGrabbed){ //If joint has not reached destination and in grab mode yet... x = objects[obj_count].getX(); y = objects[obj_count].getY(); z = objects[obj_count].getZ(); } else{ //In grab mode, bring object to a destination x = map.destination_Translation[map_num][obj_count][0]; y = map.destination_Translation[map_num][obj_count][1]; z = map.destination_Translation[map_num][obj_count][2]; } r = (float)Math.sqrt((double)(x*x + + y*y + z*z)); //Compute distance r return -(float)(Math.PI - Math.acos(( (double)(bicep_length*bicep_length) + (double)(forearm_length*forearm_length) - (double)(r*r))/(2.0*bicep_length*forearm_length))); } /** Postcondition: Return the end effector translation values as a 1x3 float array */ public float[] getEndEffector(){ float [] temp = new float[3]; r = (bicep_length * (float)Math.cos((double)bicep_NewRotation)) + (forearm_length * (float)Math.cos((double)(bicep_NewRotation + forearm_NewRotation))); //Calculate x, y, and z through forward kinematics temp[0] = r * (float)Math.cos((double)shoulder_NewRotation); temp[1] = (bicep_length * (float)Math.sin((double)bicep_NewRotation)) + (forearm_length * (float)Math.sin((double)(bicep_NewRotation + forearm_NewRotation))); temp[2] = -1.0f * r * (float)Math.sin((double)shoulder_NewRotation); return temp; } public void processEvent(Event e) { String name = e.getName(); if(name.equals("set_fraction")) kinematics(((ConstSFFloat)e.getValue()).getValue()); else if(name.equals("reset")) { reset(((ConstSFBool)e.getValue()).getValue()); } else if(name.equals("map1")) { map1(((ConstSFBool)e.getValue()).getValue()); } else if(name.equals("map2")) { map2(((ConstSFBool)e.getValue()).getValue()); } else if(name.equals("map3")) { map3(((ConstSFBool)e.getValue()).getValue()); } else { System.out.println("error: unknown event"); System.exit(3); } } }