Robotsystem from Robot Group Leipzig: sphererobot.cpp Source File

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

- File List
- File Members

# sphererobot.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: sphererobot.cpp,v $
    00023  *   Revision 1.11.4.5  2006/01/10 17:17:17  martius
    00024  *   new mode for primitives
    00025  *
    00026  *   Revision 1.11.4.4  2006/01/10 15:10:24  martius
    00027  *   fine tuning, fuer controllinterval 1
    00028  *   still not transparent
    00029  *
    00030  *   Revision 1.11.4.3  2005/12/30 22:51:45  martius
    00031  *   moved to osg
    00032  *
    00033  *   Revision 1.11.4.2  2005/11/15 12:29:27  martius
    00034  *   new selforg structure and OdeAgent, OdeRobot ...
    00035  *
    00036  *   Revision 1.11.4.1  2005/11/14 17:37:18  martius
    00037  *   moved to selforg
    00038  *
    00039  *   Revision 1.11  2005/11/09 13:24:42  martius
    00040  *   added GPL
    00041  *
    00042  ***************************************************************************/
    00043 #include <assert.h>
    00044
    00045 #include "sphererobot.h"
    00046 #include "primitive.h"
    00047 #include "joint.h"
    00048 #include "sliderservo.h"
    00049 #include "invisibleprimitive.h"
    00050
    00051 #include "matrix.h"
    00052 using namespace matrix;
    00053
    00054 namespace lpzrobots {
    00055
    00056   const int Sphererobot::sensorno;
    00057
    00058   Sphererobot::Sphererobot ( const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00059                              const SphererobotConf& conf, const char* name )
    00060     : OdeRobot ( odeHandle, osgHandle, name ), conf(conf) {
    00061
    00062
    00063     created = false;
    00064     memset(object, 0,sizeof(void*) * Last);
    00065     memset(joint,  0,sizeof(void*) * 6);
    00066     memset(slider, 0,sizeof(void*) * 3);
    00067   }
    00068
    00069   Sphererobot::~Sphererobot() {
    00070     destroy();
    00071   }
    00072
    00073   void Sphererobot::update(){
    00074     assert(created); // robot must exist
    00075
    00076     for (int i=0; i<Last; i++) {
    00077       if(object[i]) object[i]->update();
    00078     }
    00079     for (int i=0; i < 6; i++) {
    00080       if(joint[i]) joint[i]->update();
    00081     }
    00082     for (int i=0; i < 3; i++) {
    00083       if(slider[i]) slider[i]->update();
    00084     }
    00085   }
    00086
    00087
    00088   int Sphererobot::getSensors ( sensor* sensors, int sensornumber ) {
    00089     int len = min(sensornumber, 3);
    00090     for ( int n = 0; n < len; n++ ) {
    00091       sensors[n] = servo[n]->get();
    00092     }
    00093
    00094     double data[3] = {1,0,0};
    00095     Matrix v(3,1,data);
    00096     Matrix A = odeRto3x3RotationMatrix(dBodyGetRotation(object[Base]->getBody()));
    00097     Matrix v2 = A*v;
    00098     v.val(0,0)=0;
    00099     v.val(1,0)=1;
    00100     Matrix v3 = A * v;
    00101     int l= v2.convertToBuffer(sensors+3, sensornumber -3);
    00102     return v3.convertToBuffer(sensors + l + 3 , sensornumber - l -3) + l + 3;
    00103   }
    00104
    00105   void Sphererobot::setMotors ( const motor* motors, int motornumber ) {
    00106     int len = min(motornumber, 3);
    00107     for ( int n = 0; n < len; n++ ) {
    00108       servo[n]->set(motors[n]);
    00109     }
    00110   }
    00111
    00112
    00113   void Sphererobot::place(const osg::Matrix& pose){
    00114     // the position of the robot is the center of the body (without wheels)
    00115     // to set the vehicle on the ground when the z component of the position is 0
    00116     // width*0.6 is added (without this the wheels and half of the robot will be in the ground)
    00117     osg::Matrix p2;
    00118     p2 = pose * osg::Matrix::translate(osg::Vec3(0, 0, conf.diameter/2));
    00119     create(p2);
    00120   };
    00121
    00122   void Sphererobot::doInternalStuff(const GlobalData& global){}
    00123
    00124   bool Sphererobot::collisionCallback(void *data, dGeomID o1, dGeomID o2) {
    00125     //checks if both of the collision objects are part of the robot
    00126     if( o1 == (dGeomID)odeHandle.space || o2 == (dGeomID)odeHandle.space) {
    00127       int i,n;
    00128       const int N = 10;
    00129       dContact contact[N];
    00130
    00131       n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
    00132       for (i=0; i<n; i++) {
    00133         if( contact[i].geom.g1 == object[Base]->getGeom() || contact[i].geom.g2 == object[Base]->getGeom() ){
    00134           // only treat collisions with envelop
    00135           contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1;
    00136           contact[i].surface.mu = 1.0;
    00137           contact[i].surface.soft_erp = 0.5;
    00138           contact[i].surface.soft_cfm = 0.1;
    00139           //    contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
    00140           //      dContactSoftERP | dContactSoftCFM | dContactApprox1;
    00141           //    contact[i].surface.mu = frictionGround;
    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( odeHandle.world, odeHandle.jointGroup, &contact[i]);
    00147           dJointAttach ( c , dGeomGetBody(contact[i].geom.g1) , dGeomGetBody(contact[i].geom.g2));
    00148         }
    00149       }
    00150       return true;
    00151     } else {
    00152       return false;
    00153     }
    00154   }
    00155
    00156
    00157   /**
    00158    *Returns the number of motors used by the snake.
    00159    *@return number of motors
    00160    *@author Marcel Kretschmann
    00161    *@version final
    00162    **/
    00163   int Sphererobot::getMotorNumber(){
    00164     return 3;
    00165   }
    00166
    00167   /**
    00168    *Returns the number of sensors used by the robot.
    00169    *@return number of sensors
    00170    *@author Marcel Kretschmann
    00171    *@version final
    00172    **/
    00173   int Sphererobot::getSensorNumber() {
    00174     return sensorno;
    00175   }
    00176
    00177   /** returns a vector with the positions of all segments of the robot
    00178       @param vector of positions (of all robot segments)
    00179       @return length of the list
    00180   */
    00181   int Sphererobot::getSegmentsPosition(vector<Position> &poslist){
    00182     poslist.push_back(Pos(object[Base]->getPosition()).toPosition() );
    00183     poslist.push_back(Pos(object[Pendular]->getPosition()).toPosition() );
    00184     return 2;
    00185   }
    00186
    00187
    00188
    00189   /** creates vehicle at desired position
    00190       @param pos struct Position with desired position
    00191   */
    00192   void Sphererobot::create(const osg::Matrix& pose){
    00193     if (created) {
    00194       destroy();
    00195     }
    00196
    00197     // create vehicle space and add it to the top level space
    00198     odeHandle.space = dSimpleSpaceCreate (parentspace);
    00199     OsgHandle osgHandle_bottom = osgHandle.changeColor(Color(1.0, 0, 0));
    00200     OsgHandle osgHandle_pendular = osgHandle.changeColor(Color(0.0, 1.0 , 0));
    00201
    00202     object[Base] = new InvisibleSphere(conf.diameter/2);
    00203     //object[Base] = new Sphere(conf.diameter/2);
    00204     //object[Base] = new InvisibleBox(conf.diameter, conf.diameter, conf.diameter);
    00205     object[Base]->init(odeHandle, conf.spheremass, osgHandle);
    00206     object[Base]->setPose(pose);
    00207
    00208     //pendular body
    00209     object[Pendular] = new Sphere(conf.pendulardiameter/2);
    00210     object[Pendular]->init(odeHandle, conf.spheremass, osgHandle_pendular);
    00211     object[Pendular]->setPose(pose);
    00212
    00213     //first and second 3 connection bodies between the pendular an the sphere
    00214     double x , y;
    00215     for ( unsigned int alpha = 0; alpha < 3; alpha++ ) {
    00216       x=sin ( (float) alpha*2*M_PI/3 )*conf.diameter/3.5; //testing values
    00217       y=cos ( (float) alpha*2*M_PI/3 )*conf.diameter/3.5;
    00218
    00219       object[Pole1Bot+alpha] = new Box(conf.diameter/50, conf.diameter/50, conf.diameter/50);
    00220       object[Pole1Bot+alpha]->init(odeHandle, conf.slidermass, osgHandle_bottom,
    00221                                    Primitive::Body | Primitive::Draw);
    00222       object[Pole1Bot+alpha]->setPose(osg::Matrix::translate(x,y,- conf.diameter/2 + conf.diameter/9) *
    00223                                       pose);
    00224
    00225       object[Pole1Top+alpha] = new Box(conf.diameter/50, conf.diameter/50, conf.diameter/50);
    00226       object[Pole1Top+alpha]->init(odeHandle, conf.slidermass, osgHandle_pendular,
    00227                                    Primitive::Body | Primitive::Draw);
    00228       object[Pole1Top+alpha]->setPose(osg::Matrix::translate(x,y,0) * pose);
    00229
    00230       //combines the 3 upper connection bodies with the pendular
    00231       joint[alpha] = new HingeJoint(object[Pendular], object[Pole1Top+alpha],
    00232                                     object[Pole1Top+alpha]->getPosition(),
    00233                                     osg::Vec3(y, -x, 0));
    00234       joint[alpha]->init(odeHandle, osgHandle, true, conf.diameter/20);
    00235       //  dJointSetHingeParam ( hinge, dParamLoStop, -conf.hingeRange);
    00236       //     dJointSetHingeParam ( hinge, dParamHiStop,  conf.hingeRange);
    00237       //     dJointSetHingeParam  ( hinge, dParamCFM, 0.1);
    00238       //     dJointSetHingeParam ( hinge, dParamStopCFM, 0.1);
    00239       //     dJointSetHingeParam ( hinge, dParamStopERP, 0.9);
    00240
    00241       //combines the 3 lower connection bodies with the base
    00242       joint[alpha+3] = new BallJoint(object[Base], object[Pole1Bot+alpha],
    00243                                      object[Pole1Bot+alpha]->getPosition());
    00244       joint[alpha+3]->init(odeHandle, osgHandle, true, conf.diameter/40) ;
    00245
    00246       //definition of the 3 Slider-Joints, which are the controled by the robot-controler
    00247       slider[alpha] = new SliderJoint(object[Pole1Top+alpha], object[Pole1Bot+alpha],
    00248                                       (object[Pole1Top+alpha]->getPosition() +
    00249                                        object[Pole1Bot+alpha]->getPosition())/2,
    00250                                       object[Pole1Top+alpha]->getPosition() -
    00251                                       object[Pole1Bot+alpha]->getPosition() );
    00252       slider[alpha]->init(odeHandle, osgHandle, true, conf.diameter*conf.sliderrange);
    00253       // the Stop parameters are messured from the initial position!
    00254       slider[alpha]->setParam(dParamLoStop, -1.1*conf.diameter*conf.sliderrange );
    00255       slider[alpha]->setParam(dParamHiStop, 1.1*conf.diameter*conf.sliderrange );
    00256       slider[alpha]->setParam(dParamCFM, 0.1);
    00257       slider[alpha]->setParam(dParamStopCFM, 0.1);
    00258       slider[alpha]->setParam(dParamStopERP, 0.9);
    00259
    00260       servo[alpha] = new SliderServo(slider[alpha], -conf.diameter*conf.sliderrange,
    00261                                      conf.diameter*conf.sliderrange,
    00262                                      conf.pendularmass*0.1 * conf.force);
    00263
    00264     }
    00265
    00266     created=true;
    00267   };
    00268
    00269
    00270
    00271   /** destroys vehicle and space
    00272    */
    00273   void Sphererobot::destroy(){
    00274     if (created){
    00275       for (int i=0; i<Last; i++){
    00276         if(object[i]) delete object[i];
    00277       }
    00278       for (int i=0; i<6; i++){
    00279         if(joint[i]) delete joint[i];
    00280       }
    00281       for (int i=0; i<3; i++){
    00282         if(slider[i]) delete slider[i];
    00283       }
    00284
    00285       dSpaceDestroy(odeHandle.space);
    00286     }
    00287     created=false;
    00288   }
    00289
    00290
    00291 }
    00292
    00293
    00294