Robot Simulator of the Robotics Group for Self-Organization of Control: nimm4.cpp

- Main Page
- Namespaces
- Classes
- Files
- Directories
- Related Pages
- Examples

- Search for |

# nimm4.cpp

Simple example for the creation of a simulated robot (lpzrobots/ode_robots/robots/nimm4.cpp).

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius (at) informatik.uni-leipzig.de                                    *
    00004  *    fhesse (at) informatik.uni-leipzig.de                                     *
    00005  *    der (at) 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: nimm4.cpp,v $
    00023  *   Revision 1.16  2009/05/11 17:03:07  martius
    00024  *   minor substance change
    00025  *
    00026  *   Revision 1.15  2009/03/13 09:19:53  martius
    00027  *   changed texture handling in osgprimitive
    00028  *   new OsgBoxTex that supports custom texture repeats and so on
    00029  *   Box uses osgBoxTex now. We also need osgSphereTex and so on.
    00030  *   setTexture has to be called before init() of the primitive
    00031  *
    00032  *   Revision 1.14  2008/05/14 15:11:57  martius
    00033  *   typo
    00034  *
    00035  *   Revision 1.13  2008/05/07 16:45:52  martius
    00036  *   code cosmetics and documentation
    00037  *
    00038  *   Revision 1.12  2008/05/07 11:03:48  martius
    00039  *   code cosmetics
    00040  *
    00041  *   Revision 1.11  2008/04/23 07:17:16  martius
    00042  *   makefiles cleaned
    00043  *   new also true realtime factor displayed,
    00044  *    warning if out of sync
    00045  *   drawinterval in full speed is 10 frames, independent of the speed
    00046  *
    00047  *   Revision 1.10  2007/11/07 13:21:16  martius
    00048  *   doInternal stuff changed signature
    00049  *
    00050  *   Revision 1.9  2007/08/28 14:13:09  martius
    00051  *   typo
    00052  *
    00053  *   Revision 1.8  2006/07/14 12:23:40  martius
    00054  *   selforg becomes HEAD
    00055  *
    00056  *   Revision 1.7.4.17  2006/06/29 16:39:55  robot3
    00057  *   -you can now see bounding shapes if you type ./start -drawboundings
    00058  *   -includes cleared up
    00059  *   -abstractobstacle and abstractground have now .cpp-files
    00060  *
    00061  *   Revision 1.7.4.16  2006/06/25 17:00:32  martius
    00062  *   Id
    00063  *
    00064  *   Revision 1.7.4.15  2006/06/25 16:57:14  martius
    00065  *   abstractrobot is configureable
    00066  *   name and revision
    00067  *
    00068  *   Revision 1.7.4.14  2006/04/04 17:03:21  fhesse
    00069  *   docu added
    00070  *
    00071  *   Revision 1.7.4.13  2006/04/04 14:13:24  fhesse
    00072  *   documentation improved
    00073  *
    00074  *   Revision 1.7.4.12  2006/03/31 11:11:38  fhesse
    00075  *   minor changes in docu
    00076  *
    00077  *   Revision 1.7.4.11  2006/02/01 18:33:40  martius
    00078  *   use Axis type for Joint axis. very important, since otherwise Vec3 * pose is not the right direction vector anymore
    00079  *
    00080  *   Revision 1.7.4.10  2005/12/29 16:47:40  martius
    00081  *   joint has getPosition
    00082  *
    00083  *   Revision 1.7.4.9  2005/12/15 17:04:08  martius
    00084  *   Primitives are not longer inherited from OSGPrimitive, moreover
    00085  *   they aggregate them.
    00086  *   Joint have better getter and setter
    00087  *
    00088  *   Revision 1.7.4.8  2005/12/14 15:37:09  martius
    00089  *   robots are working with osg
    00090  *
    00091  *   Revision 1.7.4.7  2005/12/13 18:11:39  martius
    00092  *   still trying to port robots
    00093  *
    00094  *   Revision 1.7.4.6  2005/12/13 12:32:09  martius
    00095  *   nonvisual joints
    00096  *
    00097  *   Revision 1.7.4.5  2005/12/12 23:41:30  martius
    00098  *   added Joint wrapper
    00099  *
    00100  *   Revision 1.7.4.4  2005/12/11 23:35:08  martius
    00101  *   *** empty log message ***
    00102  *
    00103  *   Revision 1.7.4.3  2005/12/06 10:13:25  martius
    00104  *   openscenegraph integration started
    00105  *
    00106  *   Revision 1.7.4.2  2005/11/15 12:29:26  martius
    00107  *   new selforg structure and OdeAgent, OdeRobot ...
    00108  *
    00109  *   Revision 1.7.4.1  2005/11/14 17:37:17  martius
    00110  *   moved to selforg
    00111  *
    00112  *   Revision 1.7  2005/11/09 13:24:42  martius
    00113  *   added GPL
    00114  *
    00115  ***************************************************************************/
    00116 #include <assert.h>
    00117 #include <ode/ode.h>
    00118
    00119 // include primitives (box, spheres, cylinders ...)
    00120 #include "primitive.h"
    00121 #include "osgprimitive.h"
    00122
    00123 // include joints
    00124 #include "joint.h"
    00125
    00126 // include header file
    00127 #include "nimm4.h"
    00128
    00129 using namespace osg;
    00130
    00131
    00132 namespace lpzrobots {
    00133
    00134   // constructor:
    00135   // - give handle for ODE and OSG stuff
    00136   // - size of robot, maximal used force and speed factor are adjustable
    00137   // - sphereWheels switches between spheres or wheels as wheels
    00138   //   (wheels are only drawn, collision handling is always with spheres)
    00139   Nimm4::Nimm4(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00140                const std::string& name,
    00141                double size/*=1.0*/, double force /*=3*/, double speed/*=15*/,
    00142                bool sphereWheels /*=true*/)
    00143     : // calling OdeRobots construtor with name of the actual robot
    00144       OdeRobot(odeHandle, osgHandle, name, "$Id: nimm4.cpp,v 1.16 2009/05/11 17:03:07 martius Exp $")
    00145   {
    00146     // robot is not created till now
    00147     created=false;
    00148
    00149     // choose color (here the color of the "Nimm Zwei" candy is used,
    00150     // where the name of the Nimm2 and Nimm4 robots comes from ;-)
    00151     this->osgHandle.color = Color(2, 156/255.0, 0, 1.0f);
    00152
    00153     // maximal used force is calculated from the force factor and size given to the constructor
    00154     max_force   = force*size*size;
    00155
    00156     // speed and type of wheels are set
    00157     this->speed = speed;
    00158     this->sphereWheels = sphereWheels;
    00159
    00160     height=size;
    00161     length=size/2.5; // length of body
    00162     width=size/2;  // diameter of body
    00163     radius=size/6; // wheel radius
    00164     wheelthickness=size/20; // thickness of the wheels (if wheels used, no spheres)
    00165     cmass=8*size;  // mass of the body
    00166     wmass=size;    // mass of the wheels
    00167     sensorno=4;    // number of sensors
    00168     motorno=4;     // number of motors
    00169     segmentsno=5;  // number of segments of the robot
    00170
    00171     wheelsubstance.toRubber(50);
    00172   };
    00173
    00174
    00175   /** sets actual motorcommands
    00176       @param motors motors scaled to [-1,1]
    00177       @param motornumber length of the motor array
    00178   */
    00179   void Nimm4::setMotors(const motor* motors, int motornumber){
    00180     assert(created); // robot must exist
    00181     // the number of controlled motors is minimum of
    00182     // "number of motorcommands" (motornumber) and
    00183     // "number of motors inside the robot" (motorno)
    00184     int len = (motornumber < motorno)? motornumber : motorno;
    00185
    00186     // for each motor the motorcommand (between -1 and 1) multiplied with speed
    00187     // is set and the maximal force to realize this command are set
    00188     for (int i=0; i<len; i++){
    00189       joint[i]->setParam(dParamVel2, motors[i]*speed);
    00190       joint[i]->setParam(dParamFMax2, max_force);
    00191     }
    00192   };
    00193
    00194   /** returns actual sensorvalues
    00195       @param sensors sensors scaled to [-1,1] (more or less)
    00196       @param sensornumber length of the sensor array
    00197       @return number of actually written sensors
    00198   */
    00199   int Nimm4::getSensors(sensor* sensors, int sensornumber){
    00200     assert(created); // robot must exist
    00201
    00202     // the number of sensors to read is the minimum of
    00203     // "number of sensors requested" (sensornumber) and
    00204     // "number of sensors inside the robot" (sensorno)
    00205     int len = (sensornumber < sensorno)? sensornumber : sensorno;
    00206
    00207     // for each sensor the anglerate of the joint is red and scaled with 1/speed
    00208     for (int i=0; i<len; i++){
    00209       sensors[i]=joint[i]->getPosition2Rate();
    00210       sensors[i]/=speed;  //scaling
    00211     }
    00212     // the number of red sensors is returned
    00213     return len;
    00214   };
    00215
    00216
    00217   void Nimm4::place(const osg::Matrix& pose){
    00218     // the position of the robot is the center of the body (without wheels)
    00219     // to set the vehicle on the ground when the z component of the position is 0
    00220     // width*0.6 is added (without this the wheels and half of the robot will be in the ground)
    00221     Matrix p2;
    00222     p2 = pose * Matrix::translate(Vec3(0, 0, width*0.6));
    00223     create(p2);
    00224   };
    00225
    00226
    00227   /**
    00228    * updates the osg notes
    00229    */
    00230   void Nimm4::update(){
    00231     assert(created); // robot must exist
    00232
    00233     for (int i=0; i<segmentsno; i++) { // update objects
    00234       object[i]->update();
    00235     }
    00236     for (int i=0; i < 4; i++) { // update joints
    00237       joint[i]->update();
    00238     }
    00239
    00240   };
    00241
    00242   /** this function is called in each timestep. It should perform robot-internal checks,
    00243       like space-internal collision detection, sensor resets/update etc.
    00244       @param global structure that contains global data from the simulation environment
    00245   */
    00246   void Nimm4::doInternalStuff(GlobalData& global){}
    00247
    00248   /** creates vehicle at desired pose
    00249       @param pose matrix with desired position and orientation
    00250   */
    00251   void Nimm4::create( const osg::Matrix& pose ){
    00252     if (created) {  // if robot exists destroy it
    00253       destroy();
    00254     }
    00255     // create car space
    00256     odeHandle.createNewSimpleSpace(parentspace, true);
    00257
    00258     OdeHandle wheelHandle(odeHandle);
    00259     // make the material of the wheels a hard rubber
    00260     wheelHandle.substance = wheelsubstance;
    00261     // create cylinder for main body
    00262     // initialize it with ode-, osghandle and mass
    00263     // rotate and place body (here by 90° around the y-axis)
    00264     // use texture 'wood' for capsule
    00265     // put it into object[0]
    00266     Capsule* cap = new Capsule(width/2, length);
    00267     cap->getOSGPrimitive()->setTexture("Images/wood.rgb");
    00268     cap->init(odeHandle, cmass, osgHandle);
    00269     cap->setPose(Matrix::rotate(M_PI/2, 0, 1, 0) * pose);
    00270     object[0]=cap;
    00271
    00272     // create wheels
    00273     for (int i=1; i<5; i++) {
    00274       // create sphere with radius
    00275       // and initialize it with odehandle, osghandle and mass
    00276       // calculate position of wheels(must be at desired positions relative to the body)
    00277       // rotate and place body (here by 90Deg around the x-axis)
    00278       // set texture for wheels
    00279       Sphere* sph = new Sphere(radius);
    00280       sph->getOSGPrimitive()->setTexture("Images/wood.rgb");
    00281       sph->init(wheelHandle, wmass, osgHandle.changeColor(Color(0.8,0.8,0.8)));
    00282       Vec3 wpos = Vec3( ((i-1)/2==0?-1:1)*length/2.0,
    00283                         ((i-1)%2==0?-1:1)*(width*0.5+wheelthickness),
    00284                         -width*0.6+radius );
    00285       sph->setPose(Matrix::rotate(M_PI/2, 0, 0, 1) * Matrix::translate(wpos) * pose);
    00286       object[i]=sph;
    00287     }
    00288
    00289     // generate 4 joints to connect the wheels to the body
    00290     for (int i=0; i<4; i++) {
    00291       Pos anchor(dBodyGetPosition (object[i+1]->getBody()));
    00292       joint[i] = new Hinge2Joint(object[0], object[i+1], anchor, Axis(0,0,1)*pose, Axis(0,1,0)*pose);
    00293       joint[i]->init(odeHandle, osgHandle, true, 2.01 * radius);
    00294     }
    00295     for (int i=0; i<4; i++) {
    00296       // set stops to make sure wheels always stay in alignment
    00297       joint[i]->setParam(dParamLoStop, 0);
    00298       joint[i]->setParam(dParamHiStop, 0);
    00299     }
    00300
    00301     created=true; // robot is created
    00302   };
    00303
    00304
    00305   /** destroys vehicle and space
    00306    */
    00307   void Nimm4::destroy(){
    00308     if (created){
    00309       for (int i=0; i<4; i++){
    00310         if(joint[i]) delete joint[i]; // destroy bodies and geoms
    00311       }
    00312       for (int i=0; i<segmentsno; i++){
    00313         if(object[i]) delete object[i]; // destroy bodies and geoms
    00314       }
    00315       odeHandle.deleteSpace(); // destroy space
    00316     }
    00317     created=false; // robot does not exist (anymore)
    00318   }
    00319
    00320 }