test-planner.C

Go to the documentation of this file.
00001 /*! @file ArmControl/test-planner.C  grab slient objects */
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: Chin-Kai Chang <chinkaic@usc.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/ArmControl/test-planner.C $
00035 // $Id: test-planner.C 10794 2009-02-08 06:21:09Z itti $
00036 //
00037 
00038 #include "Component/ModelManager.H"
00039 #include "Image/Image.H"
00040 #include "Image/Transforms.H"
00041 #include "Image/DrawOps.H"
00042 #include "Image/Rectangle.H"
00043 #include "Image/MathOps.H"
00044 #include "Image/MatrixOps.H"
00045 #include "Image/Layout.H"
00046 #include "Media/FrameSeries.H"
00047 #include "Transport/FrameInfo.H"
00048 #include "Raster/GenericFrame.H"
00049 #include "Raster/Raster.H"
00050 #include "Neuro/BeoHeadBrain.H"
00051 #include "Util/Timer.H"
00052 #include "GUI/GeneralGUI.H"
00053 #include "GUI/ImageDisplayStream.H"
00054 #include "GUI/DebugWin.H"
00055 #include "GUI/XWinManaged.H"
00056 #include "RCBot/Motion/MotionEnergy.H"
00057 #include "ArmControl/CtrlPolicy.H"
00058 #include "ArmControl/ArmSim.H"
00059 #include "ArmControl/RobotArm.H"
00060 #include "ArmControl/ArmController.H"
00061 #include "ArmControl/ArmPlanner.H"
00062 #include "Devices/DeviceOpts.H"
00063 #include "Util/MathFunctions.H"
00064 #include <unistd.h>
00065 #include <stdio.h>
00066 #include <signal.h>
00067 
00068 
00069 #define BEOHEAD
00070 //#define SCORBOT
00071 #define GUIDISPLAY
00072 //#define CPDISPLAY
00073 //#define OUTPUT
00074 ModelManager *mgr;
00075 nub::soft_ref<Scorbot> scorbot;
00076 nub::soft_ref<ArmSim> armSim;
00077 nub::soft_ref<ArmController> armControllerScorbot;
00078 nub::soft_ref<ArmController> armControllerArmSim;
00079 nub::soft_ref<ArmPlanner> armPlanner;
00080 nub::soft_ref<BeoHeadBrain> beoHeadBrain;
00081 
00082 double desireCup[3] = {-1*1.125/4-0.15,0.15,0.25};//cup
00083 double desireBlock[3] = {-1*1.125/4,0,0.05};//red spot
00084 
00085 //! Signal handler (e.g., for control-C)
00086 void terminate(int s)
00087 {
00088 #ifdef SCORBOT
00089       armControllerScorbot->setBasePos( 0 , false);
00090       armControllerScorbot->setSholderPos( 0 , false);
00091       armControllerScorbot->setElbowPos( 0 , false);
00092       armControllerScorbot->setWrist1Pos(0 , false);
00093       armControllerScorbot->setWrist2Pos(0 , false);
00094       while(!armControllerScorbot->isFinishMove())
00095       {
00096         usleep(1000);
00097       }
00098 #endif
00099 #ifdef BEOHEAD
00100   beoHeadBrain->setRelaxNeck(true);
00101   beoHeadBrain->relaxHead();
00102 #endif
00103         LERROR("*** INTERRUPT ***");
00104         exit(0);
00105 }
00106 Point2D<int> getClick(nub::soft_ref<OutputFrameSeries> &ofs)
00107 {
00108   const nub::soft_ref<ImageDisplayStream> ids =
00109     ofs->findFrameDestType<ImageDisplayStream>();
00110 
00111   const rutz::shared_ptr<XWinManaged> uiwin =
00112     ids.is_valid()
00113     ? ids->getWindow("Output")
00114     : rutz::shared_ptr<XWinManaged>();
00115   return uiwin->getLastMouseClick();
00116 }
00117 int getKey(nub::soft_ref<OutputFrameSeries> &ofs)
00118 {
00119   const nub::soft_ref<ImageDisplayStream> ids =
00120     ofs->findFrameDestType<ImageDisplayStream>();
00121 
00122   const rutz::shared_ptr<XWinManaged> uiwin =
00123     ids.is_valid()
00124     ? ids->getWindow("Output")
00125     : rutz::shared_ptr<XWinManaged>();
00126   return uiwin->getLastKeyPress();
00127 }
00128 void output()
00129 {
00130   double *pos = armSim->getEndPos();
00131   printf("%f %f %f ",pos[0],pos[1],pos[2]);
00132   printf("%d %d %d\n",armControllerArmSim->getBase(),
00133       armControllerArmSim->getSholder(),
00134       armControllerArmSim->getElbow());
00135 }
00136 void sync()
00137 {
00138 #ifdef SCORBOT
00139       int basePos = armControllerArmSim->getBase();
00140       int sholderPos = armControllerArmSim->getSholder();
00141       int elbowPos = armControllerArmSim->getElbow();
00142       int wrist1Pos = armControllerArmSim->getWrist1();
00143       int wrist2Pos = armControllerArmSim->getWrist2();
00144 
00145       elbowPos +=sholderPos;
00146 
00147       armControllerScorbot->setBasePos( basePos , false);
00148       armControllerScorbot->setSholderPos( sholderPos , false);
00149       sleep(2);
00150       armControllerScorbot->setElbowPos( -1*elbowPos , false);
00151       sleep(2);
00152 
00153       wrist1Pos += (int)((0.2496*(float)-1*elbowPos)-39.746) ;//+ pitch + (roll + offset) ;
00154       wrist2Pos -= (int)((0.2496*(float)-1*elbowPos)-39.746) ;//- pitch + (roll + offset);
00155       armControllerScorbot->setWrist1Pos( wrist1Pos , false);
00156       armControllerScorbot->setWrist2Pos( wrist2Pos , false);
00157 #endif
00158       output();
00159 
00160 }
00161 int main(const int argc, const char **argv)
00162 {
00163 
00164   mgr = new ModelManager("Test ObjRec");
00165 
00166 #ifdef SCORBOT
00167   scorbot = nub::soft_ref<Scorbot>(new Scorbot(*mgr,"Scorbot", "Scorbot", "/dev/ttyUSB0"));
00168   armControllerScorbot =nub::soft_ref<ArmController>(new ArmController(*mgr, "ArmControllerScorbot", "ArmControllerScorbot", scorbot));
00169   mgr->addSubComponent(armControllerScorbot);
00170 #endif
00171   armSim = nub::soft_ref<ArmSim>(new ArmSim(*mgr));
00172   armControllerArmSim =nub::soft_ref<ArmController>( new ArmController(*mgr,
00173         "ArmControllerArmSim", "ArmControllerArmSim", armSim));
00174   armPlanner = nub::soft_ref<ArmPlanner>(new ArmPlanner(*mgr,"ArmPlanner","ArmPlanner",
00175         armControllerScorbot ,armControllerArmSim, armSim));
00176 
00177   MYLOGVERB = LOG_INFO;
00178 
00179   nub::soft_ref<OutputFrameSeries> ofs(new OutputFrameSeries(*mgr));
00180   mgr->addSubComponent(ofs);
00181 
00182   mgr->addSubComponent(armControllerArmSim);
00183 
00184 #ifdef BEOHEAD
00185   beoHeadBrain = nub::soft_ref<BeoHeadBrain>(new BeoHeadBrain(*mgr));
00186   mgr->addSubComponent(beoHeadBrain);
00187 #endif
00188   nub::soft_ref<GeneralGUI> armSimGUI(new GeneralGUI(*mgr));
00189   mgr->addSubComponent(armSimGUI);
00190 #ifdef SCORBOT
00191   nub::soft_ref<GeneralGUI> scorbotGUI(new GeneralGUI(*mgr));
00192   mgr->addSubComponent(scorbotGUI);
00193 #endif
00194   mgr->exportOptions(MC_RECURSE);
00195 
00196   if (mgr->parseCommandLine(
00197         (const int)argc, (const char**)argv, "", 0, 0) == false)
00198     return 1;
00199   // catch signals and redirect them to terminate for clean exit:
00200   signal(SIGHUP, terminate); signal(SIGINT, terminate);
00201   signal(SIGQUIT, terminate); signal(SIGTERM, terminate);
00202   signal(SIGALRM, terminate);
00203 
00204   mgr->start();
00205 
00206 #ifdef GUIDISPLAY
00207   //start the gui thread
00208   armSimGUI->startThread(ofs);
00209 #ifdef CPDISPLAY
00210   armSimGUI->addImage(armPlanner->getImagePtr());
00211 #endif
00212   sleep(1);
00213   //setup gui for various objects
00214 
00215   //Main GUI Window
00216 
00217 
00218   armSimGUI->addMeter(armControllerArmSim->getBasePtr(),
00219       "Sim_Base", -3000, PixRGB<byte>(0, 255, 0));
00220   armSimGUI->addMeter(armControllerArmSim->getSholderPtr(),
00221       "Sim_Sholder", -3000, PixRGB<byte>(0, 255, 0));
00222   armSimGUI->addMeter(armControllerArmSim->getElbowPtr(),
00223       "Sim_Elbow", -3000, PixRGB<byte>(0, 255, 0));
00224   armSimGUI->addMeter(armControllerArmSim->getWrist1Ptr(),
00225       "Sim_Wrist1", -3000, PixRGB<byte>(0, 255, 0));
00226   armSimGUI->addMeter(armControllerArmSim->getWrist2Ptr(),
00227       "Sim_Wrist2", -3000, PixRGB<byte>(0, 255, 0));
00228 #ifdef SCORBOT
00229   armSimGUI->addMeter(armControllerScorbot->getBasePtr(),
00230       "Motor_Base", -3000, PixRGB<byte>(0, 255, 0));
00231   armSimGUI->addMeter(armControllerScorbot->getSholderPtr(),
00232       "Motor_Sholder", -3000, PixRGB<byte>(0, 255, 0));
00233   armSimGUI->addMeter(armControllerScorbot->getElbowPtr(),
00234       "Motor_Elbow", -3000, PixRGB<byte>(0, 255, 0));
00235   armSimGUI->addMeter(armControllerScorbot->getWrist1Ptr(),
00236       "Motor_Wrist1", -3000, PixRGB<byte>(0, 255, 0));
00237   armSimGUI->addMeter(armControllerScorbot->getWrist2Ptr(),
00238       "Motor_Wrist2", -3000, PixRGB<byte>(0, 255, 0));
00239 #endif
00240 
00241 //  armSimGUI->addImage(armControllerArmSim->getPIDImagePtr());
00242 #endif
00243 #ifdef BEOHEAD
00244   beoHeadBrain->initHead();
00245   beoHeadBrain->setRelaxNeck(false);
00246 #endif
00247 #ifdef SCORBOT
00248   armControllerScorbot->setMotorsOn(true);
00249   armControllerScorbot->setPidOn(true);
00250 #endif
00251   armControllerArmSim->setPidOn(true);
00252   armControllerArmSim->setMotorsOn(true);
00253 
00254   //set The min-max joint pos
00255   ArmController::JointPos jointPos;
00256   jointPos.base = 3000;
00257   jointPos.sholder = 3000;
00258   jointPos.elbow = 2000;
00259   jointPos.wrist1 = 9999;
00260   jointPos.wrist2 = 9999;
00261   armControllerArmSim->setMaxJointPos(jointPos);
00262 
00263   jointPos.base = -8000;
00264   jointPos.sholder = -1500;
00265   jointPos.elbow = -3000;
00266   jointPos.wrist1 = -9999;
00267   jointPos.wrist2 = -9999;
00268   armControllerArmSim->setMinJointPos(jointPos);
00269 
00270   //Move the arm to 0 pos
00271   jointPos.base = 0;
00272   jointPos.sholder = 0;
00273   jointPos.elbow = 0;
00274   jointPos.wrist1 = 0;
00275   jointPos.wrist2 = 0;
00276   armControllerArmSim->setJointPos(jointPos);
00277 
00278 #ifdef SCORBOT
00279   //set The min-max joint pos
00280   jointPos.base = 7431;
00281   jointPos.sholder = 3264;
00282   jointPos.elbow = 4630;
00283   jointPos.wrist1 = 9999;
00284   jointPos.wrist2 = 9999;
00285   armControllerScorbot->setMaxJointPos(jointPos);
00286 
00287   jointPos.base = -5647;
00288   jointPos.sholder = -1296;
00289   jointPos.elbow = -2900;
00290   jointPos.wrist1 = -9999;
00291   jointPos.wrist2 = -9999;
00292   armControllerScorbot->setMinJointPos(jointPos);
00293 #endif
00294 
00295   double *desireObject = desireBlock;
00296   armSim->addObject(ArmSim::BOX,desireObject);
00297 
00298   while(1)
00299   {
00300     armSim->simLoop();
00301     Image<PixRGB<byte> > armCam = flipVertic(armSim->getFrame(-1));
00302 #ifdef BEOHEAD
00303     Image< PixRGB<byte> > inputImg = beoHeadBrain->getLeftEyeImg();
00304 #endif
00305     Image<PixRGB<byte> > simCam = flipVertic(armSim->getFrame(-1));
00306 
00307 
00308 #ifdef BEOHEAD
00309     if (!inputImg.initialized())
00310       continue;
00311 
00312     Point2D<int> targetLoc = beoHeadBrain->getTargetLoc();
00313     if (targetLoc.isValid())
00314       drawCircle(inputImg, targetLoc, 3, PixRGB<byte>(255,0,0));
00315 
00316     simCam += inputImg/2;
00317 
00318 #endif
00319     Layout<PixRGB<byte> > outDisp;
00320 
00321 
00322 #ifdef BEOHEAD
00323     outDisp = vcat(outDisp, hcat(inputImg, simCam));
00324     outDisp = hcat(outDisp, armCam);
00325 #else
00326     outDisp = vcat(outDisp, hcat(armCam, simCam));
00327 #endif
00328     ofs->writeRgbLayout(outDisp, "Output", FrameInfo("Output", SRC_POS));
00329 
00330     Point2D<int> clickLoc = getClick(ofs);
00331     if (clickLoc.isValid())
00332     {
00333      LINFO("clickPos %ix%i", clickLoc.i, clickLoc.j);
00334      clickLoc.i = clickLoc.i%320;//Let all window can click
00335      double loc[3];
00336      armSim->getObjLoc(clickLoc.i, clickLoc.j, loc);
00337 #ifdef BEOHEAD
00338       beoHeadBrain->setTarget(clickLoc);
00339 #endif
00340      LINFO("Loc %f,%f,%f", loc[0], loc[1], loc[2]);
00341      desireObject[0] = loc[0];
00342      desireObject[1] = loc[1];
00343      desireObject[2] = 0.04+ 0.04;//loc[2];
00344      armSim->moveObject(desireObject);
00345      LINFO("MOve beefore");
00346      //armPlanner->move(desireObject,0.01);
00347      LINFO("MOve AAfter");
00348      //armSim->drawLine(desireObject);
00349 
00350 
00351      //armControllerArmSim->setJointPos(jointPos);
00352 
00353     }
00354 #ifdef BEOHEAD
00355     NeoBrain::Stats stats = beoHeadBrain->getStats();
00356     //LINFO("Eye Pan %f Tilt %f",beoHeadBrain->getRightEyePanPos(),beoHeadBrain->getRightEyeTiltPos());
00357     LINFO("Eye Pan %f Tilt %f",stats.rightEyePanPos,stats.rightEyeTiltPos);
00358     LINFO("Head Pan %f Tilt %f Yaw %f",stats.headPanPos,stats.headTiltPos,stats.headYawPos);
00359     targetLoc = beoHeadBrain->getTargetLoc();
00360     LINFO("TrackPoint %i Tilt %i",targetLoc.i,targetLoc.j);
00361 #endif
00362     int key = getKey(ofs);
00363     if (key != -1)
00364     {
00365       switch(key)
00366       {
00367 
00368         //Controll arm
00369         case 10: //1
00370           armControllerArmSim->setBasePos(10, true);
00371           LINFO("Sim Base: %d",armControllerArmSim->getBase());
00372           break;
00373         case 24: //q
00374           armControllerArmSim->setBasePos(-10, true);
00375           break;
00376         case 11: //2
00377           armControllerArmSim->setSholderPos(-10, true);
00378           break;
00379         case 25: //w
00380           armControllerArmSim->setSholderPos(10, true);
00381           break;
00382         case 12: //3
00383           armControllerArmSim->setElbowPos(10, true);
00384           break;
00385         case 26: //e
00386           armControllerArmSim->setElbowPos(-10, true);
00387           break;
00388         case 13: //4
00389           armControllerArmSim->setWrist1Pos(10, true);
00390           break;
00391         case 27: //r
00392           armControllerArmSim->setWrist1Pos(-10, true);
00393           break;
00394         case 14: //5
00395           armControllerArmSim->setWrist2Pos(10, true);
00396           break;
00397         case 28: //t
00398           armControllerArmSim->setWrist2Pos(-10, true);
00399           break;
00400         case 15: //6
00401           //armControllerArmSim->setGripper(0);
00402 #ifdef SCORBOT
00403           armControllerScorbot->setGripper(0);//close
00404 #endif
00405           break;
00406         case 29: //y
00407           //armControllerArmSim->setGripper(1);
00408 #ifdef SCORBOT
00409           armControllerScorbot->setGripper(1);//open
00410 #endif
00411           break;
00412         case 65: //space
00413           armControllerArmSim->killMotors();
00414 #ifdef SCORBOT
00415           armControllerScorbot->killMotors();
00416 #endif
00417           break;
00418         case 41: //f
00419 #ifdef SCORBOT
00420           armControllerScorbot->setWrist1Pos(10, true);
00421           armControllerScorbot->setWrist2Pos(10, true);
00422 #endif
00423           break;
00424         case 55: //v
00425 #ifdef SCORBOT
00426           armControllerScorbot->setWrist1Pos(-10, true);
00427           armControllerScorbot->setWrist2Pos(-10, true);
00428 #endif
00429           break;
00430         case 42: //g
00431           break;
00432         case 56: //b roll
00433 #ifdef SCORBOT
00434           armControllerScorbot->setWrist1Pos(-10, true);
00435           armControllerScorbot->setWrist2Pos(10, true);
00436 #endif
00437           break;
00438         case 57: //n
00439           LINFO("Set gibbs true");
00440           break;
00441         case 39: //s
00442           LINFO("Sync robot");
00443           sync();
00444           break;
00445         case 43:// h
00446 #ifdef SCORBOT
00447           armControllerScorbot->setElbowPos( 0 , false);
00448           sleep(2);
00449           armControllerScorbot->setWrist1Pos( 0, false);
00450           armControllerScorbot->setWrist2Pos( 0 ,false);
00451           armControllerScorbot->setSholderPos( 0 , false);
00452           armControllerScorbot->setBasePos( 0 , false);
00453           armControllerScorbot->setGripper(1);//open
00454 
00455 #endif
00456           armControllerArmSim->setWrist1Pos( 0, false);
00457           armControllerArmSim->setWrist2Pos( 0 ,false);
00458           armControllerArmSim->setElbowPos( 0 , false);
00459           armControllerArmSim->setSholderPos( 0 , false);
00460           armControllerArmSim->setBasePos( 0 , false);
00461           armControllerArmSim->setGripper(1);//open
00462           break;
00463 
00464       }//End Switch
00465       output();
00466 
00467       LINFO("Key = %i", key);
00468     }
00469 
00470   }//End While(1)
00471   mgr->stop();
00472 
00473   return 0;
00474 
00475 }
00476