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

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

- Search for |

# sphererobot3masses.cpp

Simple example of a spherical robot with 3 internal masses (lpzrobots/ode_robots/robots/sphererobot3masses.cpp).

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius (at) informatik.uni-leipzig.de                               *
    00004  *    der (at) informatik.uni-leipzig.de                                   *
    00005  *                                                                         *
    00006  *   This program is free software; you can redistribute it and/or modify  *
    00007  *   it under the terms of the GNU General Public License as published by  *
    00008  *   the Free Software Foundation; either version 2 of the License, or     *
    00009  *   (at your option) any later version.                                   *
    00010  *                                                                         *
    00011  *   This program is distributed in the hope that it will be useful,       *
    00012  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00013  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00014  *   GNU General Public License for more details.                          *
    00015  *                                                                         *
    00016  *   You should have received a copy of the GNU General Public License     *
    00017  *   along with this program; if not, write to the                         *
    00018  *   Free Software Foundation, Inc.,                                       *
    00019  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00020  *                                                                         *
    00021  *   $Log: sphererobot3masses.cpp,v $
    00022  *   Revision 1.20  2008/05/07 16:45:52  martius
    00023  *   code cosmetics and documentation
    00024  *
    00025  *   Revision 1.19  2008/05/07 11:03:37  martius
    00026  *   code cosmetics
    00027  *
    00028  *   Revision 1.18  2007/11/07 13:21:16  martius
    00029  *   doInternal stuff changed signature
    00030  *
    00031  *   Revision 1.17  2007/09/06 18:48:00  martius
    00032  *   createNewSimpleSpace used
    00033  *
    00034  *   Revision 1.16  2007/08/24 11:57:30  martius
    00035  *   additional sensors can be before or after motor and ir sensors
    00036  *
    00037  *   Revision 1.15  2007/07/17 07:22:28  martius
    00038  *   removed invisible primitives
    00039  *
    00040  *   Revision 1.14  2007/07/03 13:05:23  martius
    00041  *   new servo constants
    00042  *
    00043  *   Revision 1.13  2007/06/21 16:24:27  martius
    00044  *   joints are deleted before objects
    00045  *
    00046  *   Revision 1.12  2007/04/05 15:11:43  martius
    00047  *   angular speed tracking
    00048  *
    00049  *   Revision 1.11  2007/04/03 16:27:31  der
    00050  *   new IR shape
    00051  *   new servo parameters
    00052  *
    00053  *   Revision 1.10  2007/03/26 13:17:43  martius
    00054  *   changes servo params
    00055  *
    00056  *   Revision 1.9  2007/02/23 15:14:17  martius
    00057  *   *** empty log message ***
    00058  *
    00059  *   Revision 1.8  2007/01/03 15:01:09  fhesse
    00060  *   created=true; added (at end of create())
    00061  *
    00062  *   Revision 1.7  2006/12/21 11:43:05  martius
    00063  *   commenting style for doxygen //< -> ///<
    00064  *   new sensors for spherical robots
    00065  *
    00066  *   Revision 1.6  2006/12/01 16:20:40  martius
    00067  *   *** empty log message ***
    00068  *
    00069  *   Revision 1.5  2006/11/17 13:44:50  martius
    00070  *   corrected z-axes sensor problem
    00071  *   there are two sensors for this situation
    00072  *
    00073  *   Revision 1.4  2006/08/08 17:04:46  martius
    00074  *   added new sensor model
    00075  *
    00076  *   Revision 1.3  2006/07/20 17:19:45  martius
    00077  *   removed using namespace std from matrix.h
    00078  *
    00079  *   Revision 1.2  2006/07/14 12:23:42  martius
    00080  *   selforg becomes HEAD
    00081  *
    00082  *   Revision 1.1.2.9  2006/07/10 12:05:02  martius
    00083  *   Matrixlib now in selforg
    00084  *   no namespace std in header files
    00085  *
    00086  *   Revision 1.1.2.8  2006/06/29 16:39:56  robot3
    00087  *   -you can now see bounding shapes if you type ./start -drawboundings
    00088  *   -includes cleared up
    00089  *   -abstractobstacle and abstractground have now .cpp-files
    00090  *
    00091  *   Revision 1.1.2.7  2006/06/25 17:00:33  martius
    00092  *   Id
    00093  *
    00094  *   Revision 1.1.2.6  2006/06/25 16:57:16  martius
    00095  *   abstractrobot is configureable
    00096  *   name and revision
    00097  *
    00098  *   Revision 1.1.2.5  2006/05/09 04:24:34  robot5
    00099  *   *** empty log message ***
    00100  *
    00101  *   Revision 1.1.2.4  2006/03/29 15:09:27  martius
    00102  *   Z-Sensors have been wrong all the time :-)
    00103  *
    00104  *   Revision 1.1.2.3  2006/02/20 10:50:20  martius
    00105  *   pause, random, windowsize, Ctrl-keys
    00106  *
    00107  *   Revision 1.1.2.2  2006/01/11 19:30:28  martius
    00108  *   transparent hull
    00109  *
    00110  *   Revision 1.1.2.1  2006/01/10 17:15:04  martius
    00111  *   was sphererobotarms
    00112  *   moved to osg
    00113  *
    00114  *   Revision 1.18.4.2  2005/11/15 12:29:27  martius
    00115  *   new selforg structure and OdeAgent, OdeRobot ...
    00116  *
    00117  *   Revision 1.18.4.1  2005/11/14 17:37:18  martius
    00118  *   moved to selforg
    00119  *
    00120  *   Revision 1.18  2005/11/09 13:26:57  martius
    00121  *   irsensorrange
    00122  *
    00123  *   Revision 1.17  2005/11/09 13:24:42  martius
    00124  *   added GPL
    00125  *
    00126  ***************************************************************************/
    00127
    00128 #include <assert.h>
    00129 #include <selforg/matrix.h>
    00130 #include <osg/Matrix>
    00131 #include "sphererobot3masses.h"
    00132
    00133 #include "irsensor.h"
    00134 #include "osgprimitive.h" // get access to graphical (OSG) primitives
    00135 #include "mathutils.h"
    00136
    00137
    00138 using namespace osg;
    00139 using namespace std;
    00140
    00141 namespace lpzrobots {
    00142
    00143
    00144   void Sphererobot3MassesConf::destroy(){
    00145     for(list<Sensor*>::iterator i = sensors.begin(); i != sensors.end(); i++){
    00146       if(*i) delete *i;
    00147     }
    00148     sensors.clear();
    00149   }
    00150
    00151   const int Sphererobot3Masses::servono;
    00152
    00153   /**
    00154    *constructor
    00155    **/
    00156   Sphererobot3Masses::Sphererobot3Masses ( const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00157                                            const Sphererobot3MassesConf& conf,
    00158                                            const std::string& name,
    00159                                            double transparency)
    00160     : OdeRobot ( odeHandle, osgHandle, name,
    00161                  "$Id: sphererobot3masses.cpp,v 1.20 2008/05/07 16:45:52 martius Exp $"),
    00162       conf(conf), transparency(transparency)
    00163   {
    00164     numberaxis=3;
    00165     init();
    00166   }
    00167
    00168   /**
    00169    *constructor
    00170    **/
    00171   Sphererobot3Masses::Sphererobot3Masses ( const OdeHandle& odeHandle,
    00172                                            const OsgHandle& osgHandle,
    00173                                            const Sphererobot3MassesConf& conf,
    00174                                            const std::string& name,
    00175                                            const std::string& revision,
    00176                                            double transparency)
    00177     : OdeRobot ( odeHandle, osgHandle, name, revision),
    00178       conf(conf),transparency(transparency)
    00179   {
    00180     numberaxis=3;
    00181     init();
    00182   }
    00183
    00184
    00185   void Sphererobot3Masses::init(){
    00186     created = false;
    00187     memset(object, 0,sizeof(void*) * Last);
    00188     memset(joint, 0,sizeof(void*) * servono);
    00189     memset(axis, 0,sizeof(void*) * servono);
    00190     memset(servo, 0,sizeof(void*) * servono);
    00191
    00192     this->conf.pendulardiameter = conf.diameter/7;
    00193   }
    00194
    00195   Sphererobot3Masses::~Sphererobot3Masses()
    00196   {
    00197     destroy();
    00198     if(conf.irSensorTempl) delete conf.irSensorTempl;
    00199   }
    00200
    00201   void Sphererobot3Masses::update()
    00202   {
    00203     for (int i=0; i < Last; i++) {
    00204       if(object[i]) object[i]->update();
    00205     }
    00206     Matrix pose(object[Base]->getPose());
    00207     for (int i=0; i < servono; i++) {
    00208       if(axis[i]){
    00209         axis[i]->setMatrix(Matrix::rotate(M_PI/2, (i==1), (i==0), (i==2)) * pose);
    00210       }
    00211     }
    00212     irSensorBank.update();
    00213   }
    00214
    00215   /**
    00216    *Writes the sensor values to an array in the memory.
    00217    *@param sensor* pointer to the array
    00218    *@param sensornumber length of the sensor array
    00219    *@return number of actually written sensors
    00220    **/
    00221   int Sphererobot3Masses::getSensors ( sensor* sensors, int sensornumber )
    00222   {
    00223     int len=0;
    00224     assert(created);
    00225     if(!conf.motor_ir_before_sensors){
    00226       FOREACH(list<Sensor*>, conf.sensors, i){
    00227         len += (*i)->get(sensors+len, sensornumber-len);
    00228       }
    00229     }
    00230
    00231     if(conf.motorsensor){
    00232       for ( unsigned int n = 0; n < numberaxis; n++ ) {
    00233         sensors[len] = servo[n]->get() * 0.5;  // we half them to decrease their influence to the control
    00234         len++;
    00235       }
    00236     }
    00237
    00238     // reading ir sensorvalues
    00239     if (conf.irAxis1 || conf.irAxis2 || conf.irAxis3 || conf.irRing || conf.irSide){
    00240       len += irSensorBank.get(sensors+len, sensornumber-len);
    00241     }
    00242
    00243     if(conf.motor_ir_before_sensors){
    00244       FOREACH(list<Sensor*>, conf.sensors, i){
    00245         len += (*i)->get(sensors+len, sensornumber-len);
    00246       }
    00247     }
    00248
    00249
    00250     return len;
    00251   }
    00252
    00253   void Sphererobot3Masses::setMotors ( const motor* motors, int motornumber ) {
    00254     int len = min((unsigned)motornumber, numberaxis);
    00255     for ( int n = 0; n < len; n++ ) {
    00256       servo[n]->set(motors[n]);
    00257     }
    00258   }
    00259
    00260
    00261   void Sphererobot3Masses::place(const osg::Matrix& pose){
    00262     osg::Matrix p2;
    00263     p2 = pose * osg::Matrix::translate(osg::Vec3(0, 0, conf.diameter/2));
    00264     create(p2);
    00265   };
    00266
    00267
    00268   void Sphererobot3Masses::doInternalStuff(GlobalData& global){
    00269     // slow down rotation around z axis because friction does not catch it.
    00270     dBodyID b = getMainPrimitive()->getBody();
    00271     double friction = odeHandle.substance.roughness;
    00272     const double* vel = dBodyGetAngularVel( b);
    00273     if(fabs(vel[2])>0.2){
    00274       dBodyAddTorque ( b , 0 , 0 , -0.05*friction*vel[2] );
    00275     }
    00276     // deaccelerates the robot
    00277     if(conf.brake){
    00278       dBodyAddTorque ( b , -conf.brake*vel[0] , -conf.brake*vel[1] , -conf.brake*vel[2] );
    00279     }
    00280
    00281     irSensorBank.reset();
    00282   }
    00283
    00284   int Sphererobot3Masses::getMotorNumber(){
    00285     return numberaxis;
    00286   }
    00287
    00288   int Sphererobot3Masses::getSensorNumber() {
    00289     int s=0;
    00290     FOREACHC(list<Sensor*>, conf.sensors, i){
    00291       s += (*i)->getSensorNumber();
    00292     }
    00293     return conf.motorsensor * numberaxis + s + irSensorBank.getSensorNumber();
    00294   }
    00295
    00296
    00297   /** creates vehicle at desired position and orientation
    00298   */
    00299   void Sphererobot3Masses::create(const osg::Matrix& pose){
    00300     if (created) {
    00301       destroy();
    00302     }
    00303
    00304     // create vehicle space and add it to the top level space
    00305     odeHandle.createNewSimpleSpace(parentspace,true);
    00306     Color c(osgHandle.color);
    00307     c.alpha() = transparency;
    00308     OsgHandle osgHandle_Base = osgHandle.changeColor(c);
    00309     OsgHandle osgHandleX[3];
    00310     osgHandleX[0] = osgHandle.changeColor(Color(1.0, 0.0, 0.0));
    00311     osgHandleX[1] = osgHandle.changeColor(Color(0.0, 1.0, 0.0));
    00312     osgHandleX[2] = osgHandle.changeColor(Color(0.0, 0.0, 1.0));
    00313
    00314     //    object[Base] = new InvisibleSphere(conf.diameter/2);
    00315     object[Base] = new Sphere(conf.diameter/2);
    00316         //object[Base] = new Box(conf.diameter, conf.diameter, conf.diameter);
    00317     object[Base]->init(odeHandle, conf.spheremass, osgHandle_Base);
    00318     object[Base]->setPose(pose);
    00319
    00320     Pos p(pose.getTrans());
    00321     Primitive* pendular[servono];
    00322     memset(pendular, 0, sizeof(void*) * servono);
    00323
    00324     //definition of the 3 Slider-Joints, which are the controled by the robot-controler
    00325     for ( unsigned int n = 0; n < numberaxis; n++ ) {
    00326       pendular[n] = new Sphere(conf.pendulardiameter/2);
    00327       pendular[n]->init(odeHandle, conf.pendularmass, osgHandleX[n],
    00328                         Primitive::Body | Primitive::Draw); // without geom
    00329       pendular[n]->setPose(pose);
    00330
    00331       joint[n] = new SliderJoint(object[Base], pendular[n],
    00332                                  p, Axis((n==0), (n==1), (n==2))*pose);
    00333       joint[n]->init(odeHandle, osgHandle, false);
    00334       // the Stop parameters are messured from the initial position!
    00335       joint[n]->setParam ( dParamLoStop, -1.1*conf.diameter*conf.pendularrange );
    00336       joint[n]->setParam ( dParamHiStop, 1.1*conf.diameter*conf.pendularrange );
    00337       joint[n]->setParam ( dParamStopCFM, 0.1);
    00338       joint[n]->setParam ( dParamStopERP, 0.9);
    00339       joint[n]->setParam ( dParamCFM, 0.001);
    00340       servo[n] = new SliderServo(joint[n],
    00341                                  -conf.diameter*conf.pendularrange,
    00342                                  conf.diameter*conf.pendularrange,
    00343       //                         conf.pendularmass*conf.motorpowerfactor,10,1);
    00344                                  conf.pendularmass*conf.motorpowerfactor,0.1,0.5);
    00345
    00346       axis[n] = new OSGCylinder(conf.diameter/100, conf.diameter - conf.diameter/100);
    00347       axis[n]->init(osgHandleX[n], OSGPrimitive::Low);
    00348     }
    00349     object[Pendular1] = pendular[0];
    00350     object[Pendular2] = pendular[1];
    00351     object[Pendular3] = pendular[2];
    00352
    00353     double sensorrange = conf.irsensorscale * conf.diameter;
    00354     RaySensor::rayDrawMode drawMode = conf.drawIRs ? RaySensor::drawAll : RaySensor::drawSensor;
    00355     double sensors_inside=0.02;
    00356     if(conf.irSensorTempl==0){
    00357       conf.irSensorTempl=new IRSensor(conf.irCharacter);
    00358     }
    00359     irSensorBank.init(odeHandle, osgHandle );
    00360     if (conf.irAxis1){
    00361       for(int i=-1; i<2; i+=2){
    00362         RaySensor* sensor = conf.irSensorTempl->clone();
    00363         Matrix R = Matrix::rotate(i*M_PI/2, 1, 0, 0) *
    00364           Matrix::translate(0,-i*(conf.diameter/2-sensors_inside),0 );
    00365         irSensorBank.registerSensor(sensor, object[Base], R, sensorrange, drawMode);
    00366       }
    00367     }
    00368     if (conf.irAxis2){
    00369       for(int i=-1; i<2; i+=2){
    00370         RaySensor* sensor = conf.irSensorTempl->clone();
    00371         Matrix R = Matrix::rotate(i*M_PI/2, 0, 1, 0) *
    00372           Matrix::translate(i*(conf.diameter/2-sensors_inside),0,0 );
    00373         //      dRFromEulerAngles(R,i*M_PI/2,-i*M_PI/2,0);
    00374         irSensorBank.registerSensor(sensor, object[Base], R, sensorrange, drawMode);
    00375       }
    00376     }
    00377     if (conf.irAxis3){
    00378       for(int i=-1; i<2; i+=2){
    00379         RaySensor* sensor = conf.irSensorTempl->clone();
    00380         Matrix R = Matrix::rotate( i==1 ? 0 : M_PI, 1, 0, 0) *
    00381           Matrix::translate(0,0,i*(conf.diameter/2-sensors_inside));
    00382         irSensorBank.registerSensor(sensor, object[Base], R, sensorrange, drawMode);
    00383       }
    00384     }
    00385     if (conf.irRing){
    00386       for(double i=0; i<2*M_PI; i+=M_PI/6){  // 12 sensors
    00387         RaySensor* sensor = conf.irSensorTempl->clone();
    00388         Matrix R = Matrix::translate(0,0,conf.diameter/2-sensors_inside) *
    00389           Matrix::rotate( i, 0, 1, 0);
    00390         irSensorBank.registerSensor(sensor, object[Base], R, sensorrange, drawMode);
    00391       }
    00392     }
    00393     if (conf.irSide){
    00394       for(double i=0; i<2*M_PI; i+=M_PI/2){
    00395         RaySensor* sensor = conf.irSensorTempl->clone();
    00396         Matrix R = Matrix::translate(0,0,conf.diameter/2-sensors_inside) *
    00397           Matrix::rotate( M_PI/2-M_PI/8, 1, 0, 0) *  Matrix::rotate( i, 0, 1, 0);
    00398         irSensorBank.registerSensor(sensor, object[Base], R, sensorrange, drawMode);
    00399         sensor = new IRSensor(conf.irCharacter);// and the other side
    00400         irSensorBank.registerSensor(sensor, object[Base],
    00401                                     R * Matrix::rotate( M_PI, 0, 0, 1),
    00402                                     sensorrange, drawMode);
    00403       }
    00404     }
    00405
    00406     FOREACH(list<Sensor*>, conf.sensors, i){
    00407       (*i)->init(object[Base]);
    00408     }
    00409
    00410   created=true;
    00411   }
    00412
    00413
    00414   /** destroys vehicle and space
    00415    */
    00416   void Sphererobot3Masses::destroy(){
    00417     if (created){
    00418       for (int i=0; i<servono; i++){
    00419         if(joint[i]) delete joint[i];
    00420         if(servo[i]) delete servo[i];
    00421         if(axis[i]) delete axis[i];
    00422       }
    00423       for (int i=0; i<Last; i++){
    00424         if(object[i]) delete object[i];
    00425       }
    00426       irSensorBank.clear();
    00427       odeHandle.deleteSpace();
    00428     }
    00429     created=false;
    00430   }
    00431
    00432 }