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

# template_sphererobot/main.cpp

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

    /***************************************************************************
     *   Copyright (C) 2005-2011 LpzRobots development team                    *
     *    Georg Martius  <georg dot martius at web dot de>                     *
     *    Frank Guettler <guettler at informatik dot uni-leipzig dot de        *
     *    Frank Hesse    <frank at nld dot ds dot mpg dot de>                  *
     *    Ralf Der       <ralfder at mis dot mpg dot de>                       *
     *                                                                         *
     *   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     *
     *   (at your option) 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 simulation environment stuff
    #include <ode_robots/simulation.h>

    // include agent (class for holding a robot, a controller and a wiring)
    #include <ode_robots/odeagent.h>
    #include <ode_robots/octaplayground.h> // arena
    #include <ode_robots/passivesphere.h>  // passive balls

    // controller
    #include <selforg/invertmotorspace.h>
    #include <selforg/sinecontroller.h>

    #include <selforg/noisegenerator.h> // include noisegenerator (used for adding noise to sensorvalues)
    #include <selforg/one2onewiring.h>  // simple wiring

    // robots
    #include <ode_robots/sphererobot3masses.h>
    #include <ode_robots/axisorientationsensor.h>

    #include <ode_robots/axisorientationsensor.h>

    // fetch all the stuff of lpzrobots into scope
    using namespace lpzrobots;

    class ThisSim : public Simulation {
    public:
      AbstractController* controller;
      Sphererobot3Masses* sphere1;

      // starting function (executed once at the beginning of the simulation loop)
      void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
      {
        setCameraHomePos(Pos(5.2728, 7.2112, 3.31768), Pos(140.539, -13.1456, 0));
        // initialization
        // - set global noise to 0.1
        global.odeConfig.setParam("noise",0.1);
        //  global.odeConfig.setParam("gravity", 0); // no gravity

        // use Playground as boundary:
        // - create pointer to playground (odeHandle contains things like world and space the
        //   playground should be created in; odeHandle is generated in simulation.cpp)
        // - setting initial position of the playground: setPosition(osg::Vec3(double x, double y, double z))
        // - push playground to the global list of obstacles (global list comes from simulation.cpp)
        OctaPlayground* playground = new OctaPlayground(odeHandle, osgHandle, osg::Vec3(10, 0.2, 1), 12);
        playground->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren
        global.obstacles.push_back(playground);

        // add passive spheres as obstacles
        // - create pointer to sphere (with odehandle, osghandle and
        //   optional parameters radius and mass,where the latter is not used here) )
        // - set Pose(Position) of sphere
        // - add sphere to list of obstacles
        for(int i=0; i<8; i++){
          PassiveSphere* s = new PassiveSphere(odeHandle, osgHandle.changeColor(Color(0.0,1.0,0.0)), 0.5);
          s->setPosition(osg::Vec3(5,0,i*3));
          global.obstacles.push_back(s);
        }

        // Spherical Robot with axis (gyro) sensors:
        // - get default configuration for robot
        // - create pointer to spherical robot (with odeHandle, osgHandle and configuration)
        // - place robot (unfortunatelly there is a type cast necessary, which is not quite understandable)
        Sphererobot3MassesConf conf = Sphererobot3Masses::getDefaultConf();
        conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::ZProjection));
        conf.diameter=1.0;
        conf.pendularrange= 0.35;
        sphere1 = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(Color(1.0,0.0,0)),
                                           conf, "Sphere1", 0.2);
        ((OdeRobot*)sphere1)->place ( Pos( 0 , 0 , 0.1 ));

        // Selforg - Controller
        // create pointer to controller
        // set some parameters
        // push controller in global list of configurables
        controller = new InvertMotorSpace(10);
        controller->setParam("epsA",0.05); // model learning rate
        controller->setParam("epsC",0.2); // controller learning rate
        controller->setParam("rootE",3);    // model and contoller learn with square rooted error
        global.configs.push_back ( controller );

        //  SineController (produces just sine waves)
        // controller = new SineController();
        // controller->setParam("sinerate", 40);
        // controller->setParam("phaseshift", 0.0);

        // create pointer to one2onewiring which uses colored-noise
        One2OneWiring* wiring = new One2OneWiring ( new ColorUniformNoise() );

        // create pointer to agent
        // initialize pointer with controller, robot and wiring
        // push agent in globel list of agents
        OdeAgent* agent = new OdeAgent ( global );
        agent->init ( controller , sphere1 , wiring );
        // the following line will enable a position tracking of the robot, which is written into a file
        // agent->setTrackOptions(TrackRobot(true, false, false, "Sphere_zaxis", 50));
        global.agents.push_back ( agent );
      }

      /** is called if a key was pressed.
          For keycodes see: osgGA::GUIEventAdapter
          @return true if the key was handled
      */
      virtual bool command(const OdeHandle&, const OsgHandle&, GlobalData& globalData,
                           int key, bool down) {
        if (down) { // only when key is pressed, not when released
          switch ( (char) key ) {
          case 'X' : dBodyAddForce ( sphere1->getMainPrimitive()->getBody() , 30 ,0 , 0 ); break;
          case 'x' : dBodyAddForce ( sphere1->getMainPrimitive()->getBody() , -30 , 0 , 0 ); break;
          case 'T' : dBodyAddTorque ( sphere1->getMainPrimitive()->getBody() , 0 , 0 , 3 ); break;
          case 't' : dBodyAddTorque ( sphere1->getMainPrimitive()->getBody() , 0 , 0 , -3 ); break;
    //       case 'S' : controller->setParam("sineRate", controller->getParam("sineRate")*1.2);
    //      printf("sineRate : %g\n", controller->getParam("sineRate"));
    //       break;
    //       case 's' : controller->setParam("sineRate", controller->getParam("sineRate")/1.2);
    //      printf("sineRate : %g\n", controller->getParam("sineRate"));
    //      break;
          default:
            return false;
          }
          return true;
        } else return false;
      }

      virtual void bindingDescription(osg::ApplicationUsage & au) const {
        au.addKeyboardMouseBinding("Simulation: X","Push robot to right (positive x)");
        au.addKeyboardMouseBinding("Simulation: x","Push robot to left (negative x)");
        au.addKeyboardMouseBinding("Simulation: T","Spin robot counter-clockwise");
        au.addKeyboardMouseBinding("Simulation: t","Spin robot clockwise");
        //    au.addKeyboardMouseBinding("Controller: S","Increase sine frequency");
        //    au.addKeyboardMouseBinding("Controller: s","Decrease sine frequency");
      }

    };

    int main (int argc, char **argv)
    {
      ThisSim sim;
      return sim.run(argc, argv) ? 0 : 1;
    }