gripperrobot.cc

神经网络和遗传算法组合应用

/*
  YAKS, a Khepera simulator including a separate GA and ANN 
  (Genetic Algoritm, Artificial Neural Net).
  Copyright (C) 2000  Johan Carlsson ([e-mail])
  
  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., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

 */

#include "gripperrobot.h"

/**
 * Constructor, creates a new gripper robot with hands open and arm horizentiell.
 *
 */
GripperRobot::GripperRobot(): Robot(){
   setArmPosition(0.8);
   ob_taken = -1;
   thingInHands = NULL;
   ob_sensed = 0.0;
   p_gripper = (200.0 / 255.0); /* Full open (according to manual) */
   crashed = 0;
}
/**
 * Deconstructor, does nothing really...
 */
GripperRobot::~GripperRobot(){
}

/**
 * Clear gripper sensor.
 */
void GripperRobot::clrGripperSensor(){
  ob_sensed = 0;
}

/**
 * Returns 1 if a gripper robot.
 * @return Allways 1 here.
 */
int GripperRobot::isaGripperRobot(){
  return(1);
}

/**
 * Check for possible crash with wall.
 * @param x1 First x coordinate of the wall.
 * @param y1 First y coordinate of the wall.
 * @param x2 Second x coordinate of the wall.
 * @param y2 Second y coordinate of the wall.
 * @return 1 if crash, otherwise 0.
 */
int GripperRobot::checkCrash(float x1, float y1, float  x2, float  y2){
   if(armDown()){
     if(g_linesCrosses(x1,y1,x2,y2,
		      lgripper[0],lgripper[1],lgripper[2],lgripper[3]))
       return(1);
     if(g_linesCrosses(x1,y1,x2,y2,
		      rgripper[0],rgripper[1],rgripper[2],rgripper[3]))
       return(1);
     if(g_linesCrosses(x1,y1,x2,y2,
		      flx,fly,frx,fry))
       return(1);
     return(0);
   }
   return(0);
}
/**
 * Resets arm position and status of gripper.
 */
void GripperRobot::resetRobot(){
  setGripperPosition(1.0);
  setArmPosition(0.5);
  ob_taken = -1;
  ob_sensed = 0.0;
  thingInHands = NULL;
  clearCrashed();
  Robot::resetRobot();
}
/**
 * Check for possible crash with round obstacle.
 * @param obj The round obstacle.
 * @return 1 if crash, otherwise 0. 
 */
int GripperRobot::checkCrash(Obst *obj){
  g_Relation relation;
   if(armDown()){
     relation = g_circleBetweenLineAB(lgripper[0],lgripper[1],lgripper[2],lgripper[3],
				      rgripper[0],rgripper[1],rgripper[2],rgripper[3],
				      obj->x,obj->y,obj->r);
     switch(relation){
     case BETWEEN:
     case INSIDE:
       ob_sensed = 1.0;
       return(0);
     case INT_LA:
     case INT_LB:
     case INT_LAB:
	return(1);
     case OUTSIDE:
	return(0);
     }
     
   }
   return(0);
}
/**
 * Check if a object is in the gripper hands.
 */
GripperEvent GripperRobot::inHands(class Sobst *obj){
  g_Relation relation;
  if(arm_pos > 0.8 || arm_pos < 0.2){
     relation = g_circleBetweenLineAB(lgripper[0],lgripper[1],lgripper[2],lgripper[3],
				      rgripper[0],rgripper[1],rgripper[2],rgripper[3],
				      obj->x,obj->y,obj->r);
     switch(relation){
     case BETWEEN:
     case INSIDE:
	ob_sensed = 1.0;
	return(GE_OBJ_PRESENT);
     case INT_LA:
     case INT_LB:
     case INT_LAB:
	return(GE_OBJ_CRASH);
     case OUTSIDE:
	return(GE_NOP);
     }
  }
  return(GE_NOP);
  
}


