Robotsystem from Robot Group Leipzig: schlange.cpp Source File

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

- File List
- File Members

# schlange.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: schlange.cpp,v $
    00023  *   Revision 1.20.4.6  2006/02/23 18:05:04  martius
    00024  *   friction with angularmotor
    00025  *
    00026  *   Revision 1.20.4.5  2006/02/01 18:33:40  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.20.4.4  2006/01/18 09:55:54  martius
    00030  *   created was uninitialised
    00031  *
    00032  *   Revision 1.20.4.3  2005/12/30 22:53:46  martius
    00033  *   removed parentspace init because done in oderobot
    00034  *
    00035  *   Revision 1.20.4.2  2005/12/29 16:45:58  martius
    00036  *   does not inherit from Roboter
    00037  *   moved to osg
    00038  *
    00039  *   Revision 1.20.4.1  2005/11/15 12:29:27  martius
    00040  *   new selforg structure and OdeAgent, OdeRobot ...
    00041  *
    00042  *   Revision 1.20  2005/11/09 13:24:42  martius
    00043  *   added GPL
    00044  *
    00045  ***************************************************************************/
    00046
    00047 #include "schlange.h"
    00048
    00049 namespace lpzrobots {
    00050
    00051   Schlange::Schlange ( const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00052                        const SchlangeConf& conf, const char* name )
    00053     : OdeRobot( odeHandle, osgHandle, name), conf(conf) {
    00054
    00055     // prepare name;
    00056     Configurable::insertCVSInfo(this->name, "$RCSfile: schlange.cpp,v $",
    00057                                 "$Revision: 1.20.4.6 $");
    00058     created=false;
    00059   }
    00060
    00061   Schlange::~Schlange()
    00062   {
    00063     if(created) destroy();
    00064   }
    00065
    00066
    00067   void Schlange::place(const osg::Matrix& pose){
    00068     // the position of the robot is the center of the body (without wheels)
    00069     // to set the vehicle on the ground when the z component of the position is 0
    00070     // width*0.6 is added (without this the wheels and half of the robot will be in the ground)
    00071     create(pose * osg::Matrix::translate(osg::Vec3(0, 0, conf.segmDia/2)));
    00072   }
    00073
    00074   void Schlange::update(){
    00075     assert(created); // robot must exist
    00076     for (vector<Primitive*>::iterator i = objects.begin(); i!= objects.end(); i++){
    00077       if(*i) (*i)->update();
    00078     }
    00079     for (vector<Joint*>::iterator i = joints.begin(); i!= joints.end(); i++){
    00080       if(*i) (*i)->update();
    00081     }
    00082   }
    00083
    00084   void Schlange::doInternalStuff(const GlobalData& global){
    00085     if(created){
    00086       // mycallback is called for internal collisions! Only once per step
    00087       dSpaceCollide(odeHandle.space, this, mycallback);
    00088     }
    00089   }
    00090
    00091   void Schlange::mycallback(void *data, dGeomID o1, dGeomID o2)
    00092   {
    00093     Schlange* me = (Schlange*) data;
    00094     int i=0;
    00095     int o1_index= -1;
    00096     int o2_index= -1;
    00097     for (vector<Primitive*>::iterator n = me->objects.begin(); n!= me->objects.end(); n++, i++){
    00098       if( (*n)->getGeom() == o1)
    00099         o1_index=i;
    00100       if( (*n)->getGeom() == o2)
    00101         o2_index=i;
    00102     }
    00103
    00104     if(o1_index >= 0 && o2_index >= 0 && abs(o1_index - o2_index) > 1){
    00105       // internal collisions
    00106       int i,n;
    00107       const int N = 10;
    00108       dContact contact[N];
    00109       n = dCollide (o1, o2, N, &contact[0].geom, sizeof(dContact));
    00110       for (i=0; i<n; i++) {
    00111         contact[i].surface.mode = 0;
    00112         contact[i].surface.mu = 0;
    00113         contact[i].surface.mu2 = 0;
    00114         dJointID c = dJointCreateContact( me->odeHandle.world, me->odeHandle.jointGroup, &contact[i]);
    00115         dJointAttach ( c , dGeomGetBody(contact[i].geom.g1) , dGeomGetBody(contact[i].geom.g2)) ;
    00116       }
    00117     }
    00118   }
    00119
    00120
    00121   /**
    00122    *This is the collision handling function for snake robots.
    00123    *This overwrides the function collisionCallback of the class robot.
    00124    *@param data
    00125    *@param o1 first geometrical object, which has taken part in the collision
    00126    *@param o2 second geometrical object, which has taken part in the collision
    00127    *@return true if the collision was threated  by the robot, false if not
    00128    **/
    00129   bool Schlange::collisionCallback(void *data, dGeomID o1, dGeomID o2)
    00130   {
    00131     //checks if one of the collision objects is part of the robot
    00132     if( o1 == (dGeomID)odeHandle.space || o2 == (dGeomID)odeHandle.space ){
    00133       int i,n;
    00134       const int N = 20;
    00135       dContact contact[N];
    00136       n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
    00137       for (i=0; i<n; i++){
    00138         //      contact[i].surface.mode = dContactMu2 | dContactSlip1 | dContactSlip2 |
    00139         //      dContactSoftERP | dContactSoftCFM | dContactApprox1;
    00140         contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
    00141           dContactSoftERP | dContactSoftCFM | dContactApprox1;
    00142         contact[i].surface.slip1 = 0.001;
    00143         contact[i].surface.slip2 = 0.001;
    00144         contact[i].surface.mu = conf.frictionGround; //*10;
    00145         //      contact[i].surface.mu2 = conf.frictionGround;
    00146         contact[i].surface.soft_erp = 0.9;
    00147         contact[i].surface.soft_cfm = 0.001;
    00148
    00149         dJointID c = dJointCreateContact( odeHandle.world, odeHandle.jointGroup, &contact[i]);
    00150         dJointAttach ( c , dGeomGetBody(contact[i].geom.g1) , dGeomGetBody(contact[i].geom.g2));
    00151       }
    00152       return true;
    00153     }
    00154     return false;
    00155   }
    00156
    00157
    00158
    00159   /** The list of all parameters with there value as allocated lists.
    00160       @param keylist,vallist will be allocated with malloc (free it after use!)
    00161       @return length of the lists
    00162   */
    00163   Configurable::paramlist Schlange::getParamList() const{
    00164     paramlist list;
    00165     list += pair<paramkey, paramval> (string("frictionground"), conf.frictionGround);
    00166     list += pair<paramkey, paramval> (string("frictionjoint"), conf.frictionJoint);
    00167     list += pair<paramkey, paramval> (string("motorpower"),   conf.motorPower);
    00168     list += pair<paramkey, paramval> (string("sensorfactor"), conf.sensorFactor);
    00169     return list;
    00170   }
    00171
    00172
    00173   Configurable::paramval Schlange::getParam(const paramkey& key) const{
    00174     if(key == "frictionground") return conf.frictionGround;
    00175     else if(key == "frictionjoint") return conf.frictionJoint;
    00176     else if(key == "motorpower") return conf.motorPower;
    00177     else if(key == "sensorfactor") return conf.sensorFactor;
    00178     else  return Configurable::getParam(key) ;
    00179   }
    00180
    00181   bool Schlange::setParam(const paramkey& key, paramval val){
    00182     if(key == "frictionground") conf.frictionGround = val;
    00183     else if(key == "motorpower") conf.motorPower = val;
    00184     else if(key == "sensorfactor") conf.sensorFactor = val;
    00185     else if(key == "frictionjoint") {
    00186       conf.frictionJoint = val;
    00187       for (vector<AngularMotor*>::iterator i = frictionmotors.begin(); i!= frictionmotors.end(); i++){
    00188         if (*i) (*i)->setPower(conf.frictionJoint);
    00189       }
    00190     } else
    00191       return Configurable::setParam(key, val);
    00192     return true;
    00193   }
    00194
    00195
    00196   int Schlange::getSegmentsPosition(vector<Position> &poslist){
    00197     assert(created);
    00198     for(int n = 0; n < conf.segmNumber; n++){
    00199       Pos p(objects[n]->getPosition());
    00200       poslist.push_back(p.toPosition());
    00201     }
    00202     return conf.segmNumber;
    00203   }
    00204
    00205
    00206
    00207   /** creates vehicle at desired position
    00208       @param pos struct Position with desired position
    00209   */
    00210   void Schlange::create(const osg::Matrix& pose){
    00211     if (created) {
    00212       destroy();
    00213     }
    00214
    00215     odeHandle.space = dSimpleSpaceCreate (parentspace);
    00216
    00217     int half = conf.segmNumber/2;
    00218     for ( int n = 0; n < conf.segmNumber; n++ ) {
    00219       Primitive* p = new Capsule(conf.segmDia/2 , conf.segmLength);
    00220       p->init(odeHandle, conf.segmMass, osgHandle);
    00221       p->setPose(osg::Matrix::rotate(M_PI/2, 0, 1, 0) *
    00222                  osg::Matrix::translate((n-half)*conf.segmLength, 0 , conf.segmDia/2) *
    00223                  pose);
    00224       p->getOSGPrimitive()->setTexture("Images/wood.rgb");
    00225       objects.push_back(p);
    00226     }
    00227
    00228     created=true;
    00229   };
    00230
    00231
    00232   /** destroys vehicle and space
    00233    */
    00234   void Schlange::destroy(){
    00235     if (created){
    00236       for (vector<Primitive*>::iterator i = objects.begin(); i!= objects.end(); i++){
    00237         if(*i) delete *i;
    00238       }
    00239       objects.clear();
    00240       for (vector<Joint*>::iterator i = joints.begin(); i!= joints.end(); i++){
    00241         if(*i) delete *i;
    00242       }
    00243       joints.clear();
    00244       for (vector<AngularMotor*>::iterator i = frictionmotors.begin(); i!= frictionmotors.end(); i++){
    00245         if(*i) delete *i;
    00246       }
    00247       frictionmotors.clear();
    00248       dSpaceDestroy(odeHandle.space);
    00249     }
    00250     created=false;
    00251   }
    00252
    00253
    00254
    00255
    00256 //   /** fix segment 0 in the sky
    00257 //    */
    00258 //   void Schlange::fixInSky(){
    00259 //     for (int i=0; i<2; i++){
    00260 //       skyJoints.push_back( dJointCreateHinge ( world , 0 ) );
    00261 //       dJointAttach ( skyJoints.back(), objektliste[0].body , 0 );
    00262 //       dJointSetUniversalAnchor ( skyJoints.back(),
    00263 //                               dBodyGetPositionAll ( objektliste[0].body , 1 ) ,
    00264 //                               dBodyGetPositionAll ( objektliste[0].body , 2 ) ,
    00265 //                               dBodyGetPositionAll ( objektliste[0].body , 3 ) );
    00266 //       if (i==0) dJointSetHingeAxis(skyJoints.back(),1,0,0);
    00267 //       if (i==1) dJointSetHingeAxis(skyJoints.back(),0,1,0);
    00268 //       dJointSetFixed(skyJoints.back());
    00269 //     }
    00270 //     /*
    00271 //       jointliste.push_back( dJointCreateHinge ( world , 0 ) );
    00272 //       dJointAttach ( jointliste.back() , objektliste[0].body , 0 );
    00273 //       dJointSetUniversalAnchor ( jointliste.back() ,
    00274 //       dBodyGetPositionAll ( objektliste[0].body , 1 ) ,
    00275 //       dBodyGetPositionAll ( objektliste[0].body , 2 ) ,
    00276 //       dBodyGetPositionAll ( objektliste[0].body , 3 ) );
    00277 //       dJointSetHingeAxis(jointliste.back(),0,1,0);
    00278 //       dJointSetFixed(jointliste.back());
    00279 //     */
    00280 //   };
    00281
    00282
    00283 }
    00284
    00285
    00286