Robotsystem of the Robot Group Leipzig: sphererobot3masses.cpp

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