Robotsystem from Robot Group Leipzig: forcedsphere.cpp Source File

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

- File List
- File Members

# forcedsphere.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: forcedsphere.cpp,v $
    00023  *   Revision 1.4.4.3  2006/01/10 22:25:09  martius
    00024  *   moved to osg
    00025  *
    00026  *   Revision 1.1.2.1  2006/01/10 17:15:04  martius
    00027  *   was sphererobotarms
    00028  *   moved to osg
    00029  *
    00030  *   Revision 1.18.4.2  2005/11/15 12:29:27  martius
    00031  *   new selforg structure and OdeAgent, OdeRobot ...
    00032  *
    00033  *   Revision 1.18.4.1  2005/11/14 17:37:18  martius
    00034  *   moved to selforg
    00035  *
    00036  *   Revision 1.18  2005/11/09 13:26:57  martius
    00037  *   irsensorrange
    00038  *
    00039  *   Revision 1.17  2005/11/09 13:24:42  martius
    00040  *   added GPL
    00041  *
    00042  ***************************************************************************/
    00043
    00044 #include <assert.h>
    00045 #include <matrix.h>
    00046 #include <osg/Matrix>
    00047 #include "primitive.h"
    00048 #include "forcedsphere.h"
    00049
    00050 using namespace osg;
    00051
    00052 namespace lpzrobots {
    00053
    00054   /**
    00055    *constructor
    00056    **/
    00057   ForcedSphere::ForcedSphere ( const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00058                  const char* name, double radius, double max_force)
    00059     : OdeRobot ( odeHandle, osgHandle, name ), radius(radius), max_force(max_force)
    00060   {
    00061
    00062     created = false;
    00063     object[0] = 0;
    00064
    00065   }
    00066
    00067   ForcedSphere::~ForcedSphere()
    00068   {
    00069     destroy();
    00070   }
    00071
    00072   void ForcedSphere::update()
    00073   {
    00074     if(object[0]) object[0]->update();
    00075   }
    00076
    00077   /**
    00078    *Writes the sensor values to an array in the memory.
    00079    *@param sensor* pointer to the array
    00080    *@param sensornumber length of the sensor array
    00081    *@return number of actually written sensors
    00082    **/
    00083   int ForcedSphere::getSensors ( sensor* sensors, int sensornumber )
    00084   {
    00085     int len=0;
    00086     matrix::Matrix A = odeRto3x3RotationMatrix ( dBodyGetRotation ( object[0]->getBody() ) );
    00087
    00088     // z-coordinate of axis position in world coordinates
    00089     len += A.row(2).convertToBuffer(sensors+len, sensornumber-len);
    00090     // rotation matrix - 9 (vectors of all axis in world coordinates
    00091     //len += A.convertToBuffer(sensors + len , sensornumber -len);
    00092
    00093     return len;
    00094   }
    00095
    00096   /**
    00097    *Reads the actual motor commands from an array, an sets all motors of the snake to this values.
    00098    *It is an linear allocation.
    00099    *@param motors pointer to the array, motor values are scaled to [-1,1]
    00100    *@param motornumber length of the motor array
    00101    **/
    00102   void ForcedSphere::setMotors ( const motor* motors, int motornumber ) {
    00103     if (motornumber==2){
    00104       dBodyAddForce(object[0]->getBody(), motors[0]*max_force, motors[1]*max_force, 0);
    00105     }
    00106   }
    00107
    00108
    00109   void ForcedSphere::place(const osg::Matrix& pose){
    00110     osg::Matrix p2;
    00111     p2 = pose * osg::Matrix::translate(osg::Vec3(0, 0, radius));
    00112     create(p2);
    00113   };
    00114
    00115
    00116   void ForcedSphere::doInternalStuff(const GlobalData& global){
    00117   }
    00118
    00119   /**
    00120    *This is the collision handling function for sphere robots.
    00121    *This overwrides the function collisionCallback of the class robot.
    00122    *@param data
    00123    *@param o1 first geometrical object, which has taken part in the collision
    00124    *@param o2 second geometrical object, which has taken part in the collision
    00125    *@return true if the collision was threated  by the robot, false if not
    00126    **/
    00127   bool ForcedSphere::collisionCallback(void *data, dGeomID o1, dGeomID o2) {
    00128     return false; // let the standard routine do it for us
    00129   }
    00130
    00131
    00132   /**
    00133    *Returns the number of motors used by the snake.
    00134    *@return number of motors
    00135    **/
    00136   int ForcedSphere::getMotorNumber(){
    00137     return 2;
    00138   }
    00139
    00140   /**
    00141    *Returns the number of sensors used by the robot.
    00142    *@return number of sensors
    00143    **/
    00144   int ForcedSphere::getSensorNumber() {
    00145     return 3;
    00146   }
    00147
    00148
    00149   /** creates vehicle at desired position
    00150       @param pos struct Position with desired position
    00151   */
    00152   void ForcedSphere::create(const osg::Matrix& pose){
    00153     if (created) {
    00154       destroy();
    00155     }
    00156
    00157     object[0] = new Sphere(radius);
    00158     object[0]->init(odeHandle, radius, osgHandle);
    00159     object[0]->setPose(pose);
    00160   }
    00161
    00162
    00163   /** destroys vehicle and space
    00164    */
    00165   void ForcedSphere::destroy(){
    00166     if (created){
    00167       for (int i=0; i<1; i++){
    00168         if(object[i]) delete object[i];
    00169       }
    00170     }
    00171     created=false;
    00172   }
    00173
    00174 }
    00175