Robot Simulator of the Robotics Group for Self-Organization of Control: template_sphererobot/main.cpp

- Main Page
- Namespaces
- Classes
- Files
- Directories
- Related Pages
- Examples

- Search for |

# template_sphererobot/main.cpp

Simple example for a simulation of one spherical robot (ode_robots/examples/template_sphererobot/main.cpp).

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius (at) informatik.uni-leipzig.de                                    *
    00004  *    fhesse (at) informatik.uni-leipzig.de                                     *
    00005  *    der (at) informatik.uni-leipzig.de                                        *
    00006  *                                                                         *
    00007  *   This program is free software; you can redistribute it and/or modify  *
    00008  *   it under the terms of the GNU General Public License as published by  *
    00009  *   the Free Software Foundation; either version 2 of the License, or     *
    00010  *   (at your option) any later version.                                   *
    00011  *                                                                         *
    00012  *   This program is distributed in the hope that it will be useful,       *
    00013  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00014  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00015  *   GNU General Public License for more details.                          *
    00016  *                                                                         *
    00017  *   You should have received a copy of the GNU General Public License     *
    00018  *   along with this program; if not, write to the                         *
    00019  *   Free Software Foundation, Inc.,                                       *
    00020  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00021  *                                                                         *
    00022  *   $Log: main.cpp,v $
    00023  *   Revision 1.1  2008/05/07 16:45:51  martius
    00024  *   code cosmetics and documentation
    00025  *
    00026  *   Revision 1.19  2008/05/01 22:03:56  martius
    00027  *   build system expanded to allow system wide installation
    00028  *   that implies  <ode_robots/> for headers in simulations
    00029  *
    00030  *   Revision 1.18  2007/01/26 12:07:08  martius
    00031  *   orientationsensor added
    00032  *
    00033  *   Revision 1.17  2006/08/04 15:07:47  martius
    00034  *   documentation
    00035  *
    00036  *   Revision 1.16  2006/07/14 12:23:54  martius
    00037  *   selforg becomes HEAD
    00038  *
    00039  *   Revision 1.15.4.5  2006/05/15 13:11:30  robot3
    00040  *   -handling of starting guilogger moved to simulation.cpp
    00041  *    (is in internal simulation routine now)
    00042  *   -CTRL-F now toggles logging to the file (controller stuff) on/off
    00043  *   -CTRL-G now restarts the GuiLogger
    00044  *
    00045  *   Revision 1.15.4.4  2006/02/20 10:50:20  martius
    00046  *   pause, random, windowsize, Ctrl-keys
    00047  *
    00048  *   Revision 1.15.4.3  2006/01/12 15:17:39  martius
    00049  *   *** empty log message ***
    00050  *
    00051  *   Revision 1.15.4.2  2006/01/10 20:33:50  martius
    00052  *   moved to osg
    00053  *
    00054  *   Revision 1.15.4.1  2005/11/15 12:30:17  martius
    00055  *   new selforg structure and OdeAgent, OdeRobot ...
    00056  *
    00057  *   Revision 1.15  2005/11/09 14:54:46  fhesse
    00058  *   nchannelcontroller used
    00059  *
    00060  *   Revision 1.14  2005/11/09 13:41:25  martius
    00061  *   GPL'ised
    00062  *
    00063  ***************************************************************************/
    00064
    00065 // include simulation environment stuff
    00066 #include <ode_robots/simulation.h>
    00067
    00068 // include agent (class for holding a robot, a controller and a wiring)
    00069 #include <ode_robots/odeagent.h>
    00070 #include <ode_robots/octaplayground.h> // arena
    00071 #include <ode_robots/passivesphere.h>  // passive balls
    00072
    00073 // controller
    00074 #include <selforg/invertmotorspace.h>
    00075 #include <selforg/sinecontroller.h>
    00076
    00077 #include <selforg/noisegenerator.h> // include noisegenerator (used for adding noise to sensorvalues)
    00078 #include <selforg/one2onewiring.h>  // simple wiring
    00079
    00080 // robots
    00081 #include <ode_robots/sphererobot3masses.h>
    00082 #include <ode_robots/axisorientationsensor.h>
    00083
    00084 #include <ode_robots/axisorientationsensor.h>
    00085
    00086 // fetch all the stuff of lpzrobots into scope
    00087 using namespace lpzrobots;
    00088
    00089
    00090 class ThisSim : public Simulation {
    00091 public:
    00092   AbstractController* controller;
    00093   Sphererobot3Masses* sphere1;
    00094
    00095   // starting function (executed once at the beginning of the simulation loop)
    00096   void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
    00097   {
    00098     setCameraHomePos(Pos(5.2728, 7.2112, 3.31768), Pos(140.539, -13.1456, 0));
    00099     // initialization
    00100     // - set global noise to 0.1
    00101     global.odeConfig.setParam("noise",0.1);
    00102     //  global.odeConfig.setParam("gravity", 0); // no gravity
    00103
    00104     // use Playground as boundary:
    00105     // - create pointer to playground (odeHandle contains things like world and space the
    00106     //   playground should be created in; odeHandle is generated in simulation.cpp)
    00107     // - setting initial position of the playground: setPosition(osg::Vec3(double x, double y, double z))
    00108     // - push playground to the global list of obstacles (global list comes from simulation.cpp)
    00109     OctaPlayground* playground = new OctaPlayground(odeHandle, osgHandle, osg::Vec3(10, 0.2, 1), 12);
    00110     playground->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren
    00111     global.obstacles.push_back(playground);
    00112
    00113     // add passive spheres as obstacles
    00114     // - create pointer to sphere (with odehandle, osghandle and
    00115     //   optional parameters radius and mass,where the latter is not used here) )
    00116     // - set Pose(Position) of sphere
    00117     // - add sphere to list of obstacles
    00118     for(int i=0; i<8; i++){
    00119       PassiveSphere* s = new PassiveSphere(odeHandle, osgHandle.changeColor(Color(0.0,1.0,0.0)), 0.5);
    00120       s->setPosition(osg::Vec3(5,0,i*3));
    00121       global.obstacles.push_back(s);
    00122     }
    00123
    00124
    00125     // Spherical Robot with axis (gyro) sensors:
    00126     // - get default configuration for robot
    00127     // - create pointer to spherical robot (with odeHandle, osgHandle and configuration)
    00128     // - place robot (unfortunatelly there is a type cast necessary, which is not quite understandable)
    00129     Sphererobot3MassesConf conf = Sphererobot3Masses::getDefaultConf();
    00130     conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::ZProjection));
    00131     conf.diameter=1.0;
    00132     conf.pendularrange= 0.35;
    00133     sphere1 = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(Color(1.0,0.0,0)),
    00134                                        conf, "Sphere1", 0.2);
    00135     ((OdeRobot*)sphere1)->place ( Pos( 0 , 0 , 0.1 ));
    00136
    00137     // Selforg - Controller
    00138     // create pointer to controller
    00139     // set some parameters
    00140     // push controller in global list of configurables
    00141     controller = new InvertMotorSpace(10);
    00142     controller->setParam("epsA",0.05); // model learning rate
    00143     controller->setParam("epsC",0.2); // controller learning rate
    00144     controller->setParam("rootE",3);    // model and contoller learn with square rooted error
    00145     global.configs.push_back ( controller );
    00146
    00147     //  SineController (produces just sine waves)
    00148     // controller = new SineController();
    00149     // controller->setParam("sinerate", 40);
    00150     // controller->setParam("phaseshift", 0.0);
    00151
    00152     // create pointer to one2onewiring which uses colored-noise
    00153     One2OneWiring* wiring = new One2OneWiring ( new ColorUniformNoise() );
    00154
    00155     // create pointer to agent (plotoptions is provided by Simulation (generated from cmdline options)
    00156     // initialize pointer with controller, robot and wiring
    00157     // push agent in globel list of agents
    00158     OdeAgent* agent = new OdeAgent ( plotoptions );
    00159     agent->init ( controller , sphere1 , wiring );
    00160     // the following line will enable a position tracking of the robot, which is written into a file
    00161     // agent->setTrackOptions(TrackRobot(true, false, false, "Sphere_zaxis", 50));
    00162     global.agents.push_back ( agent );
    00163
    00164     // display all parameters of all configurable objects on the console
    00165     showParams(global.configs);
    00166   }
    00167
    00168   /** is called if a key was pressed.
    00169       For keycodes see: osgGA::GUIEventAdapter
    00170       @return true if the key was handled
    00171   */
    00172   virtual bool command(const OdeHandle&, const OsgHandle&, GlobalData& globalData,
    00173                        int key, bool down) {
    00174     if (down) { // only when key is pressed, not when released
    00175       switch ( (char) key ) {
    00176       case 'X' : dBodyAddForce ( sphere1->getMainPrimitive()->getBody() , 30 ,0 , 0 ); break;
    00177       case 'x' : dBodyAddForce ( sphere1->getMainPrimitive()->getBody() , -30 , 0 , 0 ); break;
    00178       case 'T' : dBodyAddTorque ( sphere1->getMainPrimitive()->getBody() , 0 , 0 , 3 ); break;
    00179       case 't' : dBodyAddTorque ( sphere1->getMainPrimitive()->getBody() , 0 , 0 , -3 ); break;
    00180 //       case 'S' : controller->setParam("sineRate", controller->getParam("sineRate")*1.2);
    00181 //      printf("sineRate : %g\n", controller->getParam("sineRate"));
    00182 //       break;
    00183 //       case 's' : controller->setParam("sineRate", controller->getParam("sineRate")/1.2);
    00184 //      printf("sineRate : %g\n", controller->getParam("sineRate"));
    00185 //      break;
    00186       default:
    00187         return false;
    00188       }
    00189       return true;
    00190     } else return false;
    00191   }
    00192
    00193   virtual void bindingDescription(osg::ApplicationUsage & au) const {
    00194     au.addKeyboardMouseBinding("Simulation: X","Push robot to right (positive x)");
    00195     au.addKeyboardMouseBinding("Simulation: x","Push robot to left (negative x)");
    00196     au.addKeyboardMouseBinding("Simulation: T","Spin robot counter-clockwise");
    00197     au.addKeyboardMouseBinding("Simulation: t","Spin robot clockwise");
    00198     //    au.addKeyboardMouseBinding("Controller: S","Increase sine frequency");
    00199     //    au.addKeyboardMouseBinding("Controller: s","Decrease sine frequency");
    00200   }
    00201
    00202 };
    00203
    00204 int main (int argc, char **argv)
    00205 {
    00206   ThisSim sim;
    00207   return sim.run(argc, argv) ? 0 : 1;
    00208 }
    00209
    00210