Robotsystem from Robot Group Leipzig: nimm2.cpp Source File

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

- File List
- File Members

# nimm2.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: nimm2.cpp,v $
    00023  *   Revision 1.21.4.17  2006/03/31 12:12:41  fhesse
    00024  *   documentation improved
    00025  *
    00026  *   Revision 1.21.4.16  2006/02/01 18:33:39  martius
    00027  *   use Axis type for Joint axis. very important, since otherwise Vec3 * pose is not the right direction vector anymore
    00028  *
    00029  *   Revision 1.21.4.15  2006/01/31 15:40:23  martius
    00030  *   irRange configurable
    00031  *   even higher friction and body is allways on th ground
    00032  *
    00033  *   Revision 1.21.4.14  2006/01/26 18:37:20  martius
    00034  *   *** empty log message ***
    00035  *
    00036  *   Revision 1.21.4.13  2006/01/18 16:46:24  martius
    00037  *   enabled coloring via osgHandle
    00038  *
    00039  *   Revision 1.21.4.12  2006/01/17 17:02:19  martius
    00040  *   faster, stronger, more friction
    00041  *
    00042  *   Revision 1.21.4.11  2006/01/13 12:25:44  martius
    00043  *   typo in setmotors
    00044  *
    00045  *   Revision 1.21.4.10  2006/01/11 18:21:48  martius
    00046  *   bumpers are moving
    00047  *   wheel texture is okay
    00048  *
    00049  *   Revision 1.21.4.9  2006/01/10 17:16:22  martius
    00050  *   sensorbank cleared on destroy
    00051  *
    00052  *   Revision 1.21.4.8  2005/12/29 16:47:40  martius
    00053  *   joint has getPosition
    00054  *
    00055  *   Revision 1.21.4.7  2005/12/15 17:04:08  martius
    00056  *   Primitives are not longer inherited from OSGPrimitive, moreover
    00057  *   they aggregate them.
    00058  *   Joint have better getter and setter
    00059  *
    00060  *   Revision 1.21.4.6  2005/12/14 15:37:09  martius
    00061  *   robots are working with osg
    00062  *
    00063  *   Revision 1.21.4.5  2005/12/13 18:11:39  martius
    00064  *   still trying to port robots
    00065  *
    00066  *   Revision 1.21.4.4  2005/12/06 17:38:17  martius
    00067  *   *** empty log message ***
    00068  *
    00069  *   Revision 1.21.4.3  2005/11/16 11:26:52  martius
    00070  *   moved to selforg
    00071  *
    00072  *   Revision 1.21.4.2  2005/11/15 12:29:26  martius
    00073  *   new selforg structure and OdeAgent, OdeRobot ...
    00074  *
    00075  *   Revision 1.21.4.1  2005/11/14 17:37:17  martius
    00076  *   moved to selforg
    00077  *
    00078  *   Revision 1.21  2005/11/08 11:35:56  martius
    00079  *   removed check for sensorbank because rays are disabled now
    00080  *
    00081  *   Revision 1.20  2005/11/04 14:43:27  martius
    00082  *   added GPL
    00083  *
    00084  *                                                                 *
    00085  ***************************************************************************/
    00086
    00087 #include <ode/ode.h>
    00088 #include <assert.h>
    00089 #include <osg/Matrix>
    00090
    00091 #include "nimm2.h"
    00092 #include "irsensor.h"
    00093
    00094 using namespace osg;
    00095
    00096 namespace lpzrobots {
    00097
    00098   // constructor:
    00099   // - give handle for ODE and OSG stuff, and default configuration
    00100   Nimm2::Nimm2(const OdeHandle& odehandle, const OsgHandle& osgHandle,
    00101                const Nimm2Conf& conf = getDefaultConf()):
    00102     // calling OdeRobots construtor with name of the actual robot
    00103     OdeRobot(odehandle, osgHandle, "Nimm2"), conf(conf) {
    00104
    00105     // robot not created up to now
    00106     created=false;
    00107
    00108     // Nimm2 color ;-)
    00109     // this->osgHandle.color = Color(2, 156/255.0, 0, 1.0f);
    00110     // can be set in main.cpp of simulation
    00111
    00112     // maximal used force is calculated from the force and size given in the configuration
    00113     max_force   = conf.force*conf.size*conf.size;
    00114
    00115     height=conf.size;
    00116
    00117     width=conf.size/2;  // radius of body
    00118     radius=conf.size/4; // +conf.size/600;  //radius of wheels
    00119     wheelthickness=conf.size/20; // thickness of the wheels (if wheels used, no spheres)
    00120     cmass=4*conf.size;    // mass of body
    00121     wmass=conf.size/5.0;  // mass of wheels
    00122     if(conf.singleMotor){ //-> one dimensional robot
    00123       sensorno=1;
    00124       motorno=1;
    00125     } else { // -> both wheels actuated independently
    00126       sensorno=2;
    00127       motorno=2;
    00128     }
    00129
    00130     if (conf.cigarMode){
    00131       length=conf.size*2.0;    // long body
    00132       wheeloffset= -length/4;  // wheels at the end of the cylinder, and the opposite endas the bumper
    00133       number_bumpers=2;        // if wheels not at center only one bumper
    00134       cmass=4*conf.size;
    00135       max_force   = 2*conf.force*conf.size*conf.size;
    00136     }
    00137     else{
    00138       length=conf.size/2;     // short body
    00139       wheeloffset=0.0;        // wheels at center of body
    00140       number_bumpers=2;       // if wheels at center 2 bumpers (one at each end)
    00141     }
    00142
    00143     // increase sensornumber by 2 if front infrared sensors are used
    00144     sensorno+= conf.irFront * 2;
    00145   };
    00146
    00147
    00148   /** sets actual motorcommands
    00149       @param motors motors scaled to [-1,1]
    00150       @param motornumber length of the motor array
    00151   */
    00152   void Nimm2::setMotors(const motor* motors, int motornumber){
    00153     assert(created);
    00154     assert(motornumber == motorno);
    00155     if(conf.singleMotor){ // set the same motorcommand to both wheels
    00156       joint[0]->setParam(dParamVel2, motors[0]*conf.speed); // set velocity
    00157       joint[0]->setParam(dParamFMax2,max_force);            // set maximal force
    00158       joint[1]->setParam(dParamVel2, motors[0]*conf.speed);
    00159       joint[1]->setParam(dParamFMax2,max_force);
    00160     } else {
    00161       for (int i=0; i<2; i++){ // set different motorcommands to the wheels
    00162         joint[i]->setParam(dParamVel2, motors[i]*conf.speed);
    00163         joint[i]->setParam(dParamFMax2,max_force);
    00164       }
    00165     }
    00166   };
    00167
    00168   /** returns actual sensorvalues
    00169       @param sensors sensors scaled to [-1,1] (more or less)
    00170       @param sensornumber length of the sensor array
    00171       @return number of actually written sensors
    00172   */
    00173   int Nimm2::getSensors(sensor* sensors, int sensornumber){
    00174     assert(created);
    00175
    00176     // choose sensornumber according to number of motors
    00177     // - one motorcommand -> one sensorvalue
    00178     // - motors indepently controlled -> two sensorvalues
    00179     int len = conf.singleMotor ? 1 : 2;
    00180     for (int i=0; i<len; i++){
    00181       sensors[i]=joint[i]->getPosition2Rate();  // readout wheel velocity
    00182       sensors[i]/=conf.speed;  //scaling
    00183     }
    00184     // ask sensorbank for sensor values (from infrared sensors)
    00185     //  sensor+len is the starting point in the sensors array
    00186     len += irSensorBank.get(sensors+len, sensornumber-len);
    00187     return len;
    00188   };
    00189
    00190
    00191   void Nimm2::place(const Matrix& pose){
    00192     // the position of the robot is the center of the body (without wheels)
    00193     // to set the vehicle on the ground when the z component of the position is 0
    00194     // width*0.6 is added (without this the wheels and half of the robot will be in the ground)
    00195     Matrix p2;
    00196     p2 = pose * Matrix::translate(Vec3(0, 0, width*0.6));
    00197     create(p2);
    00198
    00199   };
    00200
    00201   /** returns a vector with the positions of all segments of the robot
    00202       @param poslist vector of positions (of all robot segments)
    00203       @return length of the list
    00204   */
    00205   int Nimm2::getSegmentsPosition(vector<Position> &poslist){
    00206     assert(created);
    00207     for (int i=0; i<3; i++){
    00208       poslist.push_back(Position(dBodyGetPosition(object[i]->getBody())));
    00209     }
    00210     return 3;
    00211   };
    00212
    00213   /**
    00214    * updates the osg notes and sensorbank
    00215    */
    00216   void Nimm2::update(){
    00217     assert(created); // robot must exist
    00218
    00219     for (int i=0; i<3; i++) { // update objects
    00220       object[i]->update();
    00221     }
    00222     for (int i=0; i < 2; i++) { // update joints
    00223       joint[i]->update();
    00224     }
    00225     if (conf.bumper){ // if bumper used update transform objects
    00226       for (int i=0; i<number_bumpers; i++){
    00227         bumper[i].trans->update();
    00228       }
    00229     }
    00230
    00231     // update sensorbank with infrared sensors
    00232     irSensorBank.update();
    00233   }
    00234
    00235
    00236   void Nimm2::mycallback(void *data, dGeomID o1, dGeomID o2){
    00237     // Nimm2* me = (Nimm2*)data;
    00238     // o1 and o2 are member of the space
    00239
    00240     // we ignore the collisions
    00241   }
    00242
    00243   bool Nimm2::collisionCallback(void *data, dGeomID o1, dGeomID o2){
    00244     //checks if one of the collision objects is part of the robot
    00245     assert(created);
    00246     bool colwithme = false;
    00247     if( o1 == (dGeomID)odeHandle.space || o2 == (dGeomID)odeHandle.space ){
    00248       if(o1 == (dGeomID)odeHandle.space) irSensorBank.sense(o2);
    00249       if(o2 == (dGeomID)odeHandle.space) irSensorBank.sense(o1);
    00250
    00251       bool colwithbody;
    00252       int i,n;
    00253       const int N = 10;
    00254       dContact contact[N];
    00255       //    n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
    00256       n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
    00257       for (i=0; i<n; i++){
    00258         colwithme = true; // there is at least one collision with some part of the robot (not sensors)
    00259         colwithbody = false;
    00260         if( contact[i].geom.g1 == object[0]->getGeom() || contact[i].geom.g2 == object[0]->getGeom() ||
    00261             ( bumper[0].trans && bumper[1].trans) && (
    00262             contact[i].geom.g1 == bumper[0].trans->getGeom() ||
    00263             contact[i].geom.g2 == bumper[0].trans->getGeom() ||
    00264             contact[i].geom.g1 == bumper[1].trans->getGeom() ||
    00265             contact[i].geom.g2 == bumper[1].trans->getGeom()) ){
    00266
    00267           colwithbody = true;
    00268         }
    00269         contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
    00270           dContactSoftERP | dContactSoftCFM | dContactApprox1;
    00271         // one could try to make the body sliping along its axis by using
    00272         //  sin(alpha), cos(alpha) for sliping params (only for body collisions)
    00273         contact[i].surface.slip1 = 0.005; // sliping in x
    00274         contact[i].surface.slip2 = 0.005; // sliping in y
    00275         if(colwithbody){
    00276           contact[i].surface.mu = 0.1; // small friction of smooth body
    00277           contact[i].surface.soft_erp = 0.5;
    00278           contact[i].surface.soft_cfm = 0.005;
    00279         }else{
    00280           contact[i].surface.mu = 5.0; //large friction
    00281           contact[i].surface.soft_erp = 0.5;
    00282           contact[i].surface.soft_cfm = 0.001;
    00283         }
    00284         dJointID c = dJointCreateContact( odeHandle.world, odeHandle.jointGroup, &contact[i]);
    00285         dJointAttach ( c , dGeomGetBody(contact[i].geom.g1) , dGeomGetBody(contact[i].geom.g2));
    00286       }
    00287     }
    00288     return colwithme;
    00289   }
    00290
    00291   void Nimm2::doInternalStuff(const GlobalData& globalData){
    00292     // dSpaceCollide(car_space, this, mycallback); // checks collisions in the car_space only (not needed)
    00293     irSensorBank.reset(); // reset sensorbank (infrared sensors)
    00294   }
    00295
    00296   /** creates vehicle at desired position
    00297       @param pos struct Position with desired position
    00298   */
    00299   void Nimm2::create(const Matrix& pose){
    00300     if (created) {
    00301       destroy();
    00302     }
    00303
    00304     // create vehicle space and add it to the top level space
    00305     // robot will be inserted in the vehicle space
    00306     odeHandle.space = dSimpleSpaceCreate (parentspace);
    00307
    00308     // create body
    00309     // - create cylinder for main body (with radius and length)
    00310     // - init cylinder with odehandle, mass and osghandle
    00311     // - rotate and place body (here by 90° around the y-axis)
    00312     // - set texture for cylinder
    00313     // - put it into object[0]
    00314     Capsule* cap = new Capsule(width/2, length);
    00315     cap->init(odeHandle, cmass, osgHandle);
    00316     cap->setPose(Matrix::rotate(M_PI/2, 0, 1, 0) * pose);
    00317     cap->getOSGPrimitive()->setTexture("Images/wood.rgb");
    00318     object[0]=cap;
    00319
    00320     // create bumper if required
    00321     // - create cylinder with radius and length
    00322     // - position bumper relative to main body
    00323     //  (using transform object "glues" it together without using joints, see ODE documentation)
    00324     // - init cylinder with odehandle, mass and osghandle
    00325     if (conf.bumper){
    00326       for (int i=0; i<number_bumpers; i++){
    00327         bumper[i].bump = new Capsule(width/4, 2*radius+width/2);
    00328         bumper[i].trans = new Transform(object[0], bumper[i].bump,
    00329                                         Matrix::rotate(M_PI/2.0, Vec3(1, 0, 0)) *
    00330                                         Matrix::translate(0, 0, i==0 ? -(length/2) : (length/2)));
    00331         bumper[i].trans->init(odeHandle, 0, osgHandle);
    00332       }
    00333     }
    00334
    00335     // create wheel bodies
    00336     OsgHandle osgHandleWheels(osgHandle);    // new osghandle with color for wheels
    00337     osgHandleWheels.color = Color(1.0,1.0,1.0);
    00338     for (int i=1; i<3; i++) {
    00339       if(conf.sphereWheels) { // for spherical wheels
    00340         Sphere* wheel = new Sphere(radius);      // create spheres
    00341         wheel->init(odeHandle, wmass, osgHandleWheels); // init with odehandle, mass, and osghandle
    00342
    00343         wheel->setPose(Matrix::rotate(M_PI/2.0, 1, 0, 0) *
    00344                        Matrix::translate(wheeloffset, (i==2 ? -1 : 1) * (width*0.5+wheelthickness), 0) *
    00345                        pose); // place wheels
    00346         wheel->getOSGPrimitive()->setTexture("Images/tire.rgb"); // set texture for wheels
    00347         object[i] = wheel;
    00348       }else{ // for "normal" wheels
    00349         //       Cylinder* wheel = new Cylinder(radius);
    00350         //       wheel->init(odeHandle, wmass, osgHandleWheels);
    00351
    00352         //       wheel->setPose(Matrix::rotate(M_PI/2.0, Vec3(1,0,0)) *
    00353         //                   Matrix::translate(wheeloffset, (i==2 ? -1 : 1) * (width*0.5+wheelthickness), 0)*
    00354         //                   pose);
    00355         //       object[i] = wheel;
    00356       }
    00357     }
    00358
    00359     // set joints between wheels and body (see ODE documentation)
    00360     // - create joint
    00361     // - init joint
    00362     // - set stop parameters
    00363     for (int i=0; i<2; i++) {
    00364       joint[i] = new Hinge2Joint(object[0], object[i+1], object[i+1]->getPosition(),
    00365                                  Axis(0, 0, 1)*pose, Axis(0, -1, 0)*pose);
    00366       joint[i]->init(odeHandle, osgHandleWheels, true, 2.01 * radius);
    00367       // set stops to make sure wheels always stay in alignment
    00368       joint[i]->setParam(dParamLoStop,0);
    00369       joint[i]->setParam(dParamHiStop,0);
    00370     }
    00371
    00372     // initialize sensorbank (for use of infrared sensors)
    00373     irSensorBank.init(odeHandle, osgHandle);
    00374
    00375     if (conf.irFront){ // add front infrared sensors to sensorbank if required
    00376       for(int i=-1; i<2; i+=2){
    00377         IRSensor* sensor = new IRSensor();
    00378         irSensorBank.registerSensor(sensor, object[0],
    00379                                     Matrix::rotate(i*M_PI/10, Vec3(0,0,1)) *
    00380                                     Matrix::translate(0,i*width/10,length/2 + width/2 - width/60 ),
    00381                                     conf.irRange, RaySensor::drawAll);
    00382       }
    00383     }
    00384     // TODO Back , Side sensors
    00385
    00386     created=true;
    00387   };
    00388
    00389
    00390   /** destroys vehicle and space
    00391    */
    00392   void Nimm2::destroy(){
    00393     if (created){
    00394       for (int i=0; i<3; i++){
    00395         if(object[i]) delete object[i];
    00396       }
    00397       for (int i=0; i<2; i++){
    00398         if(joint[i]) delete joint[i];
    00399       }
    00400       for (int i=0; i<2; i++){
    00401         if(bumper[i].bump) delete bumper[i].bump;
    00402         if(bumper[i].trans) delete bumper[i].trans;
    00403       }
    00404       irSensorBank.clear();
    00405       dSpaceDestroy(odeHandle.space);
    00406     }
    00407     created=false;
    00408   }
    00409
    00410 }
    00411