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(); }
