Robot Simulator of the Robotics Group for Self-Organization of Control: boxpile.h Source File

# boxpile.h

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005-2011 LpzRobots development team                    *
    00003  *    Georg Martius  <georg dot martius at web dot de>                     *
    00004  *    Frank Guettler <guettler at informatik dot uni-leipzig dot de        *
    00005  *    Frank Hesse    <frank at nld dot ds dot mpg dot de>                  *
    00006  *    Ralf Der       <ralfder at mis dot mpg dot de>                       *
    00007  *                                                                         *
    00008  *   This program is free software; you can redistribute it and/or modify  *
    00009  *   it under the terms of the GNU General Public License as published by  *
    00010  *   the Free Software Foundation; either version 2 of the License, or     *
    00011  *   (at your option) any later version.                                   *
    00012  *                                                                         *
    00013  *   This program is distributed in the hope that it will be useful,       *
    00014  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00015  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00016  *   GNU General Public License for more details.                          *
    00017  *                                                                         *
    00018  *   You should have received a copy of the GNU General Public License     *
    00019  *   along with this program; if not, write to the                         *
    00020  *   Free Software Foundation, Inc.,                                       *
    00021  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00022  *                                                                         *
    00023  ***************************************************************************/
    00024 #ifndef __BOXPILE_H
    00025 #define __BOXPILE_H
    00026
    00027 #include <stdio.h>
    00028 #include <cmath>
    00029
    00030 #include "primitive.h"
    00031 #include "joint.h"
    00032 #include "abstractobstacle.h"
    00033
    00034 namespace lpzrobots {
    00035
    00036 /**
    00037  *  Boxpile
    00038  */
    00039 class Boxpile : public AbstractObstacle {
    00040   Pos dimension;
    00041   int num;
    00042   int seed;
    00043   Pos boxsizemean;
    00044   Pos boxsizevar;
    00045   RandGen randGen;
    00046
    00047 public:
    00048   /**
    00049    * Constructor
    00050    * a random boxpile is fixed to the world
    00051    * @param dimension size of pile (height is always 1, use boxsize and boxvar)
    00052    * @param num number of boxes
    00053    * @param seed seed for random generator (it is fixed to generate deterministic piles)
    00054    * @param boxsizemean mean size of boxes
    00055    * @param boxsizevar  variance of boxes sizes
    00056    */
    00057   Boxpile(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00058           const osg::Vec3& dimension = osg::Vec3(5.0, 5.0, 1.0),
    00059           int num = 30, int seed = 1,
    00060           const osg::Vec3& boxsizemean = osg::Vec3(1.0, 1.0, 0.2),
    00061           const osg::Vec3& boxsizevar = osg::Vec3(0.5, 0.5, 0.1) )
    00062     : AbstractObstacle::AbstractObstacle(odeHandle, osgHandle), dimension(dimension),
    00063       num(num), boxsizemean(boxsizemean), boxsizevar(boxsizevar)
    00064   {
    00065     setTexture("Images/wood_sw.jpg");
    00066     randGen.init(seed);
    00067     this->dimension.z()=1;
    00068     obstacle_exists=false;
    00069   };
    00070
    00071
    00072   virtual void setPose(const osg::Matrix& pose){
    00073     this->pose = pose;
    00074     if (!obstacle_exists) {
    00075       create();
    00076     }
    00077   };
    00078
    00079   virtual Primitive* getMainPrimitive() const {
    00080     if(!obst.empty()) return obst[0];
    00081     else return 0;
    00082   }
    00083
    00084 protected:
    00085   virtual void create(){
    00086     OdeHandle oh(odeHandle);
    00087     oh.createNewSimpleSpace(odeHandle.space,true);
    00088     double size=dimension.length();
    00089     for(int i=0; i< num; i++){
    00090       Box* b;
    00091       Pos rand(randGen.rand()-0.5,randGen.rand()-0.5,randGen.rand()-0.5);
    00092       Pos s = boxsizemean + ((rand*2) & boxsizevar); // & component wise mult
    00093       Pos pos = (dimension & Pos(randGen.rand()-0.5,randGen.rand()-0.5,0));
    00094       double angle = randGen.rand()*M_PI;
    00095
    00096       // make sure box has positive dimensions
    00097       s.x()=fabs(s.x());
    00098       s.y()=fabs(s.y());
    00099       s.z()=fabs(s.z());
    00100       // make pile round
    00101       s.z()*=fabs((size-pos.length())/size); // linear ramping of heights
    00102
    00103       pos.z() = s.z()/2.0;
    00104       b = new Box(s);
    00105       b->setTextures(getTextures(i));
    00106       b->init(oh, 0, osgHandle, Primitive::Geom | Primitive::Draw);
    00107
    00108       b->setPose(ROTM(angle, 0,0,1)*TRANSM(pos) * pose);
    00109       obst.push_back(b);
    00110     }
    00111     obstacle_exists=true;
    00112   };
    00113
    00114 };
    00115
    00116 }
    00117
    00118 #endif
    00119