bool Actuator::pushGrip(char armName, char shelfName, geometry_msgs::Point surfacePt){ geometry_msgs::Pose gripPose; geometry_msgs::Point shelfPoint; gripPose.position = surfacePt; gripPose.orientation.w = 0.7071; gripPose.orientation.x = 0; gripPose.orientation.y = 0.7071; gripPose.orientation.z = 0; if(moveToPose(gripPose, armName)){ //shelfPoint = shelf_positions.at(shelfName); //gripPose.position.z = shelfPoint.z + 0.1; // needs to change based on shelf location gripPose.orientation.w = 0.64278760968; gripPose.orientation.x = 0; gripPose.orientation.y = 0.76604444311; gripPose.orientation.z = 0; if(moveToPose(gripPose, armName)){ closeGripper(armName); return true; } else return false; } else return false; }
void TutorialUnit::alternateGripper(bool open) { if(!open){ openGripper(); } else { closeGripper(); } }
void removeFromPin(Pin &pin) { // get disc on the middle point goTo(pin.x, pin.y, (pin.num_discs - 0.5) * DISC_HEIGHT + BASE_HEIGHT); // close Gripper! closeGripper(); // go to the top of the pin goTo(pin.x, pin.y, (3 + ALPHA) * DISC_HEIGHT + BASE_HEIGHT); // update pin state pin.remove(); }
bool Actuator::regularGrip(char armName, geometry_msgs::Point surfacePt){ geometry_msgs::Pose gripPose; gripPose.position = surfacePt; gripPose.orientation.w = 0.7071; gripPose.orientation.x = 0; gripPose.orientation.y = 0.7071; gripPose.orientation.z = 0; if(moveToPose(gripPose, armName)){ closeGripper(armName); return true; } else return false; }
bool doTask(int task) { switch (task) { case GO_FORWARD: return goForward(false); case GO_BACKWARD: return goBackward(); case CALC_NEXT_LINE: return calcNextLine(); case GO_FORWARD_AND_COUNT: return goForwardAndCount(); case TURN_LEFT: return turn90(LEFT); case TURN_RIGHT: return turn90(RIGHT); case TURN_90: // turn this based on the side return true; case TURN_180: return turn180(); case TO_VERTICAL: return craneToVer(); case TO_HORIZONTAL: return craneToHor(); case SLIDE_OUT: return slideOut(); case SLIDE_IN: return slideIn(); case OPEN_GRIPPER: return openGripper(); case CLOSE_GRIPPER: return closeGripper(); case SWITCH_SIDE: return switchSide(); default: return true; } }
~Robot() { closeGripper(); }