Robotsystem from Robot Group Leipzig: irsensor.cpp Source File

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

- File List
- File Members

# irsensor.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: irsensor.cpp,v $
    00023  *   Revision 1.4.4.5  2006/01/26 18:37:20  martius
    00024  *   *** empty log message ***
    00025  *
    00026  *   Revision 1.4.4.4  2005/12/14 15:37:19  martius
    00027  *   sensors are working with osg
    00028  *
    00029  *   Revision 1.4.4.3  2005/12/14 12:43:07  martius
    00030  *   moved to osg
    00031  *
    00032  *   Revision 1.4.4.2  2005/12/13 18:11:53  martius
    00033  *   sensors ported, but not yet finished
    00034  *
    00035  *   Revision 1.4.4.1  2005/11/14 17:37:20  martius
    00036  *   moved to selforg
    00037  *
    00038  *   Revision 1.4  2005/11/08 11:34:31  martius
    00039  *   geom is only enabled in sense function
    00040  *   there is no external collision detection anymore
    00041  *
    00042  *   Revision 1.3  2005/09/27 13:59:26  martius
    00043  *   ir sensors are working now
    00044  *
    00045  *   Revision 1.2  2005/09/27 11:03:33  fhesse
    00046  *   sensorbank added
    00047  *
    00048  *   Revision 1.1  2005/09/22 12:56:47  martius
    00049  *   ray based sensors
    00050  *
    00051  *                                                                         *
    00052  ***************************************************************************/
    00053 #include <ode/ode.h>
    00054 #include <math.h>
    00055 #include <assert.h>
    00056 #include <selforg/position.h>
    00057 #include <osg/Matrix>
    00058 #include <osg/Vec3>
    00059
    00060 #include "primitive.h"
    00061 #include "osgprimitive.h"
    00062 #include "irsensor.h"
    00063
    00064 namespace lpzrobots {
    00065
    00066 IRSensor::IRSensor(double exponent/* = 1*/){
    00067   value = 0;
    00068   len=0;
    00069   ray=0;
    00070   this->exponent = exponent;
    00071   initialised = false;
    00072   sensorBody = 0;
    00073   sensorRay  = 0;
    00074 }
    00075
    00076 IRSensor::~IRSensor(){
    00077   dGeomDestroy(transform);
    00078 }
    00079
    00080 void IRSensor::init(const OdeHandle& odeHandle,
    00081                     const OsgHandle& osgHandle,
    00082                     Primitive* body,
    00083                     const osg::Matrix pose, double range,
    00084                     rayDrawMode drawMode){
    00085   this->range = range;
    00086   this->osgHandle = osgHandle;
    00087   value = 0;
    00088   len   = range;
    00089
    00090   transform = dCreateGeomTransform (odeHandle.space);
    00091   dGeomTransformSetInfo(transform, 0);
    00092   dGeomTransformSetCleanup(transform, 1); // destroy ray geom of transform is created
    00093
    00094   ray = dCreateRay ( 0, range);
    00095   osg::Vec3 p = pose.getTrans();
    00096   dGeomSetPosition (ray, p.x(), p.y(), p.z());
    00097   dMatrix3 rot;
    00098   odeRotation(pose, rot);
    00099   dGeomSetRotation(ray, rot);
    00100
    00101   dGeomTransformSetGeom(transform, ray);
    00102   dGeomSetBody ( transform, body->getBody() );
    00103   // disable transform geom,
    00104   //  so that it is not treated by normal collision detection.
    00105   dGeomDisable (transform);
    00106
    00107   switch(drawMode){
    00108   case drawAll:
    00109   case drawRay:
    00110     sensorRay = new OSGBox(0.002, 0.002, len);
    00111     sensorRay->init(osgHandle);
    00112     if( drawMode != drawAll) break;
    00113   case drawSensor:
    00114     sensorBody = new OSGCylinder(0.05, 0.01);
    00115     sensorBody->init(osgHandle);
    00116     break;
    00117   default:
    00118     break;
    00119   }
    00120   update();
    00121   initialised = true;
    00122 };
    00123
    00124 void IRSensor::reset(){
    00125   value = 0;
    00126   len   = range;
    00127 }
    00128
    00129 bool IRSensor::sense(dGeomID object){
    00130   assert(initialised);
    00131   int n;
    00132   bool rv = false;
    00133   dContact contact;
    00134   dGeomEnable (transform); // enable transform geom of this ray
    00135   n = dCollide (object, transform, 1, &contact.geom, sizeof(dContact));
    00136   if(n) {
    00137     //     printf("ray: %x\n",ray);
    00138     //     printf("coll between: %x  %x\n",contact.geom.g1,contact.geom.g2);
    00139     len = contact.geom.depth;
    00140     value = characteritic(len);
    00141     //    printf("len= %f, value: %f, \n",len, value);
    00142     rv = true;
    00143   }
    00144   dGeomDisable (transform);// disable transform geom, so that it is not treated by normal collision detection.
    00145   return rv;
    00146 }
    00147
    00148
    00149 double IRSensor::get(){
    00150   return value;
    00151 }
    00152
    00153 void IRSensor::update(){
    00154   const dReal* pos = dGeomGetPosition (transform);
    00155   const dReal* R = dGeomGetRotation (transform);
    00156   const dReal *pos2 = dGeomGetPosition (ray); // local position
    00157   const dReal *R2 = dGeomGetRotation (ray);   // local rotation
    00158   dVector3 actual_pos;
    00159   dMatrix3 actual_R;
    00160   dMULTIPLY0_331 (actual_pos,R,pos2);
    00161   actual_pos[0] += pos[0];
    00162   actual_pos[1] += pos[1];
    00163   actual_pos[2] += pos[2];
    00164   dMULTIPLY0_333 (actual_R,R,R2);
    00165
    00166   if(sensorRay)  {
    00167     delete sensorRay;
    00168     sensorRay = new OSGBox(0.002, 0.002, len);
    00169     sensorRay->init(osgHandle);
    00170     sensorRay->setMatrix(osg::Matrix::translate(osg::Vec3(0,0,len/2.0)) * osgPose(actual_pos,actual_R));
    00171     sensorRay->setColor(Color(value*2.0, 0.0, 0.0));
    00172   }
    00173   if(sensorBody) {
    00174     delete sensorBody;
    00175     sensorBody = new OSGCylinder(0.05, 0.01);
    00176     sensorBody->init(osgHandle);
    00177     sensorBody->setMatrix(osgPose(actual_pos,actual_R));
    00178     sensorBody->setColor(Color(value*2.0, 0.0, 0.0));
    00179   }
    00180 }
    00181
    00182
    00183 dGeomID IRSensor::getGeomID(){
    00184   return ray;
    00185 }
    00186
    00187 double IRSensor::characteritic(double len){
    00188   double v = (range - len)/range;
    00189   return v < 0 ? 0 : pow(v, exponent);
    00190 }
    00191
    00192 }