Robotsystem of the Robot Group Leipzig: muscledarm.h Source File

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

- Search for |

- File List
- File Members

# muscledarm.h

Go to the documentation of this file.

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