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

# hexapod.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  *    Guillaume de Chambrier <s0672742 at sms dot ed dot ac dot uk>        *
    00008  *                                                                         *
    00009  *   This program is free software; you can redistribute it and/or modify  *
    00010  *   it under the terms of the GNU General Public License as published by  *
    00011  *   the Free Software Foundation; either version 2 of the License, or     *
    00012  *   (at your option) any later version.                                   *
    00013  *                                                                         *
    00014  *   This program is distributed in the hope that it will be useful,       *
    00015  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00016  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00017  *   GNU General Public License for more details.                          *
    00018  *                                                                         *
    00019  *   You should have received a copy of the GNU General Public License     *
    00020  *   along with this program; if not, write to the                         *
    00021  *   Free Software Foundation, Inc.,                                       *
    00022  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00023  *                                                                         *
    00024  ***************************************************************************/
    00025
    00026
    00027 #ifndef __HEXAPOD_H
    00028 #define __HEXAPOD_H
    00029
    00030 #include <selforg/inspectable.h>
    00031
    00032 #include <ode_robots/oderobot.h>
    00033 #include <ode_robots/raysensorbank.h>
    00034
    00035 namespace lpzrobots {
    00036
    00037   class Primitive;
    00038   class Joint;
    00039   class OneAxisServo;
    00040   class TwoAxisServo;
    00041
    00042
    00043   struct HexapodConf {
    00044     double size;       ///< scaling factor for robot (diameter of body)
    00045     double legLength;  ///< length of the legs in units of size
    00046     int    legNumber;  ///<  number of snake elements
    00047     double width;      ///< body with in units of size
    00048     double height;     ///< body with in units of size
    00049     double mass;       ///< chassis mass
    00050     double relLegmass; ///< relative overall leg mass
    00051     double percentageBodyMass;
    00052
    00053     double coxaPower; ///< maximal force for at hip joint motors
    00054     double coxaJointLimitV; ///< angle range for vertical direction of legs
    00055     double coxaJointLimitH; ///< angle range for horizontal direction of legs
    00056     double coxaDamping;     ///< damping of hip joint servos
    00057     double coxaSpeed;       ///< speed of the hip servo
    00058
    00059     bool useTebiaJoints;    /// whether to use joints at the knees
    00060     double tebiaPower;       ///< spring strength in the knees
    00061     double tebiaJointLimit;  ///< angle range for knees
    00062     double tebiaOffset;      ///< angle offset for knees
    00063     double tebiaDamping; ///< damping in the knees
    00064
    00065     double legSpreading;   ///< angle at which legs are spread to the front and back
    00066
    00067     bool tarsus;                  ///< whether to use a tarsus or not
    00068     int numTarsusSections; ///< the number of sections in the tarsus
    00069     bool useTarsusJoints; ///< whether to use joints on the tarsus
    00070     bool useBigBox;       ///< whether to use a big invisible box on the back of the robot
    00071
    00072     double T; ///< T is the for the time for calculating the cost of transport over time
    00073     double *v;
    00074
    00075     bool ignoreInternalCollisions;
    00076
    00077     bool useContactSensors;
    00078     matrix::Matrix m;
    00079     int *legContacts;
    00080     double irSensors;
    00081     bool irFront;
    00082     bool irBack;
    00083     bool irLeft;
    00084     bool irRight;
    00085     double irRangeFront;
    00086     double irRangeBack;
    00087     double irRangeLeft;
    00088     double irRangeRight;
    00089   };
    00090
    00091   struct Leg {
    00092     int legID;
    00093     dGeomID geomid;
    00094     dBodyID bodyID;
    00095
    00096     dJointID joint;
    00097   };
    00098
    00099
    00100   class Hexapod : public OdeRobot, public Inspectable {
    00101   public:
    00102
    00103     /**
    00104      * constructor of VierBeiner robot
    00105      * @param odeHandle data structure for accessing ODE
    00106      * @param osgHandle ata structure for accessing OSG
    00107      * @param conf configuration object
    00108      */
    00109     Hexapod(const OdeHandle& odeHandle, const OsgHandle& osgHandle, const HexapodConf& conf,
    00110                const std::string& name);
    00111
    00112     virtual ~Hexapod(){ destroy(); };
    00113
    00114     static HexapodConf getDefaultConf(){
    00115       HexapodConf c;
    00116       c.size       = 1;
    00117       c.width      = 1.0/3.0; //1.0/1.5
    00118       c.height     = 1.0/8.0; //1.0/4.0
    00119       c.legNumber  = 6;
    00120       c.legLength  = 0.6;
    00121       c.percentageBodyMass = 0.7;
    00122       c.mass       = 1.0;
    00123       c.v = new double[1];
    00124       c.relLegmass = 1;
    00125       c.coxaPower  = 1;
    00126       c.coxaJointLimitV = M_PI/8.0;  ///< angle range for vertical direction of legs
    00127       c.coxaJointLimitH = M_PI/4.0;
    00128       c.coxaDamping = 0.0; // Georg: no damping required for new servos
    00129       c.coxaSpeed   = 30; // The speed calculates how it works
    00130       c.T = 1.0;
    00131
    00132       c.legSpreading=M_PI/12.0;
    00133
    00134       c.useTebiaJoints = true;
    00135       c.tebiaPower = 1.2;
    00136       c.tebiaJointLimit = M_PI/4; // +- 45 degree
    00137       c.tebiaDamping = 0.01; // Georg: the damping reduces the swinging of the system
    00138       c.tebiaOffset = 0.0;
    00139
    00140       c.tarsus = true;
    00141       c.numTarsusSections = 2;
    00142       c.useTarsusJoints=true;
    00143       c.useBigBox=true;
    00144
    00145       c.ignoreInternalCollisions=true;
    00146
    00147       c.useContactSensors=false;
    00148       c.legContacts = new int[6];
    00149       c.irSensors=false;
    00150       c.irFront=false;
    00151       c.irBack=false;
    00152       c.irLeft=false;
    00153       c.irRight=false;
    00154       c.irRangeFront=3;
    00155       c.irRangeBack=2;
    00156       c.irRangeLeft=2;
    00157       c.irRangeRight=2;
    00158       //      c.elasticity = 10;
    00159       return c;
    00160     }
    00161
    00162     /**
    00163      * updates the OSG nodes of the vehicle
    00164      */
    00165     virtual void update();
    00166
    00167
    00168     /** sets the pose of the vehicle
    00169         @param pose desired pose matrix
    00170     */
    00171     virtual void place(const osg::Matrix& pose);
    00172
    00173     /** returns actual sensorvalues
    00174         @param sensors sensors scaled to [-1,1]
    00175         @param sensornumbHexapod::getDefaultConf()er length of the sensor array
    00176         @return number of actually written sensors
    00177     */
    00178     virtual int getSensors(sensor* sensors, int sensornumber);
    00179
    00180     /** sets actual motorcommands
    00181         @param motors motors scaled to [-1,1]
    00182         @param motornumber length of the motor array
    00183     */
    00184     virtual void setMotors(const motor* motors, int motornumber);
    00185
    00186     /** returns number of sensors
    00187      */
    00188     virtual int getSensorNumber();
    00189
    00190     /** returns number of motors
    00191      */
    00192     virtual int getMotorNumber();
    00193     /** checks for internal collisions and treats them.
    00194      *  In case of a treatment return true (collision will be ignored by other objects
    00195      *  and the default routine)  else false (collision is passed to other objects and
    00196      *  (if not treated) to the default routine).
    00197      */
    00198     virtual bool collisionCallback(void *data, dGeomID o1, dGeomID o2);
    00199
    00200     /** this function is called in each timestep. It should perform robot-internal checks,
    00201         like space-internal collision detection, sensor resets/update etc.
    00202         @param globalData structure that contains global data from the simulation environment
    00203     */
    00204     virtual void doInternalStuff(GlobalData& globalData);
    00205
    00206
    00207    // virtual void Hexapod::updateLegTouch(int);
    00208
    00209     /**
    00210      * calculates the total energy consumption of all servos.
    00211      */
    00212     double round(double,int);
    00213
    00214     virtual double energyConsumption();
    00215
    00216     virtual double energyConsumpThroughtHeatLoss(const dReal *torques);
    00217
    00218     virtual double outwardMechanicalPower(const dReal *torques,const dReal *angularV);
    00219
    00220     virtual double costOfTransport(double E, double W, double V, double T);
    00221
    00222     virtual double getMassOfRobot();
    00223
    00224     virtual double *getPosition(){
    00225         return position;
    00226     }
    00227
    00228     /******** CONFIGURABLE ***********/
    00229     virtual void notifyOnChange(const paramkey& key);
    00230
    00231     virtual void resetMotorPower(double power);
    00232
    00233     virtual double getPower();
    00234
    00235     /** the main object of the robot, which is used for position and speed tracking */
    00236     virtual Primitive* getMainPrimitive() const { return objects[0]; }
    00237   protected:
    00238
    00239     /** creates vehicle at desired pose
    00240         @param pose 4x4 pose matrix
    00241     */
    00242     virtual void create(const osg::Matrix& pose);
    00243
    00244     /** destroys vehicle and space
    00245      */
    00246     virtual void destroy();
    00247
    00248 public:
    00249     HexapodConf conf;
    00250     double legmass;    // leg mass
    00251     int countt;
    00252     bool created;      // true if robot was created
    00253     RaySensorBank irSensorBank; // a collection of ir sensors
    00254
    00255   public:
    00256     double costOfTran;
    00257     double* energyOneStep; ///< energy consumption for one time step
    00258     double E_t; ///< energy consumption over a period t;
    00259     bool recordGait;
    00260     double *heights;
    00261     double *angles;
    00262   private:
    00263     double hcorrection;
    00264     bool *dones;
    00265     bool check;
    00266     double t;
    00267     FILE* f;
    00268     double timeCounter;
    00269     double *position;
    00270     std::vector<dReal*> pos_record;
    00271     dMass *massOfobject;
    00272     bool getPos1;
    00273     double distance;
    00274     double time;
    00275     double speed;
    00276
    00277     std::vector<Leg> legContact;
    00278     Leg* legContactArray;
    00279     std::vector<dGeomID> footIDs;
    00280   protected:
    00281     // some objects explicitly needed for ignored collision pairs
    00282     Primitive *trunk, *irbox, *stabaliserTransform, *bigboxtransform, *headtrans;
    00283     std::vector<Primitive*> legs;
    00284     std::vector<Primitive*> thorax;
    00285     std::vector<Pos> thoraxPos;
    00286
    00287
    00288     std::vector <TwoAxisServo*> hipservos; // motor
    00289     std::vector <OneAxisServo*> tebiasprings;
    00290     std::vector <OneAxisServo*> tarsussprings;
    00291     std::vector <OneAxisServo*> whiskersprings;
    00292
    00293   };
    00294
    00295 }
    00296
    00297 #endif