/*    Values from real robot
 *
    Value  Status               Angle   Simulator
------------------------------------------------- 
     255 ARM GROUND FRONT       52.8      1.0
     235 ARM HORIZONTAL FRONT   180.0   
     133 ARM VERTICAL            90.0     0.5
      30 ARM HORIZONTAL BACK   -180.0  
       0 ARM GROUND BACK       -232.8     0.0

     a   b
     |   |
     |   |
     |\ /|
 e---c-T-d---f        ac = bd = 42.5 mm
 |   _____   |        fh = eg = 74.0 mm
 |  / ___ \  |        gh = ef = 77.0 mm
 | | /   \ | |         8-  -8 = 56.0 mm
 | ||     || |       l8-  -8l = 58.0 mm
 | ||     || |      c | = d | = 11.0 mm
 | l8-   -8l |
 g-----------h

 khepera-gripper    khepera-radio-gripper
 ___________          I ___________
O______|____|  T      IO______|____|  T
 |     |      40.5 mm L-|     |       |
 *-(o)-*       L        *-----*      55.0 mm 
                        |     |       |
                        *-(o)-*       L

 */

/**
 * Get the gripper to a sane state.
 *
 */

void GripperRobot::getSane(){
   if(ob_taken >= 0){
      ob_sensed = 1.0;
   }else{
      thingInHands = NULL;
   }
}

/**
 * Set arm chrashed status.
 *
 */
void GripperRobot::armCrashed(){
   crashed++;
}

/**
 * Get arm crash status.
 */
int GripperRobot::getCrashed() const{
   return(crashed);
}

/**
 * Clear arm crash status.
 */
void GripperRobot::clearCrashed(){
   crashed=0;
}
/**
 * Drop the object that the robot is holding.
 */
void GripperRobot::releaseThingInHands(){
   if(thingInHands){
      thingInHands->taken=0.0;
      thingInHands = NULL;
      ob_taken = -1;
   }
}

/**
 * Give a thing to the robot so it can hold it.
 * @param thing The small round obstacle to hold.
 * @param index The environment index of the object.
 */
void GripperRobot::setThingInHands(Sobst *thing, int index){
   ob_taken = index;
   thingInHands = thing;
   thing->taken = 1.0;
   ob_sensed = 1.0;
}

/**
 * Check if the robots hands is closed.
 * @return 1 if closed, otherwise 0.
 */

int GripperRobot::handsClosed() const{
   return(p_gripper == 0.0 ? 1 : 0);
}

/**
 * Check if the robot is holding something.
 * @return 1 if so otherwise 0.
 */

int GripperRobot::holdingSomething() const{
   if(ob_taken >= 0)
      return(1);
   else
      return(0);
}
/**
 * Updates the coordinates of the object that the robot is holding.
 */
void GripperRobot::updateThingInHandsCoord(){
   if(ob_taken >= 0){
      thingInHands->x = headx;
      thingInHands->y = heady; 
   }
}
/**
 * Recall last saved robot state.
 */
void GripperRobot::recallState(){
  Robot::recallState();
  updateGripperCords();
}
/**
 * Updates the gripper arm state.
 *
 */
