BeoBotSim.C

00001 /*!@file BeoSub/BeoBotSim.C Sub Simulator */
00002 
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00005 // 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: Farhan Baluch <fbaluch@usc.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Beobot/BeoBotSim.C $
00035 // $Id: BeoBotSim.C 10794 2009-02-08 06:21:09Z itti $
00036 //
00037 
00038 #include "Beobot/BeoBotSim.H"
00039 #include "Component/OptionManager.H"
00040 #include "Util/MathFunctions.H"
00041 #include "Util/Assert.H"
00042 #include "rutz/compat_snprintf.h"
00043 #include "Image/MatrixOps.H"
00044 #include <stdio.h>
00045 #include <stdlib.h>
00046 #include <signal.h>
00047 
00048 
00049 namespace {
00050 
00051   void nearCallback (void *data, dGeomID o1, dGeomID o2){
00052     BeoBotSim *beoBotSim = (BeoBotSim *)data;
00053     const int MaxContacts = 10;
00054 
00055     //create a contact joint to simulate collisions
00056     dContact contact[MaxContacts];
00057     int nContacts = dCollide (o1,o2,MaxContacts,
00058         &contact[0].geom,sizeof(dContact));
00059     for (int i=0; i<nContacts; i++) {
00060       contact[i].surface.mode = dContactSoftCFM ; //| dContactBounce; // | dContactSoftCFM;
00061       contact[i].surface.mu = 0.5;
00062       contact[i].surface.mu2 = 0.5;
00063       contact[i].surface.bounce = 0.01;
00064       contact[i].surface.bounce_vel = 0.01;
00065       contact[i].surface.soft_cfm = 0.001;
00066 
00067       dJointID c = dJointCreateContact (beoBotSim->getWorld(),
00068           beoBotSim->getContactgroup(),&contact[i]);
00069       dJointAttach (c,
00070           dGeomGetBody(contact[i].geom.g1),
00071           dGeomGetBody(contact[i].geom.g2));
00072     }
00073   }
00074 }
00075 
00076 // ######################################################################
00077 BeoBotSim::BeoBotSim(OptionManager& mgr, const std::string& descrName,
00078     const std::string& tagName, bool showWorld) :
00079   ModelComponent(mgr, descrName, tagName),
00080   itsCarLength(1.5), //the length of the car
00081   itsCarWidth(2.0),
00082   itsCarWeight(30), // the weight of the sub in kg
00083 
00084 
00085   vp(new ViewPort("BeoBotSim")),
00086   itsSpeed(0),
00087   itsSteeringAng(0),
00088   itsWorldView(true),
00089   itsShowWorld(showWorld),
00090   itsWorldDisp(NULL)
00091 {
00092 
00093   if (itsShowWorld)
00094   {
00095     itsWorldDisp = new XWinManaged(vp->getDims(), -1, -1, "World View");
00096   }
00097 
00098   pthread_mutex_init(&itsDispLock, NULL);
00099 
00100 }
00101 
00102 BeoBotSim::~BeoBotSim()
00103 {
00104   dSpaceDestroy(space);
00105   dWorldDestroy(world);
00106 
00107   delete vp;
00108   delete itsWorldDisp;
00109   pthread_mutex_destroy(&itsDispLock);
00110 }
00111 
00112 void BeoBotSim::start2()
00113 {
00114   //        setDrawStuff();
00115   world = dWorldCreate();
00116   space = dHashSpaceCreate(0);
00117   contactgroup = dJointGroupCreate(0);
00118   ground = dCreatePlane(space, 0, 0, 1, 0);
00119   dWorldSetGravity(world,0,0,-9.8);
00120 
00121   dWorldSetCFM (world,1e-6);
00122   dWorldSetERP (world,1);
00123   //dWorldSetAutoDisableFlag (world,1);
00124   dWorldSetContactMaxCorrectingVel (world,0.1);
00125   //set the contact penetration
00126   dWorldSetContactSurfaceLayer(world, 0.001);
00127 
00128 
00129   makeCar();
00130 
00131   //set the viewpoint
00132   double xyz[3] = {0 , -3.0, 15};
00133   double hpr[3] = {90.0,-45,0.0};
00134   vp->dsSetViewpoint (xyz,hpr);
00135 
00136 }
00137 
00138 void BeoBotSim::makeCar()
00139 {
00140   dMass mass;
00141   itsCarBody = dBodyCreate(world);
00142   dBodySetPosition(itsCarBody,0.66*5,4.33*5,1); //car is on the ground
00143 
00144   dMatrix3 R;
00145   dRFromAxisAndAngle (R,1,0,0,-M_PI/2);
00146   dBodySetRotation(itsCarBody, R);
00147   dMassSetZero(&mass);
00148   dMassSetCappedCylinderTotal(&mass,itsCarWeight,3,itsCarWidth,itsCarLength);
00149   dMassRotate(&mass, R);
00150   dBodySetMass(itsCarBody,&mass);
00151   itsCarGeom = dCreateCCylinder(space, itsCarWidth, itsCarLength);
00152   dGeomSetRotation(itsCarGeom, R);
00153   dGeomSetBody(itsCarGeom, itsCarBody);
00154 
00155 }
00156 
00157 void BeoBotSim::drawCar()
00158 {
00159   //double r, length;
00160   dReal r, length;
00161 
00162   dGeomCCylinderGetParams(itsCarGeom,&r,&length);
00163 
00164   vp->dsSetColor(1,1,0);
00165   vp->dsDrawCappedCylinder(
00166       dBodyGetPosition(itsCarBody),
00167       dBodyGetRotation(itsCarBody),
00168       length,
00169       r);
00170 
00171 }
00172 
00173 void BeoBotSim::drawArena()
00174 {
00175 
00176   double pos[3];
00177 
00178   pos[0] = 0; pos[1] = 1.0*5; pos[2] = 1.80;
00179   drawGate(pos);
00180 
00181   pos[0] = 0.16*5; pos[1] = 3.8*5; pos[2] = 1.83;
00182   drawBuoy(pos);
00183 
00184   pos[0] = 0.66*5; pos[1] = 4.33*5; pos[2] = 0.6;
00185   drawPipeline(0, pos);
00186 
00187   pos[0] = 1.16*5; pos[1] = 4.66*5; pos[2] = 0.6;
00188   drawBin(0, pos);
00189 
00190   pos[0] = 1.75*5; pos[1] = 4.91*5; pos[2] = 0.6;
00191   drawPipeline(M_PI/4, pos);
00192 
00193   pos[0] = 2.33*5; pos[1] = 5.41*5; pos[2] = 0.6;
00194   drawPipeline(0, pos);
00195 
00196   pos[0] = 3.0*5; pos[1] = 5.25*5; pos[2] = 0.6;
00197   drawPipeline(-M_PI/4, pos);
00198 
00199   pos[0] = 3.5*5; pos[1] = 4.083*5; pos[2] = 0.6;
00200   drawBin(M_PI/2, pos);
00201 
00202   pos[0] = 3.5*5; pos[1] = 4.083*5; pos[2] = 1.83;
00203   drawBuoy(pos);
00204 
00205   pos[0] = 3.83*5; pos[1] = 4.33*5; pos[2] = 0.6;
00206   drawPipeline(-M_PI/4, pos);
00207 
00208 
00209   pos[0] = 4.75*5; pos[1] = 3.5*5; pos[2] = 0.6;
00210   drawPinger(pos);
00211 }
00212 
00213 
00214 void BeoBotSim::drawGate(const double *gatePos)
00215 {
00216   double pos[3];
00217   double R[12];
00218   //Gate
00219   vp->dsSetColor(0,0,0);
00220   //Top
00221   pos[0] = gatePos[0]; pos[1] = gatePos[1]; pos[2] = gatePos[2];
00222 
00223   dRFromAxisAndAngle (R,0,1,0,-M_PI/2);
00224   vp->dsDrawCappedCylinder(pos, R, 3.05f, 0.1f);
00225 
00226   //side
00227   dRSetIdentity(R);
00228   pos[0] = gatePos[0]-(3.05/2); pos[1] = gatePos[1]; pos[2] = gatePos[2]/2;
00229   vp->dsDrawCappedCylinder(pos, R, 1.83, 0.1);
00230 
00231   pos[0] = gatePos[0]+(3.05/2); pos[1] = gatePos[1]; pos[2] = gatePos[2]/2;
00232   vp->dsDrawCappedCylinder(pos, R, 1.83, 0.1);
00233 }
00234 
00235 
00236 void BeoBotSim::drawBuoy(const double *bouyPos)
00237 {
00238   static int frame = 0;
00239   static bool bouyOn = false;
00240   double pos[3];
00241   double R[12];
00242 
00243   //start Buoy
00244   //flash buoy
00245   if (frame++ > 5) //this sets the frame rate
00246   {
00247     bouyOn = !bouyOn;
00248     frame = 0;
00249   }
00250 
00251   if (bouyOn)
00252     vp->dsSetColor(1,0,0);
00253   else
00254     vp->dsSetColor(0.5,0,0);
00255   pos[0] = bouyPos[0]; pos[1] = bouyPos[1]; pos[2] = bouyPos[2];
00256   dRSetIdentity(R);
00257   vp->dsDrawSphere(pos, R, 0.20);
00258   double pos1[3];
00259   vp->dsSetColor(0,0,0);
00260   pos1[0] = pos[0]; pos1[1] = pos[1]; pos1[2] = 0;
00261   vp->dsDrawLine(pos, pos1);
00262 
00263 }
00264 
00265 void BeoBotSim::drawPipeline(const double ori, const double *pipePos)
00266 {
00267 
00268   double sides[3] = {1.2, 0.15, 0.1};
00269   double R[12];
00270 
00271   dRFromAxisAndAngle (R,0,0,1,ori);
00272 
00273   vp->dsSetColor(1,0.5,0);
00274 
00275   vp->dsDrawBox(pipePos, R, sides);
00276 }
00277 
00278 void BeoBotSim::drawBin(const double ori, const double *binPos)
00279 {
00280 
00281   double sides[3];
00282   double R[12];
00283 
00284   dRFromAxisAndAngle (R,0,0,1,ori);
00285 
00286   vp->dsSetColor(1,1,1);
00287   sides[0] = 0.6; sides[1] = 0.8; sides[2] = 0.1;
00288   vp->dsDrawBox(binPos, R, sides);
00289 
00290   vp->dsSetColor(0,0,0);
00291   sides[0] = 0.3; sides[1] = 0.6; sides[2] = 0.15;
00292   vp->dsDrawBox(binPos, R, sides);
00293 }
00294 
00295 void BeoBotSim::drawPinger(const double *pingerPos)
00296 {
00297   double pos[3];
00298   double R[12];
00299 
00300   vp->dsSetColor(1,1,1);
00301   pos[0] = pingerPos[0]; pos[1] = pingerPos[1]; pos[1] = pingerPos[2] + 1.2/2;
00302   vp->dsDrawCappedCylinder(pos, R, 1.2, 0.1);
00303 
00304 }
00305 
00306 
00307 void BeoBotSim::handleWinEvents(XEvent& event)
00308 {
00309 }
00310 
00311 
00312 void BeoBotSim::simLoop()
00313 {
00314 
00315   //set the trusters
00316   dBodyAddRelTorque(itsCarBody,0, itsSteeringAng, 0);
00317   dBodyAddRelForceAtRelPos(itsCarBody,0,0,itsSpeed, 0,0,0);
00318 
00319   //Folow the sub with the camera
00320   //  const double *bodyPos = dBodyGetPosition(itsSubBody);
00321   //const double *bodyR = dBodyGetRotation(itsSubBody);
00322       //  const dReal *bodyPos = dBodyGetPosition(itsCarBody);
00323       // const dReal *bodyR = dBodyGetRotation(itsCarBody);
00324 
00325 
00326 
00327  dSpaceCollide (space,this,&nearCallback); //check for collisions
00328  dWorldStep(world,0.1);
00329  dJointGroupEmpty (contactgroup); //delete the contact joints
00330 
00331   if (itsShowWorld)
00332   {
00333     itsWorldDisp->drawImage(flipVertic(getFrame(0)));
00334   }
00335 }
00336 
00337 
00338 Image<PixRGB<byte> > BeoBotSim::getFrame(int camera)
00339 {
00340   const dReal *bodyPos = dBodyGetPosition(itsCarBody);
00341   const dReal *bodyR = dBodyGetRotation(itsCarBody);
00342 
00343   double cam_xyz[3], cam_hpr[3] = {0.0,0.0,0.0};
00344 
00345   switch (camera)
00346   {
00347     case 0: //world camera
00348       cam_xyz[0] = bodyPos[0];
00349       cam_xyz[1] =  bodyPos[1]-5;
00350       cam_xyz[2] =  10;
00351 
00352       cam_hpr[0]   = 90.0;
00353       cam_hpr[1]   = -45.0;
00354       cam_hpr[2]   = 0.0;
00355 
00356       break;
00357     case 1:
00358       cam_xyz[0] = bodyPos[0];
00359       cam_xyz[1] = bodyPos[1];
00360       cam_xyz[2] = bodyPos[2];
00361 
00362       cam_hpr[0]   = (atan2(bodyR[4], bodyR[0])*180/M_PI) + 90; //yaw
00363 
00364       break;
00365     case 2:
00366       cam_xyz[0] = bodyPos[0];
00367       cam_xyz[1] = bodyPos[1];
00368       cam_xyz[2] = bodyPos[2];
00369 
00370       cam_hpr[0]   = (atan2(bodyR[4], bodyR[0])*180/M_PI) + 90; //yaw
00371           // cam_hpr[1]   = -90; //yaw
00372 
00373 
00374       break;
00375   }
00376   pthread_mutex_lock(&itsDispLock);
00377   if (camera != -1)
00378     vp->dsSetViewpoint (cam_xyz,cam_hpr);
00379   vp->initFrame();
00380   drawCar();
00381   drawArena();
00382   vp->updateFrame();
00383   pthread_mutex_unlock(&itsDispLock);
00384 
00385 
00386   return vp->getFrame();
00387 
00388 }
00389 
00390 
00391 void BeoBotSim::setSpeed(float spd)
00392 {
00393     itsSpeed = spd;
00394 
00395 }
00396 
00397 void BeoBotSim::setSteering(float angle)
00398 {
00399     itsSteeringAng = angle;
00400 
00401 }
00402 
00403 
00404 
00405 
00406 
00407 
00408 // ######################################################################
00409 /* So things look consistent in everyone's emacs... */
00410 /* Local Variables: */
00411 /* indent-tabs-mode: nil */
00412 /* End: */
Generated on Sun May 8 08:40:12 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3