Robot Simulator of the Robotics Group for Self-Organization of Control: sphererobot3masses.cpp

# sphererobot3masses.cpp

Simple example of a spherical robot with 3 internal masses (lpzrobots/ode_robots/robots/sphererobot3masses.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 <assert.h>
    #include <selforg/matrix.h>
    #include <osg/Matrix>
    #include "sphererobot3masses.h"

    #include "irsensor.h"
    #include "osgprimitive.h" // get access to graphical (OSG) primitives
    #include "mathutils.h"

    using namespace osg;
    using namespace std;

    namespace lpzrobots {

      void Sphererobot3MassesConf::destroy(){
        for(list<Sensor*>::iterator i = sensors.begin(); i != sensors.end(); i++){
          if(*i) delete *i;
        }
        sensors.clear();
      }

      const int Sphererobot3Masses::servono;

      /**
       *constructor
       **/
      Sphererobot3Masses::Sphererobot3Masses ( const OdeHandle& odeHandle, const OsgHandle& osgHandle,
                                               const Sphererobot3MassesConf& conf,
                                               const std::string& name,
                                               double transparency)
        : OdeRobot ( odeHandle, osgHandle, name,
                     "$Id$"),
          conf(conf), transparency(transparency)
      {
        numberaxis=3;
        init();

        addParameter("pendularrange",&this->conf.pendularrange,0,0.4,
                     "range of the masses along the sliders");
      }

      /**
       *constructor
       **/
      Sphererobot3Masses::Sphererobot3Masses ( const OdeHandle& odeHandle,
                                               const OsgHandle& osgHandle,
                                               const Sphererobot3MassesConf& conf,
                                               const std::string& name,
                                               const std::string& revision,
                                               double transparency)
        : OdeRobot ( odeHandle, osgHandle, name, revision),
          conf(conf),transparency(transparency)
      {
        numberaxis=3;
        init();
      }

      void Sphererobot3Masses::init(){
        created = false;
        objects.resize(Last);
        joints.resize(servono);
        memset(axis, 0,sizeof(void*) * servono);
        memset(servo, 0,sizeof(void*) * servono);

        this->conf.pendulardiameter = conf.diameter/7;
      }

      Sphererobot3Masses::~Sphererobot3Masses()
      {
        destroy();
        if(conf.irSensorTempl) delete conf.irSensorTempl;
        FOREACH(std::list<Sensor*>, conf.sensors, s){
          if(*s) delete *s;
        }
      }

      void Sphererobot3Masses::update()
      {
        for (int i=0; i < Last; i++) {
          if(objects[i]) objects[i]->update();
        }
        Matrix pose(objects[Base]->getPose());
        for (int i=0; i < servono; i++) {
          if(axis[i]){
            axis[i]->setMatrix(Matrix::rotate(M_PI/2, (i==1), (i==0), (i==2)) * Matrix::translate(0 ,0, (i==0?-1:1)*conf.axesShift)* pose);
          }
        }
        irSensorBank.update();
        FOREACH(std::list<Sensor*>, conf.sensors, s){
          (*s)->update();
        }
      }

      /**
       *Writes the sensor values to an array in the memory.
       *@param sensor* pointer to the array
       *@param sensornumber length of the sensor array
       *@return number of actually written sensors
       **/
      int Sphererobot3Masses::getSensors ( sensor* sensors, int sensornumber )
      {
        int len=0;
        assert(created);
        if(!conf.motor_ir_before_sensors){
          FOREACH(list<Sensor*>, conf.sensors, i){
            len += (*i)->get(sensors+len, sensornumber-len);
          }
        }

        if(conf.motorsensor){
          for ( unsigned int n = 0; n < numberaxis; n++ ) {
            sensors[len] = servo[n]->get() * 0.5;  // we half them to decrease their influence to the control
            len++;
          }
        }

        // reading ir sensorvalues
        if (conf.irAxis1 || conf.irAxis2 || conf.irAxis3 || conf.irRing || conf.irSide){
          len += irSensorBank.get(sensors+len, sensornumber-len);
        }

        if(conf.motor_ir_before_sensors){
          FOREACH(list<Sensor*>, conf.sensors, i){
            len += (*i)->get(sensors+len, sensornumber-len);
          }
        }

        return len;
      }

      void Sphererobot3Masses::setMotors ( const motor* motors, int motornumber ) {
        int len = min((unsigned)motornumber, numberaxis);
        for ( int n = 0; n < len; n++ ) {
          servo[n]->set(motors[n]);
        }
      }

      void Sphererobot3Masses::place(const osg::Matrix& pose){
        osg::Matrix p2;
        p2 = pose * osg::Matrix::translate(osg::Vec3(0, 0, conf.diameter/2));
        create(p2);
      };

      void Sphererobot3Masses::doInternalStuff(GlobalData& global){
        // slow down rotation around z axis because friction does not catch it.
        dBodyID b = getMainPrimitive()->getBody();
        double friction = odeHandle.substance.roughness;
        const double* vel = dBodyGetAngularVel( b);
        if(fabs(vel[2])>0.2){
          dBodyAddTorque ( b , 0 , 0 , -0.05*friction*vel[2] );
        }
        // deaccelerates the robot
        if(conf.brake){
          dBodyAddTorque ( b , -conf.brake*vel[0] , -conf.brake*vel[1] , -conf.brake*vel[2] );
        }

        irSensorBank.reset();
        FOREACH(list<Sensor*>, conf.sensors, s){
          (*s)->sense(global);
        }

      }

      int Sphererobot3Masses::getMotorNumber(){
        return numberaxis;
      }

      int Sphererobot3Masses::getSensorNumber() {
        int s=0;
        FOREACHC(list<Sensor*>, conf.sensors, i){
          s += (*i)->getSensorNumber();
        }
        return conf.motorsensor * numberaxis + s + irSensorBank.getSensorNumber();
      }

      /** creates vehicle at desired position and orientation
      */
      void Sphererobot3Masses::create(const osg::Matrix& pose){
        if (created) {
          destroy();
        }

        // create vehicle space and add it to the top level space
        odeHandle.createNewSimpleSpace(parentspace,true);
        Color c(osgHandle.color);
        c.alpha() = transparency;
        OsgHandle osgHandle_Base = osgHandle.changeColor(c);
        OsgHandle osgHandleX[3];
        osgHandleX[0] = osgHandle.changeColor(Color(1.0, 0.0, 0.0));
        osgHandleX[1] = osgHandle.changeColor(Color(0.0, 1.0, 0.0));
        osgHandleX[2] = osgHandle.changeColor(Color(0.0, 0.0, 1.0));

        //    objects[Base] = new InvisibleSphere(conf.diameter/2);
        objects[Base] = new Sphere(conf.diameter/2);
            //objects[Base] = new Box(conf.diameter, conf.diameter, conf.diameter);
        objects[Base]->init(odeHandle, conf.spheremass, osgHandle_Base);
        objects[Base]->setPose(pose);

        Pos p(pose.getTrans());
        Primitive* pendular[servono];
        memset(pendular, 0, sizeof(void*) * servono);

        //definition of the 3 Slider-Joints, which are the controled by the robot-controler
        for ( unsigned int n = 0; n < numberaxis; n++ ) {
          pendular[n] = new Sphere(conf.pendulardiameter/2);
          pendular[n]->init(odeHandle, conf.pendularmass, osgHandleX[n],
                            Primitive::Body | Primitive::Draw); // without geom
          pendular[n]->setPose(Matrix::translate(0,0,(n==0?-1:1)*conf.axesShift)*pose);

          joints[n] = new SliderJoint(objects[Base], pendular[n],
                                     p, Axis((n==0), (n==1), (n==2))*pose);
          joints[n]->init(odeHandle, osgHandle, false);
          // the Stop parameters are messured from the initial position!
          // the stops are set by the servo
          //joints[n]->setParam ( dParamLoStop, -1.1*conf.diameter*conf.pendularrange );
          //      joints[n]->setParam ( dParamHiStop, 1.1*conf.diameter*conf.pendularrange );

          joints[n]->setParam ( dParamStopCFM, 0.1);
          joints[n]->setParam ( dParamStopERP, 0.9);
          joints[n]->setParam ( dParamCFM, 0.001);
          // see also setParam() for the stops
          servo[n] = new SliderServo(dynamic_cast<OneAxisJoint*>(joints[n]),
                                     -conf.diameter*conf.pendularrange,
                                     conf.diameter*conf.pendularrange,
          //                         conf.pendularmass*conf.motorpowerfactor,10,1);
                                     conf.pendularmass*conf.motorpowerfactor,0.1,0.5);

          axis[n] = new OSGCylinder(conf.diameter/100, conf.diameter - conf.diameter/100);
          axis[n]->init(osgHandleX[n], OSGPrimitive::Low);
        }
        objects[Pendular1] = pendular[0];
        objects[Pendular2] = pendular[1];
        objects[Pendular3] = pendular[2];

        double sensorrange = conf.irsensorscale * conf.diameter;
        RaySensor::rayDrawMode drawMode = conf.drawIRs;
        double sensors_inside=0.02;
        if(conf.irSensorTempl==0){
          conf.irSensorTempl=new IRSensor(conf.irCharacter);
        }
        irSensorBank.init(odeHandle, osgHandle );
        if (conf.irAxis1){
          for(int i=-1; i<2; i+=2){
            RaySensor* sensor = conf.irSensorTempl->clone();
            Matrix R = Matrix::rotate(i*M_PI/2, 1, 0, 0) *
              Matrix::translate(0,-i*(conf.diameter/2-sensors_inside),0 );
            irSensorBank.registerSensor(sensor, objects[Base], R, sensorrange, drawMode);
          }
        }
        if (conf.irAxis2){
          for(int i=-1; i<2; i+=2){
            RaySensor* sensor = conf.irSensorTempl->clone();
            Matrix R = Matrix::rotate(i*M_PI/2, 0, 1, 0) *
              Matrix::translate(i*(conf.diameter/2-sensors_inside),0,0 );
            //      dRFromEulerAngles(R,i*M_PI/2,-i*M_PI/2,0);
            irSensorBank.registerSensor(sensor, objects[Base], R, sensorrange, drawMode);
          }
        }
        if (conf.irAxis3){
          for(int i=-1; i<2; i+=2){
            RaySensor* sensor = conf.irSensorTempl->clone();
            Matrix R = Matrix::rotate( i==1 ? 0 : M_PI, 1, 0, 0) *
              Matrix::translate(0,0,i*(conf.diameter/2-sensors_inside));
            irSensorBank.registerSensor(sensor, objects[Base], R, sensorrange, drawMode);
          }
        }
        if (conf.irRing){
          for(double i=0; i<2*M_PI; i+=M_PI/6){  // 12 sensors
            RaySensor* sensor = conf.irSensorTempl->clone();
            Matrix R = Matrix::translate(0,0,conf.diameter/2-sensors_inside) *
              Matrix::rotate( i, 0, 1, 0);
            irSensorBank.registerSensor(sensor, objects[Base], R, sensorrange, drawMode);
          }
        }
        if (conf.irSide){
          for(double i=0; i<2*M_PI; i+=M_PI/2){
            RaySensor* sensor = conf.irSensorTempl->clone();
            Matrix R = Matrix::translate(0,0,conf.diameter/2-sensors_inside) *
              Matrix::rotate( M_PI/2-M_PI/8, 1, 0, 0) *  Matrix::rotate( i, 0, 1, 0);
            irSensorBank.registerSensor(sensor, objects[Base], R, sensorrange, drawMode);
            sensor = new IRSensor(conf.irCharacter);// and the other side
            irSensorBank.registerSensor(sensor, objects[Base],
                                        R * Matrix::rotate( M_PI, 0, 0, 1),
                                        sensorrange, drawMode);
          }
        }

        FOREACH(list<Sensor*>, conf.sensors, i){
          (*i)->init(objects[Base]);
        }

      created=true;
      }

      /** destroys vehicle and space
       */
      void Sphererobot3Masses::destroy(){
        if (created){
          for (int i=0; i<servono; i++){
            if(servo[i]) delete servo[i];
            if(axis[i]) delete axis[i];
          }
          irSensorBank.clear();
          odeHandle.deleteSpace();
        }
        created=false;
      }

      void Sphererobot3Masses::notifyOnChange(const paramkey& key){
        for (int i=0; i<servono; i++){
          if(servo[i]) servo[i]->setMinMax(-conf.diameter*conf.pendularrange,
                                           conf.diameter*conf.pendularrange);
        }
      }

    }