00001 /*************************************************************************** 00002 * Copyright (C) 2005 by Robot Group Leipzig * 00003 * martius@informatik.uni-leipzig.de * 00004 * fhesse@informatik.uni-leipzig.de * 00005 * der@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: muscledarm.h,v $ 00023 * Revision 1.5 2006/07/20 17:19:44 martius 00024 * removed using namespace std from matrix.h 00025 * 00026 * Revision 1.4 2006/07/14 12:23:40 martius 00027 * selforg becomes HEAD 00028 * 00029 * Revision 1.1.4.14 2006/06/25 16:57:13 martius 00030 * abstractrobot is configureable 00031 * name and revision 00032 * 00033 * Revision 1.1.4.13 2006/05/05 16:20:27 fhesse 00034 * hand with fixed joint -> to allow tracing 00035 * (does not work with transform while only geom) 00036 * 00037 * Revision 1.1.4.12 2006/03/31 16:13:59 fhesse 00038 * *** empty log message *** 00039 * 00040 * Revision 1.1.4.11 2006/03/30 12:34:56 martius 00041 * documentation updated 00042 * 00043 * Revision 1.1.4.10 2006/03/28 14:24:37 fhesse 00044 * minor changes 00045 * 00046 * Revision 1.1.4.9 2006/01/31 09:58:11 fhesse 00047 * basically working now 00048 * 00049 * Revision 1.1.4.8 2006/01/13 12:22:07 fhesse 00050 * partially working 00051 * 00052 * Revision 1.1.4.7 2006/01/10 16:45:53 fhesse 00053 * not working osg version 00054 * 00055 * Revision 1.1.4.6 2006/01/10 09:38:00 fhesse 00056 * partially moved to osg 00057 * 00058 * Revision 1.1.4.5 2005/12/16 16:36:05 fhesse 00059 * manual control via keyboard 00060 * setMotors via dJointAddSliderForce 00061 * 00062 * Revision 1.1.4.4 2005/11/24 16:15:57 fhesse 00063 * moved from main branch, sensors improved 00064 * 00065 * Revision 1.3 2005/11/17 16:29:25 fhesse 00066 * initial version 00067 * 00068 * Revision 1.2 2005/11/15 12:36:27 fhesse 00069 * muscles drawn as muscles, sphere drawn at tip of lower arm 00070 * 00071 * Revision 1.1 2005/11/11 15:37:06 fhesse 00072 * preinitial version 00073 * * 00074 * * 00075 ***************************************************************************/ 00076 00077 00078 #ifndef __MUSCLEDARM_H 00079 #define __MUSCLEDARM_H 00080 00081 #include "oderobot.h" 00082 #include <selforg/configurable.h> 00083 #include "primitive.h" 00084 #include "joint.h" 00085 00086 namespace lpzrobots{ 00087 00088 00089 00090 #define SIDE (0.2) /* side length of a box */ 00091 #define MASS (1.0) /* mass of a capped cylinder */ 00092 #define includeMusclesGraphics false 00093 00094 00095 /* Enumeration of different parts and joints */ 00096 // left, right up and down correspond to view from top, when base is on the bottom 00097 enum parts {base, upperArm, lowerArm, 00098 mainMuscle11, //left mainMuscle bottom part 00099 mainMuscle12, //left mainMuscle top part 00100 mainMuscle21, //right mainMuscle lower part 00101 mainMuscle22, //right mainMuscle upper part 00102 smallMuscle11, 00103 smallMuscle12, 00104 smallMuscle21, 00105 smallMuscle22, 00106 smallMuscle31, 00107 smallMuscle32, 00108 smallMuscle41, 00109 smallMuscle42, 00110 hand, 00111 NUMParts}; 00112 00113 enum joints {HJ_BuA, // hinge joint between base and upperArm 00114 HJ_uAlA, // hinge joint between upperArm and lowerArm 00115 00116 HJ_BmM11, // hinge joint between base and mainMuscle11 00117 HJ_lAmM12, // hinge joint between lowerArm and mainMuscle12 00118 HJ_BmM21, // hinge joint between base and mainMuscle21 00119 HJ_lAmM22, // hinge joint between lowerArm and mainMuscle22 00120 00121 HJ_BsM11, // hinge joint between base and smallMuscle11 00122 HJ_uAsM12, // hinge joint between upperArm and smallMuscle12 00123 HJ_BsM21, // hinge joint between base and smallMuscle21 00124 HJ_uAsM22, // hinge joint between upperArm and smallMuscle22 00125 HJ_lAsM31, // hinge joint between lowerArm and smallMuscle31 00126 HJ_uAsM32, // hinge joint between upperArm and smallMuscle32 00127 HJ_lAsM41, // hinge joint between lowerArm and smallMuscle41 00128 HJ_uAsM42, // hinge joint between upperArm and smallMuscle42 00129 00130 SJ_mM1, // sliderJoint between mainMuscle11 and mainMuscle12 00131 SJ_mM2, // sliderJoint between mainMuscle21 and mainMuscle22 00132 00133 SJ_sM1, // slider Joint between smallMuscle11 ans smallMuscle12 00134 SJ_sM2, // slider Joint between smallMuscle21 ans smallMuscle22 00135 SJ_sM3, // slider Joint between smallMuscle31 ans smallMuscle32 00136 SJ_sM4, // slider Joint between smallMuscle41 ans smallMuscle42 00137 00138 FJ_lAH, // fixed joint between lowerArm and hand 00139 NUMJoints}; 00140 00141 00142 00143 typedef struct { 00144 bool jointAngleSensors; // choose sensors, all combinations are possible 00145 bool jointAngleRateSensors; 00146 bool muscleLengthSensors; 00147 bool jointActuator; // if true, two motors at the joints are used 00148 // if false, six muscles are used 00149 00150 } MuscledArmConf; 00151 00152 class MuscledArm : public OdeRobot{ 00153 public: 00154 00155 double force_[6]; 00156 00157 MuscledArm(const OdeHandle& odeHandle, const OsgHandle& osgHandle, const MuscledArmConf& conf, 00158 const std::string& name); 00159 00160 static MuscledArmConf getDefaultConf(){ 00161 MuscledArmConf conf; 00162 conf.jointAngleSensors=false; 00163 conf.jointAngleRateSensors=true; 00164 conf.muscleLengthSensors=false; 00165 conf.jointActuator=false; 00166 return conf; 00167 } 00168 00169 virtual ~MuscledArm(){}; 00170 00171 00172 /// update the subcomponents 00173 virtual void update(); 00174 00175 00176 /** sets the pose of the vehicle 00177 @param pose desired 4x4 pose matrix 00178 */ 00179 virtual void place(const osg::Matrix& pose); 00180 00181 00182 /** returns actual sensorvalues 00183 @param sensors sensors scaled to [-1,1] 00184 @param sensornumber length of the sensor array 00185 @return number of actually written sensors 00186 */ 00187 virtual int getSensors(sensor* sensors, int sensornumber); 00188 00189 /** sets actual motorcommands 00190 @param motors motors scaled to [-1,1] 00191 @param motornumber length of the motor array 00192 */ 00193 virtual void setMotors(const motor* motors, int motornumber); 00194 00195 /** returns number of sensors 00196 */ 00197 virtual int getSensorNumber(){ 00198 return sensorno; 00199 }; 00200 00201 /** returns number of motors 00202 */ 00203 virtual int getMotorNumber(){ 00204 return motorno; 00205 }; 00206 00207 /* /\** returns position of hand (=sphere at the end of lower arm) */ 00208 /* @return position robot position in struct Position */ 00209 /* *\/ */ 00210 /* virtual osg::Vec3 MuscledArm::getPosition(); */ 00211 00212 /** returns a vector with the positions of all segments of the robot 00213 @param poslist vector of positions (of all robot segments) 00214 @return length of the list 00215 */ 00216 virtual int getSegmentsPosition(std::vector<Position> &poslist); 00217 00218 virtual bool collisionCallback(void *data, dGeomID o1, dGeomID o2); 00219 /** this function is called in each timestep. It should perform robot-internal checks, 00220 like space-internal collision detection, sensor resets/update etc. 00221 @param globalData structure that contains global data from the simulation environment 00222 */ 00223 virtual void doInternalStuff(const GlobalData& globalData); 00224 00225 /** The list of all parameters with there value as allocated lists. 00226 */ 00227 paramlist getParamList() const; 00228 00229 virtual paramval getParam(const paramkey& key) const; 00230 00231 virtual bool setParam(const paramkey& key, paramval val); 00232 00233 virtual Primitive* getMainObject() const; 00234 00235 protected: 00236 /** the main object of the robot, which is used for position and speed tracking */ 00237 //virtual Primitive* getMainPrimitive() const { return object[lowerArm]; } 00238 virtual Primitive* getMainPrimitive() const { return object[hand]; } 00239 00240 /** creates vehicle at desired pose 00241 @param pose 4x4 pose matrix 00242 */ 00243 virtual void create(const osg::Matrix& pose); 00244 00245 /** destroys vehicle and space 00246 */ 00247 virtual void destroy(); 00248 00249 static void mycallback(void *data, dGeomID o1, dGeomID o2); 00250 00251 double dBodyGetPositionAll ( dBodyID basis , int para ); 00252 double dGeomGetPositionAll ( dGeomID basis , int para ); 00253 00254 void BodyCreate(int n, dMass m, dReal x, dReal y, dReal z, 00255 dReal qx, dReal qy, dReal qz, dReal qangle); 00256 00257 MuscledArmConf conf; 00258 00259 static const int armanzahl= 3; 00260 00261 00262 Primitive* object[NUMParts]; 00263 Joint* joint[NUMJoints]; 00264 00265 Position old_dist[NUMParts]; // used for damping 00266 00267 paramval factorMotors; 00268 paramval factorSensors; 00269 paramval damping; 00270 paramval print; 00271 00272 int segmentsno; // number of motorsvehicle segments 00273 00274 00275 00276 double gelenkabstand; 00277 double SOCKEL_LAENGE; 00278 double SOCKEL_BREITE; 00279 double SOCKEL_HOEHE; 00280 double SOCKEL_MASSE; 00281 00282 int sensorno; //number of sensors 00283 int motorno; // number of motors 00284 00285 bool created; // true if robot was created 00286 00287 00288 00289 dSpaceID parentspace; 00290 00291 int printed; 00292 00293 double max_l; 00294 double max_r, min_l, min_r; 00295 00296 double base_width; 00297 double base_length; 00298 double upperArm_width; 00299 double upperArm_length; 00300 double lowerArm_width; 00301 double lowerArm_length; 00302 double joint_offset; 00303 double mainMuscle_width; 00304 double mainMuscle_length; 00305 double smallMuscle_width; 00306 double smallMuscle_length; 00307 00308 }; 00309 00310 } 00311 #endif
1.4.7