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

# amosII.h

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2012 by                                                 *
    00003  *    Martin Biehl <mab@physik3.gwdg.de>                                   *
    00004  *    Guillaume de Chambrier <s0672742@sms.ed.ac.uk>                       *
    00005  *    martius@informatik.uni-leipzig.de                                    *
    00006  *    Timo Nachstedt <nachstedt@physik3.gwdg.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
    00025 #ifndef __AMOSII_H
    00026 #define __AMOSII_H
    00027
    00028 #include <ode_robots/oderobot.h>
    00029 #include <selforg/inspectable.h>
    00030 #include <ode_robots/contactsensor.h>
    00031 //-------------Add by Ren relativepositionsensor.h-------
    00032 #include <ode_robots/relativepositionsensor.h>
    00033 #include <ode_robots/amosiisensormotordefinition.h>
    00034
    00035 /**
    00036  * forward declarations
    00037  */
    00038 namespace lpzrobots {
    00039   class HingeJoint;
    00040   class IRSensor;
    00041   class Joint;
    00042   class OneAxisServo;
    00043   class Primitive;
    00044   class RaySensorBank;
    00045   class SliderJoint;
    00046   class SpeedSensor;
    00047   class Spring;
    00048   class TwoAxisServo;
    00049 }
    00050
    00051 namespace lpzrobots {
    00052
    00053   struct AmosIIConf {
    00054       /**
    00055        * @name flags
    00056        *
    00057        * Enable or disable different element and features
    00058        */
    00059       /**@{*/
    00060       /** fix the shoulder element to the trunk. */
    00061       bool useShoulder;
    00062       /** whether to use joints at the knees or fix them */
    00063       bool useTebiaJoints;
    00064       /** use spring foot */
    00065       bool useFoot;
    00066       /** use the hinge joint in the back */
    00067       bool useBack;
    00068       /**
    00069        * if true, rubber substance is used for feet instead of the substance
    00070        * used for the rest of the robot
    00071        */
    00072       bool rubberFeet;
    00073       /** decide whether you wand to use a local velocity sensors.
    00074        *  If yes it gets velocity vector in local coordinates and pass it as
    00075        *  sensorvalues */
    00076       bool useLocalVelSensor;
    00077       /** Use binary leg contact sensors. If false, a force sensor is used. */
    00078       bool legContactSensorIsBinary;
    00079       /**@}*/
    00080
    00081       /** scaling factor for robot (length of body) */
    00082       double size;
    00083       /** trunk width */
    00084       double width;
    00085       /** trunk height */
    00086       double height;
    00087       /** length of the front of the body (if back joint is used) */
    00088       double frontLength;
    00089       /** radius of a wheel */
    00090       double wheel_radius;
    00091       /** width of a wheel */
    00092       double wheel_width;
    00093       /** mass of a wheel */
    00094       double wheel_mass;
    00095
    00096       /** trunk mass */
    00097       double trunkMass;
    00098       /** mass of the front part of the robot (if backboine joint is used) */
    00099       double frontMass;
    00100       /** mass of the shoulders (if used) */
    00101       double shoulderMass;
    00102       /** mass of the coxa limbs */
    00103       double coxaMass;
    00104       /** mass of the second limbs */
    00105       double secondMass;
    00106       /** mass of the tebia limbs */
    00107       double tebiaMass;
    00108       /** mass of the feet */
    00109       double footMass;
    00110
    00111       /** fix legs to trunk at this distance from bottom of trunk */
    00112       double shoulderHeight;
    00113
    00114       /** distance between hindlegs and middle legs */
    00115       double legdist1;
    00116
    00117       /** distance between middle legs and front legs */
    00118       double legdist2;
    00119
    00120       /** @name Leg extension from trunk
    00121        *
    00122        *  amosII has a fixed but adjustable joint that decides how the legs
    00123        *  extend from the trunk.here you can adjust these joints, if
    00124        *  shoulder = 0 this still influences how the legs extend (the coxa-trunk
    00125        *  connection in that case)
    00126        */
    00127       /**@{*/
    00128       /** angle in rad around vertical axis at leg-trunk fixation for front
    00129        *  legs*/
    00130       double fLegTrunkAngleV;
    00131       /** angle in rad around horizontal axis at leg-trunk fixation for front
    00132        *  legs */
    00133       double fLegTrunkAngleH;
    00134       /** rotation of front legs around own axis */
    00135       double fLegRotAngle;
    00136       /** angle in rad around vertical axis at leg-trunk fixation for middle
    00137        *  legs */
    00138       double mLegTrunkAngleV;
    00139       /** angle in rad around horizontal axis at leg-trunk fixation for middle
    00140        *  legs */
    00141       double mLegTrunkAngleH;
    00142       /** rotation of middle legs around own axis */
    00143       double mLegRotAngle;
    00144       /** angle in rad around vertical axis at leg-trunk fixation for rear legs
    00145        * */
    00146       double rLegTrunkAngleV;
    00147       /** angle in rad around horizontal axis at leg-trunk fixation for rear
    00148        *  legs */
    00149       double rLegTrunkAngleH;
    00150       /** rotation of rear legs around own axis */
    00151       double rLegRotAngle;
    00152       /**@}*/
    00153
    00154       /**
    00155        * @name leg part dimensions
    00156        *
    00157        * the lengths and radii of the individual leg parts
    00158        */
    00159       /**@{*/
    00160       /** length of the shoulder limbs (if used) */
    00161       double shoulderLength;
    00162       /** radius of the shoulder limbs (if used) */
    00163       double shoulderRadius;
    00164       /** length of the coxa limbs */
    00165       double coxaLength;
    00166       /** radius of the coxa limbs */
    00167       double coxaRadius;
    00168       /** length of the second limbs */
    00169       double secondLength;
    00170       /** radius of the second limbs */
    00171       double secondRadius;
    00172       /** length of the tebia limbs including fully extended foot spring
    00173        *  (if used) */
    00174       double tebiaLength;
    00175       /** radius of the tebia limbs */
    00176       double tebiaRadius;
    00177       /** range of the foot spring */
    00178       double footRange;
    00179       /** radius of the foot capsules, choose different from tebiaRadius */
    00180       double footRadius;
    00181       /**@}*/
    00182
    00183       /**
    00184        * @name Joint Limits
    00185        *
    00186        * set limits for each joint
    00187        */
    00188       /**{*/
    00189       /** smaller limit of the backbone joint, positive is down */
    00190       double backJointLimitD;
    00191       /** upper limit of the backbone joint, positive is down */
    00192       double backJointLimitU;
    00193       /** forward limit of the front TC joints, negative is forward
    00194        *  (zero specified by fcoxazero) */
    00195       double fcoxaJointLimitF;
    00196       /** backward limit of the front TC joints, negative is forward
    00197        *  (zero specified by fcoxaZero) */
    00198       double fcoxaJointLimitB;
    00199       /** forward limit of the middle TC joints, negative is forward
    00200        *  (zero specified by fcoxaZero) */
    00201       double mcoxaJointLimitF;
    00202       /** backward limit of the middle TC joints, negative is forward
    00203        *  (zero specified by fcoxaZero) */
    00204       double mcoxaJointLimitB;
    00205       /** forward limit of the rear TC joints, negative is forward
    00206        *  (zero specified by fcoxaZero) */
    00207       double rcoxaJointLimitF;
    00208       /** backward limit of the rear TC joints, negative is forward
    00209        *  (zero specified by fcoxaZero) */
    00210       double rcoxaJointLimitB;
    00211       /** lower limit of the CTr joints, positive is down */
    00212       double secondJointLimitD;
    00213       /** upper limit of the CTr joints, positive is down */
    00214       double secondJointLimitU;
    00215       /** lower limit of the FTi joints, positive is down */
    00216       double tebiaJointLimitD;
    00217       /** upper limit of the FTi joints, positive is down */
    00218       double tebiaJointLimitU;
    00219       /**}*/
    00220
    00221       /** preload of the foot spring */
    00222       double footSpringPreload;
    00223       /** upper limit of the foot spring = maximum value
    00224        *  (negative is downwards (spring extends)) */
    00225       double footSpringLimitU;
    00226       /** lower limit of the foot spring = minimum value
    00227        *  (negative is downwards (spring extends)) */
    00228       double footSpringLimitD;
    00229
    00230       /** maximal force of the backbone joint */
    00231       double backPower;
    00232       /** maximal force of the TC joint servos */
    00233       double coxaPower;
    00234       /** maximal force of the CTr joint servos */
    00235       double secondPower;
    00236       /** maximal force of the FTi joint servos */
    00237       double tebiaPower;
    00238       /** maximal force of the foot spring servos */
    00239       double footPower;
    00240
    00241       /** damping of the backbone joint servo */
    00242       double backDamping;
    00243       /** damping of the TC joint servos */
    00244       double coxaDamping;
    00245       /** damping of the CTr joint servo */
    00246       double secondDamping;
    00247       /** damping of the FTi joint servo */
    00248       double tebiaDamping;
    00249       /** damping of the foot spring servo */
    00250       double footDamping;
    00251
    00252       /** maximal velocity of the backbone joint servo */
    00253       double backMaxVel;
    00254       /** maximal velocity of the TC joint servo */
    00255       double coxaMaxVel;
    00256       /** maximal velocity of the CTr joint servo */
    00257       double secondMaxVel;
    00258       /** maximal velocity of the FTi joint servo */
    00259       double tebiaMaxVel;
    00260       /** maximal velocity of the foot spring servo */
    00261       double footMaxVel;
    00262
    00263       /**
    00264        * @name front ultrasonic sensors
    00265        *
    00266        * configure the front ultrasonic sensors
    00267        */
    00268       /**{*/
    00269       /** angle versus x axis */
    00270       double usAngleX;
    00271       /** angle versus y axis */
    00272       double usAngleY;
    00273       /** choose between parallel or antiparallel front ultrasonic sensors true
    00274        *  means parallel */
    00275       bool usParallel;
    00276       /** range of the front ultrasonic sensors */
    00277       double usRangeFront;
    00278       /**}*/
    00279
    00280       /** range of the infrared sensors at the legs */
    00281       double irRangeLeg;
    00282
    00283       /** path to texture for legs */
    00284       std::string texture;
    00285       /** path to texture for trunk */
    00286       std::string bodyTexture;
    00287
    00288       //-----------Add GoalSensor by Ren------------------------
    00289       std::vector<Primitive*> GoalSensor_references;
    00290       //-----------Add GoalSensor by Ren------------------------
    00291
    00292       // Internal variable storing the currently used version
    00293       int amos_version;
    00294
    00295   };
    00296
    00297   class AmosII : public OdeRobot, public Inspectable {
    00298     public:
    00299       enum LegPos {
    00300         L0, L1, L2, R0, R1, R2, LEG_POS_MAX
    00301       };
    00302       enum LegPosUsage {
    00303         LEG, WHEEL, UNUSED
    00304       };
    00305       enum LegJointType {
    00306         // thoroca-coxal joint for forward (+) and backward (-) movements
    00307         TC,
    00308         // coxa-trochanteral joint for elevation (+) and depression (-) of
    00309         // the leg
    00310         CTR,
    00311         // femur-tibia joints for extension (+) and flexion (-) of the
    00312         // tibia
    00313         FTI,
    00314         // maximum value, used for iteration
    00315         LEG_JOINT_TYPE_MAX
    00316       };
    00317       typedef AmosIIMotorNames MotorName;
    00318       typedef AmosIISensorNames SensorName;
    00319
    00320       /**
    00321        * Returns the default configuration values
    00322        */
    00323       static AmosIIConf getDefaultConf(double _scale = 1.0, bool _useShoulder = 1, bool _useFoot = 1,
    00324           bool _useBack = 0);
    00325
    00326       static AmosIIConf getAmosIIv1Conf(double _scale = 1.0, bool _useShoulder = 1, bool _useFoot = 1,
    00327           bool _useBack = 0);
    00328
    00329       static AmosIIConf getAmosIIv2Conf(double _scale = 1.0, bool _useShoulder = 1, bool _useFoot = 1,
    00330           bool _useBack = 0);
    00331
    00332       /**
    00333        * constructor
    00334        * @param odeHandle data structure for accessing ODE
    00335        * @param osgHandle ata structure for accessing OSG
    00336        * @param conf configuration object
    00337        * @param name name to display for this robot
    00338        */
    00339       AmosII(const OdeHandle& odeHandle, const OsgHandle& osgHandle, const AmosIIConf& conf = getDefaultConf(),
    00340           const std::string& name = "AmosII robot");
    00341
    00342       virtual ~AmosII();
    00343
    00344       /**
    00345        * updates the OSG nodes of the vehicle
    00346        */
    00347       virtual void update();
    00348
    00349       /**
    00350        * sets the pose of the vehicle
    00351        * @param pose desired pose matrix
    00352        */
    00353       virtual void place(const osg::Matrix& pose);
    00354
    00355       /**
    00356        * returns actual sensorvalues
    00357        * @param sensors sensor array with sensors scaled to [-1,1]
    00358        * @param sensornumber length of the sensor array
    00359        * @return number of actually written sensors
    00360        */
    00361       virtual int getSensors(sensor* sensors, int sensornumber);
    00362
    00363       /**
    00364        * sets actual motorcommands
    00365        * @param motors motors scaled to [-1,1]
    00366        * @param motornumber length of the motor array
    00367        */
    00368       virtual void setMotors(const motor* motors, int motornumber);
    00369
    00370       /**
    00371        * returns number of sensors
    00372        */
    00373       virtual int getSensorNumber();
    00374
    00375       /**
    00376        * returns number of motors
    00377        */
    00378       virtual int getMotorNumber();
    00379
    00380       /**
    00381        * this function is called in each timestep. It should perform
    00382        * robot-internal checks,like space-internal collision detection, sensor
    00383        * resets/update etc.
    00384        * @param globalData structure that contains global data from the
    00385        *                   simulation environment
    00386        */
    00387       virtual void doInternalStuff(GlobalData& globalData);
    00388
    00389       virtual double getMassOfRobot();
    00390
    00391       void setLegPosUsage(LegPos leg, LegPosUsage usage);
    00392
    00393       // Configurable Interface
    00394       virtual bool setParam(const paramkey& key, paramval val);
    00395
    00396       /**
    00397        * the main object of the robot, which is used for position and speed
    00398        * tracking
    00399        */
    00400       virtual Primitive* getMainPrimitive() const;
    00401
    00402       /**
    00403        * returns the MotorName enum value for the given joint at the given
    00404        * leg. If the value for leg or joint are not valid AMOSII_MOTOR_MAX
    00405        * is returned.
    00406        *
    00407        * @param leg leg position
    00408        * @param joint leg joint type
    00409        * @return the motor name value or AMOSII_MOTOR_MAX if parameters are
    00410        *         invalid
    00411        */
    00412       static MotorName getMotorName(LegPos leg, LegJointType joint);
    00413
    00414       /**
    00415        * Returns the joint type of the given motor. If the given motor name
    00416        * is not associated with a leg joint JOINT_TYPE_MAX is returend and a
    00417        * warning is given out.
    00418        *
    00419        * @param MotorName name of the motor
    00420        * @return joint type controlled by this motor or JOINT_TYPE_MAX if
    00421        *         MotorName is invalid
    00422        */
    00423       static LegJointType getLegJointType(MotorName);
    00424
    00425       /**
    00426        * Returns the leg of the given motor. If the given motor name is not
    00427        * associated wit a leg LEG_POS_MAX is returned and a warning is given
    00428        * out.
    00429        *
    00430        * @param MotorName name of the motor
    00431        * @return the leg on which this motor operates or LEG_POS_MAX if
    00432        *         MotorName is invalid
    00433        */
    00434       static LegPos getMotorLegPos(MotorName);
    00435
    00436     protected:
    00437
    00438       struct Leg {
    00439           Leg();
    00440           HingeJoint * tcJoint;
    00441           HingeJoint * ctrJoint;
    00442           HingeJoint * ftiJoint;
    00443           SliderJoint * footJoint;
    00444           OneAxisServo * tcServo;
    00445           OneAxisServo * ctrServo;
    00446           OneAxisServo * ftiServo;
    00447           Spring * footSpring;
    00448           Primitive * shoulder;
    00449           Primitive * coxa;
    00450           Primitive * second;
    00451           Primitive * tibia;
    00452           Primitive * foot;
    00453       };
    00454
    00455       /**
    00456        * creates vehicle at desired pose
    00457        *
    00458        * @param pose 4x4 pose matrix
    00459        */
    00460       virtual void create(const osg::Matrix& pose);
    00461
    00462       /**
    00463        * destroys vehicle and space
    00464        */
    00465       virtual void destroy();
    00466
    00467       /**
    00468        * Assign a human readable name to a motor. This name is used for the
    00469        * associated inspectable value as used e.g. in guilogger.
    00470        *
    00471        * @param motorNo index of the motor (for standard motors defined by
    00472        *        the MotorName enum)
    00473        * @param name human readable name for the motor
    00474        */
    00475       void nameMotor(const int motorNo, const char* name);
    00476
    00477       /**
    00478        * Assign a human readable name to a sensor. This name is used for the
    00479        * associated inspectable value as used e.g. in guilogger.
    00480        *
    00481        * @param motorNo index of the motor (for standard motors defined by
    00482        *        the SensorName enum)
    00483        * @param name human readable name for the sensor
    00484        */
    00485       void nameSensor(const int sensorNo, const char* name);
    00486
    00487     private:
    00488
    00489       /** typedefs */
    00490       typedef std::map<LegPos, HingeJoint*> HingeJointMap;
    00491       typedef std::map<LegPos, Leg> LegMap;
    00492       typedef std::map<LegPos, ContactSensor*> LegContactMap;
    00493       typedef std::map<MotorName, OneAxisServo*> MotorMap;
    00494       typedef std::map<LegPos, LegPosUsage> LegPosUsageMap;
    00495       typedef std::map<LegPos, IRSensor*> LegIRSensorMap;
    00496       typedef std::vector<Primitive*> PrimitiveList;
    00497       typedef std::vector<Joint*> JointList;
    00498       typedef std::vector<OneAxisServo*> ServoList;
    00499
    00500       AmosIIConf conf;
    00501       bool created; // true if robot was created
    00502
    00503       /** a collection of ir sensors **/
    00504       RaySensorBank * irSensorBank;
    00505
    00506       /** speed sensor */
    00507       SpeedSensor * speedsensor;
    00508
    00509       /**
    00510        * statistics
    00511        * theses values are updated in every timestep and have to be updated
    00512        * to make them available to the lpzrobots::Inspectable interface
    00513        */
    00514       /** position of the robot */
    00515       Position position;
    00516
    00517       /**
    00518        * defines for everey leg position the way it is used (e.g  place
    00519        * a leg or a wheel at this position)
    00520        */
    00521       LegPosUsageMap legPosUsage;
    00522
    00523       /**
    00524        * used for detection of leg contacts
    00525        */
    00526       LegContactMap legContactSensors;
    00527
    00528       // this map knows which IR sensor to find at which leg
    00529       LegIRSensorMap irLegSensors;
    00530
    00531       // holds the two front ultrasonic sensors
    00532       IRSensor * usSensorFrontLeft;
    00533       IRSensor * usSensorFrontRight;
    00534
    00535       // body in case of no hinge joint being used
    00536       Primitive *trunk;
    00537
    00538       // front part of body (when hinge joint is used)
    00539       Primitive *front;
    00540
    00541       // back part of body (when hinge joint is used)
    00542       Primitive *center;
    00543
    00544       // information on all legs
    00545       LegMap legs;
    00546
    00547       // back bone joint
    00548       OneAxisServo * backboneServo;
    00549
    00550       // all the objects
    00551       PrimitiveList objects;
    00552
    00553       // all the joints
    00554       JointList joints;
    00555
    00556       // passive servos without a Motorname
    00557       ServoList passiveServos;
    00558
    00559       // contains all active servos
    00560       MotorMap servos;
    00561
    00562       //---------------Add GoalSensor by Ren---------------
    00563       std::vector<RelativePositionSensor> GoalSensor; // Relative position sensors
    00564       bool GoalSensor_active;
    00565       //---------------Add GoalSensor by Ren---------------
    00566
    00567   };
    00568 }
    00569
    00570 #endif