vgrab-server.C

Go to the documentation of this file.
00001 /*!@file ArmControl/vgrab-server.C */
00002 
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2000-2005   //
00005 // by the University of Southern California (USC) and the iLab at USC.  //
00006 // See http://iLab.usc.edu for information about this project.          //
00007 // //////////////////////////////////////////////////////////////////// //
00008 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00009 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00010 // in Visual Environments, and Applications'' by Christof Koch and      //
00011 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00012 // pending; application number 09/912,225 filed July 23, 2001; see      //
00013 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00014 // //////////////////////////////////////////////////////////////////// //
00015 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00016 //                                                                      //
00017 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00018 // redistribute it and/or modify it under the terms of the GNU General  //
00019 // Public License as published by the Free Software Foundation; either  //
00020 // version 2 of the License, or (at your option) any later version.     //
00021 //                                                                      //
00022 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00023 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00024 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00025 // PURPOSE.  See the GNU General Public License for more details.       //
00026 //                                                                      //
00027 // You should have received a copy of the GNU General Public License    //
00028 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00029 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00030 // Boston, MA 02111-1307 USA.                                           //
00031 // //////////////////////////////////////////////////////////////////// //
00032 //
00033 // Primary maintainer for this file: Lior Elazary <elazary@usc.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/ArmControl/vgrab-server.C $
00035 // $Id: vgrab-server.C 10794 2009-02-08 06:21:09Z itti $
00036 //
00037 
00038 #include "Component/ModelManager.H"
00039 #include "Image/Image.H"
00040 #include "Image/Pixels.H"
00041 #include "Media/FrameSeries.H"
00042 #include "NeovisionII/nv2_common.h"
00043 #include "NeovisionII/nv2_label_server.h"
00044 #include <iostream> // for std::cin
00045 #include <signal.h>
00046 #include "ArmControl/CtrlPolicy.H"
00047 #include "ArmControl/ArmSim.H"
00048 #include "ArmControl/RobotArm.H"
00049 #include "ArmControl/ArmController.H"
00050 #include "ArmControl/ArmPlanner.H"
00051 
00052 bool terminate = false;
00053 struct nv2_label_server* server;
00054 nub::soft_ref<Scorbot> scorbot;
00055 nub::soft_ref<ArmController> armControllerScorbot;
00056 
00057 void terminateProc(int s)
00058 {
00059   LINFO("Ending application\n");
00060   ArmController::JointPos jointPos;
00061   //set The min-max joint pos
00062   jointPos.base = 0;
00063   jointPos.sholder = 0;
00064   jointPos.elbow = 0;
00065   jointPos.wrist1 = 0;
00066   jointPos.wrist2 = 0;
00067 
00068   armControllerScorbot->setJointPos(jointPos);
00069   armControllerScorbot->setControllerOn(true);
00070   sleep(1);
00071   while(!armControllerScorbot->isFinishMove())
00072   {
00073     usleep(1000);
00074   }
00075   LINFO("Reached home position");
00076   armControllerScorbot->setControllerOn(false);
00077 
00078         scorbot->stopAllMotors();
00079   nv2_label_server_destroy(server);
00080   terminate = true;
00081   exit(0);
00082 }
00083 
00084 PID<float> basePID(-0.75f, 0.55, 0.0,
00085     -20, -20,
00086     10,   //error_threshold
00087     -35,   //no move pos threshold
00088     35   //no move neg threshold
00089     );
00090 
00091 
00092 PID<float> sholderPID(-20.0f, 0.0, 0.0,
00093     -20, -20,
00094     10,   //error_threshold
00095     -20,   //no move pos threshold
00096     20,   //no move neg threshold
00097     100, //max motor
00098     -100, //min motor
00099     150, //no move thresh
00100     true, //run pid
00101     1.0, //speed
00102     0.001, //pos static err thresh
00103     -0.001 //neg startic err thresh
00104     );
00105 
00106 PID<float> elbowPID(-1.0f, 0.25, 0.0,
00107     -20, -20,
00108     10,   //error_threshold
00109     -40,   //no move pos threshold
00110     40   //no move neg threshold
00111     );
00112 
00113 
00114 #define Z_THRESH -0.180
00115 bool moveToObject(const Point2D<int> fix,
00116     const nub::soft_ref<Scorbot>& scorbot)
00117 {
00118 
00119   bool moveDone = false;
00120   LINFO("Moving to %ix%i", fix.i, fix.j);
00121 
00122   int baseVal = (int)basePID.update(320/2, fix.i);
00123   //Get the z value
00124   float ef_x, ef_y, ef_z;
00125 
00126   scorbot->getEF_pos(ef_x, ef_y, ef_z);
00127   LINFO("Z %f X %f", ef_z, ef_x);
00128   sholderPID.update(Z_THRESH, ef_z);
00129 
00130   if (baseVal) //move base first
00131   {
00132     LINFO("Base val %i", baseVal);
00133     scorbot->setMotor(RobotArm::BASE, baseVal);
00134     scorbot->setMotor(RobotArm::SHOLDER, 0);
00135     //scorbot->setMotor(RobotArm::ELBOW, 0);
00136   } else {
00137     scorbot->setMotor(RobotArm::BASE, 0);
00138 
00139 
00140     if (ef_z > Z_THRESH)
00141     {
00142       int sholderVal = 0;
00143       if (ef_z > -0.1)
00144       {
00145         float ang = scorbot->getEncoderAng(RobotArm::SHOLDER);
00146         LINFO("Ang %f", ang);
00147         if (ang > -M_PI/4  && ang < M_PI/4)
00148           sholderVal = 60;
00149         else
00150           sholderVal = 13;
00151       }
00152       else
00153       {
00154         sholderVal = (int)sholderPID.update(Z_THRESH, ef_z);
00155         LINFO("Sholder PID %f", sholderPID.getErr());
00156       }
00157       LINFO("Sholder val %i", sholderVal);
00158       scorbot->setMotor(RobotArm::SHOLDER, sholderVal);
00159 
00160     } else {
00161       int sholderVal = (int)sholderPID.update(Z_THRESH, ef_z);
00162       scorbot->setMotor(RobotArm::SHOLDER, sholderVal);
00163 
00164 
00165       if (fabs(elbowPID.getErr()) < 10 &&
00166           fabs(sholderPID.getErr()) < 0.10 &&
00167           fabs(basePID.getErr()) < 10)
00168         moveDone = true;
00169 
00170     }
00171 
00172 
00173   }
00174   int elbowVal = (int)elbowPID.update(240/2, fix.j);
00175   LINFO("Elbow val %i",elbowVal);
00176   if (ef_x > 0.2)
00177   {
00178         scorbot->setMotor(RobotArm::ELBOW, elbowVal);
00179   } else {
00180         scorbot->setMotor(RobotArm::ELBOW, 0);
00181   }
00182 
00183 
00184   LINFO("ERR: %f %f %f", elbowPID.getErr(),
00185           sholderPID.getErr(),
00186           basePID.getErr());
00187 
00188   return moveDone;
00189 }
00190 
00191 int main(const int argc, const char **argv)
00192 {
00193 
00194   MYLOGVERB = LOG_INFO;
00195   ModelManager *mgr = new ModelManager("Test ObjRec");
00196 
00197   //nub::ref<OutputFrameSeries> ofs(new OutputFrameSeries(*mgr));
00198   //mgr->addSubComponent(ofs);
00199 
00200   scorbot = nub::soft_ref<Scorbot>(new Scorbot(*mgr,"Scorbot", "Scorbot"
00201         , "/dev/ttyUSB0"));
00202 
00203   armControllerScorbot = nub::soft_ref<ArmController>(new ArmController(*mgr,
00204         "ArmControllerScorbot", "ArmControllerScorbot", scorbot));
00205   mgr->addSubComponent(armControllerScorbot);
00206 
00207 
00208   if (mgr->parseCommandLine(
00209         (const int)argc, (const char**)argv, "<server ip>", 1, 1) == false)
00210     return 1;
00211   mgr->start();
00212 
00213 
00214   armControllerScorbot->setMotorsOn(true);
00215   armControllerScorbot->setPidOn(true);
00216   armControllerScorbot->setControllerOn(false);
00217   ArmController::JointPos jointPos;
00218 
00219   //set The min-max joint pos
00220   jointPos.base = 7431;
00221   jointPos.sholder = 3264;
00222   jointPos.elbow = 4630;
00223   jointPos.wrist1 = 9999;
00224   jointPos.wrist2 = 9999;
00225   armControllerScorbot->setMaxJointPos(jointPos);
00226 
00227   jointPos.base = -5647;
00228   jointPos.sholder = -1296;
00229   jointPos.elbow = -2900;
00230   jointPos.wrist1 = -9999;
00231   jointPos.wrist2 = -9999;
00232   armControllerScorbot->setMinJointPos(jointPos);
00233 
00234 
00235   //// catch signals and redirect them to terminate for clean exit:
00236   signal(SIGHUP, terminateProc); signal(SIGINT, terminateProc);
00237   signal(SIGQUIT, terminateProc); signal(SIGTERM, terminateProc);
00238   signal(SIGALRM, terminateProc);
00239 
00240 
00241   //get command line options
00242   const char *server_ip = mgr->getExtraArg(0).c_str();
00243 
00244   server = nv2_label_server_create(9930,
00245         server_ip,
00246         9931);
00247 
00248   nv2_label_server_set_verbosity(server,0); //allow warnings
00249 
00250 
00251   armControllerScorbot->setGripper(1);
00252   LINFO("Starting");
00253   LINFO("Hit return to grab");
00254   getchar();
00255   while(!terminate)
00256   {
00257 
00258     struct nv2_image_patch p;
00259     const enum nv2_image_patch_result res =
00260       nv2_label_server_get_current_patch(server, &p);
00261 
00262     std::string objName = "nomatch";
00263     if (res == NV2_IMAGE_PATCH_END)
00264     {
00265       fprintf(stdout, "ok, quitting\n");
00266       break;
00267     }
00268     else if (res == NV2_IMAGE_PATCH_NONE)
00269     {
00270       usleep(10000);
00271       continue;
00272     }
00273     else if (res == NV2_IMAGE_PATCH_VALID &&
00274        p.type == NV2_PIXEL_TYPE_RGB24)
00275     {
00276 
00277       const Image<PixRGB<byte> > im((const PixRGB<byte>*) p.data,
00278           p.width, p.height);
00279       bool moveDone = moveToObject(Point2D<int>(p.fix_x, p.fix_y), scorbot);
00280       LINFO("MoveDone %i", moveDone);
00281       if (moveDone)
00282       {
00283         sleep(2);
00284         LINFO("Grasping Object");
00285         //TODO
00286 
00287         //Move wrist to object
00288         ArmController::JointPos currentJointPos =
00289           armControllerScorbot->getJointPos();
00290         ArmController::JointPos touchJointPos =
00291           armControllerScorbot->getJointPos();
00292         currentJointPos.wrist1 = -250;
00293         currentJointPos.wrist2 = 250;
00294         currentJointPos.gripper = 0;
00295         currentJointPos.reachable = true;
00296         LINFO("Set Pos");
00297         armControllerScorbot->setJointPos(currentJointPos, false);
00298         armControllerScorbot->setControllerOn(true);
00299         sleep(1);
00300         while(!armControllerScorbot->isFinishMove())
00301         {
00302           usleep(1000);
00303         }
00304 
00305         //Grasp the object
00306         LINFO("Ready to Grasp object");
00307         armControllerScorbot->setGripper(0);
00308         while(!armControllerScorbot->isFinishMove())
00309         {
00310           usleep(1000);
00311         }
00312 
00313                 int gripperVal = scorbot->getEncoder(RobotArm::GRIPPER);
00314                 LINFO("Gripper Value %i", gripperVal);
00315                 if (gripperVal < 700)
00316                 {
00317 
00318                         LINFO("Object grasped");
00319 
00320                         //Do somthing with the block (script commands);
00321                         currentJointPos.sholder = 1000;
00322                         currentJointPos.elbow = -1000;
00323                         currentJointPos.base = -1000;
00324                         currentJointPos.gripper = 0;
00325                         currentJointPos.reachable = true;
00326                         LINFO("Set Pos");
00327                         armControllerScorbot->setJointPos(currentJointPos, false);
00328                         armControllerScorbot->setControllerOn(true);
00329                         sleep(1);
00330                         while(!armControllerScorbot->isFinishMove())
00331                         {
00332                                 usleep(1000);
00333                         }
00334 
00335                         //Tell the brain we grasped the object
00336                         struct nv2_patch_label l;
00337                         l.protocol_version = NV2_LABEL_PROTOCOL_VERSION;
00338                         l.patch_id = p.id;
00339                         snprintf(l.source, sizeof(l.source), "%s", "VGrab");
00340                         snprintf(l.name, sizeof(l.name), "%s", "Object Grasped");
00341                         snprintf(l.extra_info, sizeof(l.extra_info),
00342                                         "auxiliary information");
00343 
00344                         nv2_label_server_send_label(server, &l);
00345                         LINFO("Send Msg to head");
00346 
00347 
00348                         sleep(2);
00349                         //Show the block to the head
00350                         currentJointPos.sholder = 1000;
00351                         currentJointPos.elbow = -1000;
00352                         currentJointPos.base = -2000;
00353                         currentJointPos.gripper = 0;
00354                         currentJointPos.wrist1 = 900;
00355                         currentJointPos.wrist2 = 900;
00356                         currentJointPos.reachable = true;
00357                         LINFO("Set Pos");
00358                         armControllerScorbot->setJointPos(currentJointPos, false);
00359                         armControllerScorbot->setControllerOn(true);
00360                         sleep(1);
00361                         while(!armControllerScorbot->isFinishMove())
00362                         {
00363                                 usleep(1000);
00364                         }
00365 
00366                         sleep(2);
00367 
00368                         //Move to the place where is pick up
00369                         armControllerScorbot->setJointPos(touchJointPos, false);
00370                         armControllerScorbot->setControllerOn(true);
00371                         sleep(1);
00372                         while(!armControllerScorbot->isFinishMove())
00373                         {
00374                                 usleep(1000);
00375                         }
00376                         //Left up a little bit
00377                         currentJointPos =
00378                                 armControllerScorbot->getJointPos();
00379 
00380                         currentJointPos.sholder = 700;
00381                         currentJointPos.elbow = -700;
00382                         armControllerScorbot->setJointPos(currentJointPos, false);
00383                         armControllerScorbot->setControllerOn(true);
00384                         sleep(1);
00385                         //drop the object
00386                         armControllerScorbot->setGripper(1);
00387                         //Move to home position
00388                         currentJointPos.base = 0;
00389                         currentJointPos.sholder = 0;
00390                         currentJointPos.elbow = 0;
00391                         currentJointPos.wrist1 = 0;
00392                         currentJointPos.wrist2 = 0;
00393                         currentJointPos.gripper = 0;
00394                         armControllerScorbot->setJointPos(currentJointPos);
00395                         sleep(1);
00396                         while(!armControllerScorbot->isFinishMove())
00397                         {
00398                                 usleep(1000);
00399                         }
00400 
00401                         LINFO("Hit return to grab");
00402                         getchar();
00403                         armControllerScorbot->setControllerOn(false);
00404                 } else {
00405                         armControllerScorbot->setGripper(1);
00406                         LINFO("Did not grab object. try again");
00407                         ArmController::JointPos currentJointPos =
00408                                 armControllerScorbot->getJointPos();
00409                         currentJointPos.sholder -= 300;
00410                         currentJointPos.elbow -= -300;
00411                         currentJointPos.gripper = 0;
00412                         currentJointPos.wrist1 = 0;
00413                         currentJointPos.wrist2 = 0;
00414                         currentJointPos.reachable = true;
00415                         armControllerScorbot->setJointPos(currentJointPos, false);
00416                         armControllerScorbot->setControllerOn(true);
00417                         sleep(1);
00418 
00419                         armControllerScorbot->setControllerOn(false);
00420                         moveDone = false;
00421                 }
00422 
00423 
00424 
00425       }
00426 
00427 
00428     }
00429 
00430     nv2_image_patch_destroy(&p);
00431 
00432     sleep(1);
00433   }
00434 
00435   nv2_label_server_destroy(server);
00436 
00437 }
Generated on Sun May 8 08:40:11 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3