void GripperRobot::updateGripperCords(){
   
   double  grip;
   double  dir;
   
   grip=p_gripper;
  if(arm_pos > 0.5){
     ldirection = (double)g_controlAngle((int)direction + 90);
     rdirection = (double)g_controlAngle((int)direction - 90);
     
     /* T is in front of robot */
     gx = g_displaceX(getX(),getRadius(),g_controlAngle((int)direction-180));
     gy = g_displaceY(getY(),getRadius(),g_controlAngle((int)direction-180));
     
     headx = g_displaceX(gx,(g_cos(p_arm)*74.0),direction);
     heady = g_displaceY(gy,(g_cos(p_arm)*74.0),direction);
     /* Calculate point e and f */
     flx = g_displaceX(headx,38.5,ldirection);
     fly = g_displaceY(heady,38.5,ldirection);
     frx = g_displaceX(headx,38.5,rdirection);
     fry = g_displaceY(heady,38.5,rdirection);
     /* Gripper vertices, vector ac and bd. */
     fdeltax = ((flx - frx) / 2.0) * p_gripper;
     fdeltay = ((fly - fry) / 2.0) * p_gripper;
     /* vector ac */
     lgripper[0]  = flx - fdeltax;
     lgripper[1]  = fly - fdeltay;        
     lgripper[2]  = g_displaceX(lgripper[0],(g_cos(p_arm)*42.5),direction);
     lgripper[3]  = g_displaceY(lgripper[1],(g_cos(p_arm)*42.5),direction);
    /* vector bd */
     rgripper[0]  = frx + fdeltax;
     rgripper[1]  = fry + fdeltay;        
     rgripper[2]  = g_displaceX(rgripper[0],(g_cos(p_arm)*42.5),direction);
     rgripper[3]  = g_displaceY(rgripper[1],(g_cos(p_arm)*42.5),direction);
  }
  else if(arm_pos <= 0.5){
    ldirection = (double)g_controlAngle((int)direction + 90);
    rdirection = (double)g_controlAngle((int)direction - 90);
    dir = g_controlAngle((int)direction - 180);
    /* T is in back of robot */
    gx = g_displaceX(getX(),getRadius(),g_controlAngle((int)direction-180));
    gy = g_displaceY(getY(),getRadius(),g_controlAngle((int)direction-180));
    
    headx = g_displaceX(gx,(g_cos(p_arm)*74.0),direction);
    heady = g_displaceY(gy,(g_cos(p_arm)*74.0),direction);

    /* Calculate point e and f */
    flx = g_displaceX(headx,38.5,ldirection);
    fly = g_displaceY(heady,38.5,ldirection);
    frx = g_displaceX(headx,38.5,rdirection);
    fry = g_displaceY(heady,38.5,rdirection);
    /* Gripper vertices, vector ac and bd. */
    fdeltax = ((flx - frx) / 2.0) * p_gripper;
    fdeltay = ((fly - fry) / 2.0) * p_gripper;
    /* vector ac */
    lgripper[0]  = flx - fdeltax;
    lgripper[1]  = fly - fdeltay;        
    lgripper[2]  = g_displaceX(lgripper[0],(g_cos(p_arm)*42.5),direction);
    lgripper[3]  = g_displaceY(lgripper[1],(g_cos(p_arm)*42.5),direction);
    /* vector bd */
    rgripper[0]  = frx + fdeltax;
    rgripper[1]  = fry + fdeltay;        
    rgripper[2]  = g_displaceX(rgripper[0],(g_cos(p_arm)*42.5),direction);
    rgripper[3]  = g_displaceY(rgripper[1],(g_cos(p_arm)*42.5),direction);
  }
}


/**
 * Set gripper position (hands).
 * @param val if > 0.5 open, else close.
 * @return 1 if status changed, otherwise 0.
 */
int GripperRobot::setGripperPosition(double val){

  if(val > 0.5){
     if(p_gripper == 0.0){
	p_gripper = 200.0 / 255.0;
	return(1);
     }else{
	return(0);
     }
  }
  else{
     if(p_gripper == 0.0){
	return(0);
     }else{
	p_gripper = 0.0;
	return(1);
     }
  }
}

/**
 * Set the position of the arm.
 * @param val Arm position [0.0 - down back .. 1.0 - down front] 
 */
void GripperRobot::setArmPosition(double val){
   if(val > 1.0)
      val = 1.0;
   if(val < 0.0)
      val = 0.0;
   arm_pos = val;
   p_arm = g_DGRtoRAD(((52.8+232.8)*val)-232.8);
}

/**
 * Get object sensed.
 * @return 1.0 if object sensed otherwise 0.0 .
 */
double GripperRobot::getObjectSensed() const{
  return (double)(ob_sensed);
}

/**
 * Check if gripper touches ground.
 * @return 1 if gripper touches ground 0 other wise.
 */
int GripperRobot::armDown() const{
  return(arm_pos < 0.2 ? 1 : (arm_pos > 0.8 ? 1 : 0));
}

/**
 * Reads position of a device, for now gripper and gripper arm.
 * @param device 0 for gripper, 1 for arm
 * @return The wanted value, or 0.0 if device does not exist (produces error on stdout).
 */
double GripperRobot::readPosition(int device) const{
  switch(device){
  case 0:
    return p_gripper;
    break;
  case 1:
    return arm_pos;
    break;
  default:
    printf("Gripper::readPosition device %d doesn't exist\n",device);
    return 0;
  }
  return 0;
}