Robotsystem from Robot Group Leipzig: hurlingsnake.cpp Source File

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

- File List
- File Members

# hurlingsnake.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: hurlingsnake.cpp,v $
    00023  *   Revision 1.9.4.4  2005/12/30 22:54:38  martius
    00024  *   removed parentspace!
    00025  *
    00026  *   Revision 1.9.4.3  2005/12/21 17:34:59  martius
    00027  *   moved to osg
    00028  *
    00029  *   Revision 1.9.4.2  2005/11/15 12:29:26  martius
    00030  *   new selforg structure and OdeAgent, OdeRobot ...
    00031  *
    00032  *   Revision 1.9.4.1  2005/11/14 17:37:17  martius
    00033  *   moved to selforg
    00034  *
    00035  *   Revision 1.9  2005/11/09 13:26:31  martius
    00036  *   added factorSensors
    00037  *
    00038  *   Revision 1.8  2005/10/06 17:14:24  martius
    00039  *   switched to stl lists
    00040  *
    00041  *   Revision 1.7  2005/09/22 12:24:37  martius
    00042  *   removed global variables
    00043  *   OdeHandle and GlobalData are used instead
    00044  *   sensor prepared
    00045  *
    00046  *   Revision 1.6  2005/08/31 17:18:15  fhesse
    00047  *   setTextures added, Mass is now sphere (not box anymore)
    00048  *
    00049  *   Revision 1.5  2005/08/29 06:41:22  martius
    00050  *   kosmetik
    00051  *
    00052  *   Revision 1.4  2005/08/03 20:35:28  martius
    00053  *   *** empty log message ***
    00054  *
    00055  *   Revision 1.3  2005/07/27 13:23:09  martius
    00056  *   new color and position construction
    00057  *
    00058  *   Revision 1.2  2005/07/26 17:04:21  martius
    00059  *   lives in its own space now
    00060  *
    00061  *   Revision 1.1  2005/07/21 12:17:04  fhesse
    00062  *   new hurling snake, todo: add collision space, clean up, comment
    00063  *
    00064  *
    00065  *                                                                 *
    00066  ***************************************************************************/
    00067
    00068 #include "hurlingsnake.h"
    00069
    00070 namespace lpzrobots {
    00071
    00072   /**
    00073    * Constructor
    00074    * @param w world in which robot should be created
    00075    * @param s space in which robot should be created
    00076    * @param c contactgroup for collision treatment
    00077    */
    00078   HurlingSnake::HurlingSnake(const OdeHandle& odeHandle, const OsgHandle& osgHandle)
    00079     : OdeRobot(odeHandle, osgHandle), oldp(0,0,0){
    00080
    00081     // prepare name;
    00082     name.clear();
    00083     Configurable::insertCVSInfo(name, "$RCSfile: hurlingsnake.cpp,v $",
    00084                                 "$Revision: 1.9.4.4 $");
    00085
    00086     factorForce=3.0;
    00087     factorSensor=20.0;
    00088     frictionGround=0.3;
    00089
    00090     created=false;
    00091
    00092     this->osgHandle.color=Color(1,1,0);
    00093
    00094     NUM= 10;            /* number of spheres */
    00095     //    SIDE= 0.2;            /* side length of a box */
    00096     MASS= 1.0;          /* mass of a sphere*/
    00097     RADIUS= 0.1732f;    /* sphere radius */
    00098
    00099     sensorno = 2;
    00100     motorno  = 2;
    00101
    00102   };
    00103
    00104
    00105   void HurlingSnake::update(){
    00106     assert(created); // robot must exist
    00107
    00108     for (int i=0; i<NUM; i++) {
    00109       object[i]->update();
    00110     }
    00111     for (int i=0; i < NUM-1; i++) {
    00112       joint[i]->update();
    00113     }
    00114   }
    00115
    00116   void HurlingSnake::place(const osg::Matrix& pose){
    00117     // lift the snake about its radius
    00118     osg::Matrix p2;
    00119     p2 = pose * osg::Matrix::translate(osg::Vec3(0, 0, RADIUS));
    00120     create(p2);
    00121   };
    00122
    00123   void HurlingSnake::doInternalStuff(const GlobalData& global){
    00124     // mycallback is called for internal collisions! Only once per step
    00125     dSpaceCollide(odeHandle.space, this, mycallback);
    00126   }
    00127
    00128   void HurlingSnake::mycallback(void *data, dGeomID o1, dGeomID o2){
    00129     // internal collisions
    00130     HurlingSnake* me = (HurlingSnake*)data;
    00131     int i,n;
    00132     const int N = 10;
    00133     dContact contact[N];
    00134     n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
    00135     for (i=0; i<n; i++){
    00136       contact[i].surface.mode = 0;
    00137       contact[i].surface.mu = 0;
    00138       contact[i].surface.mu2 = 0;
    00139       //     contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
    00140       //       dContactSoftERP | dContactSoftCFM | dContactApprox1;
    00141       //     contact[i].surface.mu = 0.0;
    00142       //     contact[i].surface.slip1 = 0.005;
    00143       //     contact[i].surface.slip2 = 0.005;
    00144       //     contact[i].surface.soft_erp = 1;
    00145       //     contact[i].surface.soft_cfm = 0.00001;
    00146       dJointID c = dJointCreateContact( me->odeHandle.world, me->odeHandle.jointGroup, &contact[i]);
    00147       dJointAttach ( c , dGeomGetBody(contact[i].geom.g1) , dGeomGetBody(contact[i].geom.g2)) ;
    00148     }
    00149   }
    00150   bool HurlingSnake::collisionCallback(void *data, dGeomID o1, dGeomID o2){
    00151     //checks if one of the collision objects is part of the robot
    00152     if( o1 == (dGeomID)odeHandle.space || o2 == (dGeomID)odeHandle.space){
    00153
    00154       // the rest is for collisions of some snake elements with the rest of the world
    00155       int i,n;
    00156       const int N = 10;
    00157       dContact contact[N];
    00158
    00159       n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
    00160       for (i=0; i<n; i++){
    00161         contact[i].surface.mode = 0;
    00162         contact[i].surface.mu = frictionGround;
    00163         contact[i].surface.mu2 = 0;
    00164         //      contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
    00165         //        dContactSoftERP | dContactSoftCFM | dContactApprox1;
    00166         //      contact[i].surface.mu = frictionGround;
    00167         //      contact[i].surface.slip1 = 0.005;
    00168         //      contact[i].surface.slip2 = 0.005;
    00169         //      contact[i].surface.soft_erp = 1;
    00170         //      contact[i].surface.soft_cfm = 0.00001;
    00171         dJointID c = dJointCreateContact( odeHandle.world, odeHandle.jointGroup, &contact[i]);
    00172         dJointAttach ( c , dGeomGetBody(contact[i].geom.g1) , dGeomGetBody(contact[i].geom.g2)) ;
    00173       }
    00174       return true;
    00175     }
    00176     return false;
    00177   }
    00178
    00179
    00180
    00181   /** returns actual sensorvalues
    00182       @param sensors sensors scaled to [-1,1]
    00183       @param sensornumber length of the sensor array
    00184       @return number of actually written sensors
    00185   */
    00186   int HurlingSnake::getSensors(sensor* sensors, int sensornumber){
    00187     int len = (sensornumber < sensorno)? sensornumber : sensorno;
    00188
    00189     Pos p(object[NUM-1]->getPosition());      //read actual position
    00190     Pos s = (p - oldp)*factorSensor;
    00191
    00192     sensors[0]=s.x();
    00193     sensors[1]=s.y();
    00194     oldp=p;
    00195     return len;
    00196   }
    00197
    00198
    00199   /** sets actual motorcommands
    00200       @param motors motors scaled to [-1,1]
    00201       @param motornumber length of the motor array
    00202   */
    00203   void HurlingSnake::setMotors(const motor* motors, int motornumber){
    00204     //  dBodyAddForce (object[NUM-1].body,motors[0]*factorForce,motors[1]*factorForce,motors[2]*factorForce);
    00205     dBodyAddForce (object[NUM-1]->getBody(),motors[0]*factorForce,motors[1]*factorForce,0);
    00206   }
    00207
    00208
    00209   /** returns number of sensors
    00210    */
    00211   int HurlingSnake::getSensorNumber(){
    00212     return sensorno;
    00213   }
    00214
    00215   /** returns number of motors
    00216    */
    00217   int HurlingSnake::getMotorNumber(){
    00218     return motorno;
    00219   }
    00220
    00221
    00222   /** returns a vector with the positions of all segments of the robot
    00223       @param vector of positions (of all robot segments)
    00224       @return length of the list
    00225   */
    00226   int HurlingSnake::getSegmentsPosition(vector<Position> &poslist){
    00227     Position pos;
    00228     for (int i=0; i<NUM; i++){
    00229       Pos p = object[i]->getPosition();
    00230       poslist.push_back(p.toPosition());
    00231     }
    00232     return NUM;
    00233   }
    00234
    00235
    00236
    00237   void HurlingSnake::create(const osg::Matrix& pose){
    00238     if (created){
    00239       destroy();
    00240     }
    00241     // create vehicle space and add it to parentspace
    00242     odeHandle.space = dSimpleSpaceCreate (parentspace);
    00243
    00244     for (int i=0; i<NUM; i++) {
    00245       object[i] = new Sphere(RADIUS);
    00246       if (i==NUM-1){
    00247         OsgHandle osgHandle_head = osgHandle;
    00248         osgHandle_head.color = Color(1, 0, 0);
    00249         object[i]->init(odeHandle, MASS, osgHandle_head);
    00250       } else {
    00251         object[i]->init(odeHandle, MASS, osgHandle);
    00252       }
    00253       object[i]->setPose(osg::Matrix::translate(i*RADIUS*2*1.1, 0, 0) * pose);
    00254       object[i]->getOSGPrimitive()->setTexture("Images/wood.rgb");
    00255     }
    00256     oldp = object[NUM-1]->getPosition();
    00257     for (int i=0; i<(NUM-1); i++) {
    00258       Pos p1(object[i]->getPosition());
    00259       Pos p2(object[i+1]->getPosition());
    00260       joint[i] = new BallJoint(object[i],object[i+1], (p1+p2)/2 );
    00261       joint[i]->init(odeHandle, osgHandle, true, RADIUS/10);
    00262     }
    00263
    00264     created=true;
    00265   }
    00266
    00267
    00268   /** destroys robot
    00269    */
    00270   void HurlingSnake::destroy(){
    00271     if (created){
    00272       for (int i=0; i<NUM; i++){
    00273         if(object[i]) delete object[i];
    00274       }
    00275       for (int i=0; i<NUM-1; i++){
    00276         if(joint[i]) delete joint[i];
    00277       }
    00278       dSpaceDestroy(odeHandle.space);
    00279     }
    00280     created=false;
    00281   }
    00282
    00283
    00284
    00285   Configurable::paramlist HurlingSnake::getParamList() const{
    00286     paramlist list;
    00287     list.push_back( pair<paramkey, paramval> (string("factorForce"), factorForce));
    00288     list.push_back( pair<paramkey, paramval> (string("factorSensor"), factorSensor));
    00289     list.push_back( pair<paramkey, paramval> (string("frictionGround"), frictionGround));
    00290     list.push_back( pair<paramkey, paramval> (string("place"), 0));
    00291     return list;
    00292   }
    00293
    00294
    00295   Configurable::paramval HurlingSnake::getParam(const paramkey& key) const{
    00296     if(key == "factorForce") return factorForce;
    00297     else if(key == "factorSensor") return factorSensor;
    00298     else if(key == "frictionGround") return frictionGround;
    00299     else  return Configurable::getParam(key) ;
    00300   }
    00301
    00302
    00303   bool HurlingSnake::setParam(const paramkey& key, paramval val){
    00304     if(key == "factorForce") factorForce=val;
    00305     else if(key == "factorSensor") factorSensor=val;
    00306     else if(key == "frictionGround") frictionGround=val;
    00307     else if(key == "place") {
    00308       OdeRobot::place(Pos(0,0,3)) ;
    00309     }
    00310     else return Configurable::setParam(key, val);
    00311     return true;
    00312   }
    00313
    00314 }
    00315