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
1.6.3