Robotsystem from Robot Group Leipzig: sphererobot3masses.cpp Source File

- Main Page
- Modules
- Namespaces
- Classes
- Files
- Directories
- Examples

- File List
- File Members

# sphererobot3masses.cpp

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius@informatik.uni-leipzig.de                                    *
    00004  *    fhesse@informatik.uni-leipzig.de                                     *
    00005  *    der@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: sphererobot3masses.cpp,v $
    00023  *   Revision 1.1.2.4  2006/03/29 15:09:27  martius
    00024  *   Z-Sensors have been wrong all the time :-)
    00025  *
    00026  *   Revision 1.1.2.3  2006/02/20 10:50:20  martius
    00027  *   pause, random, windowsize, Ctrl-keys
    00028  *
    00029  *   Revision 1.1.2.2  2006/01/11 19:30:28  martius
    00030  *   transparent hull
    00031  *
    00032  *   Revision 1.1.2.1  2006/01/10 17:15:04  martius
    00033  *   was sphererobotarms
    00034  *   moved to osg
    00035  *
    00036  *   Revision 1.18.4.2  2005/11/15 12:29:27  martius
    00037  *   new selforg structure and OdeAgent, OdeRobot ...
    00038  *
    00039  *   Revision 1.18.4.1  2005/11/14 17:37:18  martius
    00040  *   moved to selforg
    00041  *
    00042  *   Revision 1.18  2005/11/09 13:26:57  martius
    00043  *   irsensorrange
    00044  *
    00045  *   Revision 1.17  2005/11/09 13:24:42  martius
    00046  *   added GPL
    00047  *
    00048  ***************************************************************************/
    00049
    00050 #include <assert.h>
    00051 #include <matrix.h>
    00052 #include <osg/Matrix>
    00053 #include "sphererobot3masses.h"
    00054
    00055 #include "irsensor.h"
    00056 #include "invisibleprimitive.h"
    00057
    00058 using namespace osg;
    00059
    00060 namespace lpzrobots {
    00061
    00062   const int Sphererobot3Masses::servono;
    00063
    00064   /**
    00065    *constructor
    00066    **/
    00067   Sphererobot3Masses::Sphererobot3Masses ( const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00068                                            const Sphererobot3MassesConf& conf, const char* name,
    00069                                            double transparency)
    00070     : OdeRobot ( odeHandle, osgHandle, name ), conf(conf)
    00071   {
    00072
    00073     created = false;
    00074     memset(object, 0,sizeof(void*) * Last);
    00075     memset(joint, 0,sizeof(void*) * servono);
    00076     memset(axis, 0,sizeof(void*) * servono);
    00077     memset(servo, 0,sizeof(void*) * servono);
    00078
    00079     this->conf.pendulardiameter = conf.diameter/7;
    00080     this->transparency=transparency;
    00081
    00082   }
    00083
    00084   Sphererobot3Masses::~Sphererobot3Masses()
    00085   {
    00086     destroy();
    00087   }
    00088
    00089   void Sphererobot3Masses::update()
    00090   {
    00091     for (int i=0; i < Last; i++) {
    00092       if(object[i]) object[i]->update();
    00093     }
    00094     Matrix pose(object[Base]->getPose());
    00095     for (int i=0; i < servono; i++) {
    00096       if(axis[i]){
    00097         axis[i]->setMatrix(Matrix::rotate(M_PI/2, (i==1), (i==0), (i==2)) * pose);
    00098       }
    00099     }
    00100     irSensorBank.update();
    00101   }
    00102
    00103   /**
    00104    *Writes the sensor values to an array in the memory.
    00105    *@param sensor* pointer to the array
    00106    *@param sensornumber length of the sensor array
    00107    *@return number of actually written sensors
    00108    **/
    00109   int Sphererobot3Masses::getSensors ( sensor* sensors, int sensornumber )
    00110   {
    00111     int len=0;
    00112     matrix::Matrix A = odeRto3x3RotationMatrix ( dBodyGetRotation ( object[Base]->getBody() ) );
    00113
    00114     if(conf.motorsensor){
    00115       for ( int n = 0; n < servono; n++ ) {
    00116         sensors[len] = servo[n]->get() * 0.2;
    00117         len++;
    00118       }
    00119     }
    00120     if(conf.axisZsensor){
    00121       // z-coordinate of axis position in world coordinates
    00122       //      len += A.row(2).convertToBuffer(sensors+len, sensornumber-len);
    00123
    00124       // world coordinates of z-axis
    00125       len += A.column(2).convertToBuffer(sensors+len, sensornumber-len);
    00126     }
    00127     if(conf.axisXYZsensor){
    00128       // rotation matrix - 9 (vectors of all axis in world coordinates
    00129       len += A.convertToBuffer(sensors + len , sensornumber -len);
    00130     }
    00131
    00132     //   // angular velocities (local coord.) - 3
    00133     //   Matrix angVelOut(3, 1, dBodyGetAngularVel( object[ Base ].body ));
    00134     //   Matrix angVelIn = A*angVelOut;
    00135     //   len += angVelIn.convertToBuffer(sensors+len,  sensornumber-len );
    00136
    00137     // reading ir sensorvalues
    00138     if (conf.irAxis1 || conf.irAxis2 || conf.irAxis3){
    00139       len += irSensorBank.get(sensors+len, sensornumber-len);
    00140     }
    00141
    00142     return len;
    00143   }
    00144
    00145   /**
    00146    *Reads the actual motor commands from an array, an sets all motors of the snake to this values.
    00147    *It is an linear allocation.
    00148    *@param motors pointer to the array, motor values are scaled to [-1,1]
    00149    *@param motornumber length of the motor array
    00150    **/
    00151   void Sphererobot3Masses::setMotors ( const motor* motors, int motornumber ) {
    00152     int len = min(motornumber, servono);
    00153     for ( int n = 0; n < len; n++ ) {
    00154       servo[n]->set(motors[n]);
    00155     }
    00156   }
    00157
    00158
    00159   void Sphererobot3Masses::place(const osg::Matrix& pose){
    00160     osg::Matrix p2;
    00161     p2 = pose * osg::Matrix::translate(osg::Vec3(0, 0, conf.diameter/2));
    00162     create(p2);
    00163   };
    00164
    00165
    00166   void Sphererobot3Masses::doInternalStuff(const GlobalData& global){
    00167     irSensorBank.reset();
    00168   }
    00169
    00170   /**
    00171    *This is the collision handling function for sphere robots.
    00172    *This overwrides the function collisionCallback of the class robot.
    00173    *@param data
    00174    *@param o1 first geometrical object, which has taken part in the collision
    00175    *@param o2 second geometrical object, which has taken part in the collision
    00176    *@return true if the collision was threated  by the robot, false if not
    00177    **/
    00178   bool Sphererobot3Masses::collisionCallback(void *data, dGeomID o1, dGeomID o2) {
    00179     //checks if either of both of the collision objects are part of the robot
    00180     if( o1 == (dGeomID)odeHandle.space || o2 == (dGeomID)odeHandle.space) {
    00181       if(o1 == (dGeomID)odeHandle.space) irSensorBank.sense(o2);
    00182       if(o2 == (dGeomID)odeHandle.space) irSensorBank.sense(o1);
    00183
    00184       // inner space collisions are not treated!
    00185       int i,n;
    00186       const int N = 40;
    00187       dContact contact[N];
    00188
    00189       n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
    00190       for (i=0; i<n; i++) {
    00191         if( contact[i].geom.g1 == object[Base]->getGeom() || contact[i].geom.g2 == object[Base]->getGeom() ){
    00192           // only treat collisions with envelop
    00193           contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactApprox1;
    00194           //      dContactSoftERP | dContactSoftCFM |
    00195           contact[i].surface.mu = 2.0;
    00196           contact[i].surface.slip1 = 0.005;
    00197           contact[i].surface.slip2 = 0.005;
    00198           //    contact[i].surface.soft_erp = 1; // 0.95;
    00199           //    contact[i].surface.soft_cfm = 0.00001;
    00200           dJointID c = dJointCreateContact( odeHandle.world, odeHandle.jointGroup, &contact[i]);
    00201           dJointAttach ( c , dGeomGetBody(contact[i].geom.g1) , dGeomGetBody(contact[i].geom.g2));
    00202         }
    00203       }
    00204       return true;
    00205     } else {
    00206       return false;
    00207     }
    00208   }
    00209
    00210
    00211   /**
    00212    *Returns the number of motors used by the snake.
    00213    *@return number of motors
    00214    **/
    00215   int Sphererobot3Masses::getMotorNumber(){
    00216     return servono;
    00217   }
    00218
    00219   /**
    00220    *Returns the number of sensors used by the robot.
    00221    *@return number of sensors
    00222    **/
    00223   int Sphererobot3Masses::getSensorNumber() {
    00224     return conf.motorsensor * servono + conf.axisZsensor * servono + conf.axisXYZsensor * servono * 3
    00225       + (conf.irAxis1 + conf.irAxis2 + conf.irAxis3) * 2;
    00226   }
    00227
    00228
    00229   /** creates vehicle at desired position
    00230       @param pos struct Position with desired position
    00231   */
    00232   void Sphererobot3Masses::create(const osg::Matrix& pose){
    00233     if (created) {
    00234       destroy();
    00235     }
    00236
    00237     // create vehicle space and add it to the top level space
    00238     odeHandle.space = dSimpleSpaceCreate (parentspace);
    00239     Color c(osgHandle.color);
    00240     c.alpha() = transparency;
    00241     OsgHandle osgHandle_Base = osgHandle.changeColor(c);
    00242     OsgHandle osgHandleX[3];
    00243     osgHandleX[0] = osgHandle.changeColor(Color(1.0, 0.0, 0.0));
    00244     osgHandleX[1] = osgHandle.changeColor(Color(0.0, 1.0, 0.0));
    00245     osgHandleX[2] = osgHandle.changeColor(Color(0.0, 0.0, 1.0));
    00246
    00247     //    object[Base] = new InvisibleSphere(conf.diameter/2);
    00248     object[Base] = new Sphere(conf.diameter/2);
    00249     //object[Base] = new InvisibleBox(conf.diameter, conf.diameter, conf.diameter);
    00250     object[Base]->init(odeHandle, conf.spheremass, osgHandle_Base);
    00251     object[Base]->setPose(pose);
    00252
    00253     Pos p(pose.getTrans());
    00254     Primitive* pendular[3];
    00255
    00256     //definition of the 3 Slider-Joints, which are the controled by the robot-controler
    00257     for ( unsigned int n = 0; n < 3; n++ ) {
    00258       pendular[n] = new Sphere(conf.pendulardiameter/2);
    00259       pendular[n]->init(odeHandle, conf.pendularmass, osgHandleX[n],
    00260                         Primitive::Body | Primitive::Draw); // without geom
    00261       pendular[n]->setPose(pose);
    00262
    00263       joint[n] = new SliderJoint(object[Base], pendular[n],
    00264                                  p, osg::Vec3((n==0), (n==1), (n==2)));
    00265       joint[n]->init(odeHandle, osgHandle, false);
    00266       // the Stop parameters are messured from the initial position!
    00267       joint[n]->setParam ( dParamLoStop, -1.1*conf.diameter*conf.pendularrange );
    00268       joint[n]->setParam ( dParamHiStop, 1.1*conf.diameter*conf.pendularrange );
    00269       joint[n]->setParam ( dParamStopCFM, 0.1);
    00270       joint[n]->setParam ( dParamStopERP, 0.9);
    00271       joint[n]->setParam ( dParamCFM, 0.001);
    00272       servo[n] = new SliderServo(joint[n],
    00273                                  -conf.diameter*conf.pendularrange,
    00274                                  conf.diameter*conf.pendularrange,
    00275                                  conf.pendularmass);
    00276
    00277       axis[n] = new OSGCylinder(conf.diameter/100, conf.diameter - conf.diameter/100);
    00278       axis[n]->init(osgHandleX[n], OSGPrimitive::Low);
    00279     }
    00280     object[Pendular1] = pendular[0];
    00281     object[Pendular2] = pendular[1];
    00282     object[Pendular3] = pendular[2];
    00283
    00284     double sensorrange = conf.irsensorscale * conf.diameter;
    00285     RaySensor::rayDrawMode drawMode = conf.drawIRs ? RaySensor::drawAll : RaySensor::drawSensor;
    00286
    00287     irSensorBank.init(odeHandle, osgHandle );
    00288     if (conf.irAxis1){
    00289       for(int i=-1; i<2; i+=2){
    00290         IRSensor* sensor = new IRSensor(1.5);
    00291         Matrix R = Matrix::rotate(i*M_PI/2, 1, 0, 0) * Matrix::translate(0,-i*conf.diameter/2,0 );
    00292         irSensorBank.registerSensor(sensor, object[Base], R, sensorrange, drawMode);
    00293       }
    00294     }
    00295     if (conf.irAxis2){
    00296       for(int i=-1; i<2; i+=2){
    00297         IRSensor* sensor = new IRSensor(1.5);
    00298         Matrix R = Matrix::rotate(i*M_PI/2, 0, 1, 0) * Matrix::translate(i*conf.diameter/2,0,0 );
    00299         //      dRFromEulerAngles(R,i*M_PI/2,-i*M_PI/2,0);
    00300         irSensorBank.registerSensor(sensor, object[Base], R, sensorrange, drawMode);
    00301       }
    00302     }
    00303     if (conf.irAxis3){
    00304       for(int i=-1; i<2; i+=2){
    00305         IRSensor* sensor = new IRSensor(1.5);
    00306         Matrix R = Matrix::rotate( i==1 ? 0 : M_PI, 1, 0, 0) * Matrix::translate(0,0,i*conf.diameter/2);
    00307         irSensorBank.registerSensor(sensor, object[Base], R, sensorrange, drawMode);
    00308       }
    00309     }
    00310   }
    00311
    00312
    00313   /** destroys vehicle and space
    00314    */
    00315   void Sphererobot3Masses::destroy(){
    00316     if (created){
    00317       for (int i=0; i<Last; i++){
    00318         if(object[i]) delete object[i];
    00319       }
    00320       for (int i=0; i<servono; i++){
    00321         if(joint[i]) delete joint[i];
    00322         if(servo[i]) delete servo[i];
    00323         if(axis[i]) delete axis[i];
    00324       }
    00325       irSensorBank.clear();
    00326       dSpaceDestroy(odeHandle.space);
    00327     }
    00328     created=false;
    00329   }
    00330
    00331 }
    00332