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

# muscledarm.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  *                                                                         *
    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
    00026 #ifndef __MUSCLEDARM_H
    00027 #define __MUSCLEDARM_H
    00028
    00029 #include "oderobot.h"
    00030 #include <selforg/configurable.h>
    00031 #include "primitive.h"
    00032 #include "joint.h"
    00033
    00034 namespace lpzrobots{
    00035
    00036
    00037
    00038 #define SIDE (0.2)              /* side length of a box */
    00039 #define MASS (1.0)              /* mass of a capped cylinder */
    00040 #define includeMusclesGraphics false
    00041
    00042
    00043   /* Enumeration of different parts and joints */
    00044   // left, right up and down correspond to view from top, when base is on the bottom
    00045   enum parts {base, upperArm, lowerArm,
    00046               mainMuscle11, //left mainMuscle bottom part
    00047               mainMuscle12, //left mainMuscle top part
    00048               mainMuscle21, //right mainMuscle lower part
    00049               mainMuscle22, //right mainMuscle upper part
    00050               smallMuscle11,
    00051               smallMuscle12,
    00052               smallMuscle21,
    00053               smallMuscle22,
    00054               smallMuscle31,
    00055               smallMuscle32,
    00056               smallMuscle41,
    00057               smallMuscle42,
    00058               hand,
    00059               NUMParts};
    00060
    00061   enum joints {HJ_BuA,    // hinge joint between base and upperArm
    00062                HJ_uAlA,   // hinge joint between upperArm and lowerArm
    00063
    00064                HJ_BmM11,  // hinge joint between base and mainMuscle11
    00065                HJ_lAmM12, // hinge joint between lowerArm and mainMuscle12
    00066                HJ_BmM21,  // hinge joint between base and mainMuscle21
    00067                HJ_lAmM22, // hinge joint between lowerArm and mainMuscle22
    00068
    00069                HJ_BsM11,  // hinge joint between base and smallMuscle11
    00070                HJ_uAsM12, // hinge joint between upperArm and smallMuscle12
    00071                HJ_BsM21,  // hinge joint between base and smallMuscle21
    00072                HJ_uAsM22, // hinge joint between upperArm and smallMuscle22
    00073                HJ_lAsM31, // hinge joint between lowerArm and smallMuscle31
    00074                HJ_uAsM32, // hinge joint between upperArm and smallMuscle32
    00075                HJ_lAsM41, // hinge joint between lowerArm and smallMuscle41
    00076                HJ_uAsM42, // hinge joint between upperArm and smallMuscle42
    00077
    00078                SJ_mM1, // sliderJoint between mainMuscle11 and mainMuscle12
    00079                SJ_mM2, // sliderJoint between mainMuscle21 and mainMuscle22
    00080
    00081                SJ_sM1, // slider Joint between smallMuscle11 ans smallMuscle12
    00082                SJ_sM2, // slider Joint between smallMuscle21 ans smallMuscle22
    00083                SJ_sM3, // slider Joint between smallMuscle31 ans smallMuscle32
    00084                SJ_sM4, // slider Joint between smallMuscle41 ans smallMuscle42
    00085
    00086                FJ_lAH, // fixed joint between lowerArm and hand
    00087                NUMJoints};
    00088
    00089
    00090
    00091   typedef struct {
    00092     bool jointAngleSensors; // choose sensors, all combinations are possible
    00093     bool jointAngleRateSensors;
    00094     bool muscleLengthSensors;
    00095     bool jointActuator;  // if true, two motors at the joints are used
    00096                          // if false, six muscles are used
    00097
    00098   } MuscledArmConf;
    00099
    00100   class MuscledArm : public OdeRobot{
    00101   public:
    00102
    00103     double force_[6];
    00104
    00105     MuscledArm(const OdeHandle& odeHandle, const OsgHandle& osgHandle, const MuscledArmConf& conf,
    00106                const std::string& name);
    00107
    00108     static MuscledArmConf getDefaultConf(){
    00109       MuscledArmConf conf;
    00110       conf.jointAngleSensors=false;
    00111       conf.jointAngleRateSensors=true;
    00112       conf.muscleLengthSensors=false;
    00113       conf.jointActuator=false;
    00114       return conf;
    00115     }
    00116
    00117     virtual ~MuscledArm(){};
    00118
    00119
    00120     /// update the subcomponents
    00121     virtual void update();
    00122
    00123
    00124     /** sets the pose of the vehicle
    00125         @param pose desired 4x4 pose matrix
    00126     */
    00127     virtual void place(const osg::Matrix& pose);
    00128
    00129
    00130     /** returns actual sensorvalues
    00131         @param sensors sensors scaled to [-1,1]
    00132         @param sensornumber length of the sensor array
    00133         @return number of actually written sensors
    00134     */
    00135     virtual int getSensors(sensor* sensors, int sensornumber);
    00136
    00137     /** sets actual motorcommands
    00138         @param motors motors scaled to [-1,1]
    00139         @param motornumber length of the motor array
    00140     */
    00141     virtual void setMotors(const motor* motors, int motornumber);
    00142
    00143     /** returns number of sensors
    00144      */
    00145     virtual int getSensorNumber(){
    00146       return sensorno;
    00147     };
    00148
    00149     /** returns number of motors
    00150      */
    00151     virtual int getMotorNumber(){
    00152       return motorno;
    00153     };
    00154
    00155 /*     /\** returns position of hand (=sphere at the end of lower arm)  */
    00156 /*      @return position robot position in struct Position   */
    00157 /*     *\/ */
    00158 /*     virtual osg::Vec3 MuscledArm::getPosition(); */
    00159
    00160     /** returns a vector with the positions of all segments of the robot
    00161         @param poslist vector of positions (of all robot segments)
    00162         @return length of the list
    00163     */
    00164     virtual int getSegmentsPosition(std::vector<Position> &poslist);
    00165
    00166     /** this function is called in each timestep. It should perform robot-internal checks,
    00167         like space-internal collision detection, sensor resets/update etc.
    00168         @param globalData structure that contains global data from the simulation environment
    00169     */
    00170     virtual void doInternalStuff(GlobalData& globalData);
    00171
    00172
    00173     virtual Primitive* getMainObject() const;
    00174
    00175   protected:
    00176     /** the main object of the robot, which is used for position and speed tracking */
    00177     //virtual Primitive* getMainPrimitive() const { return object[lowerArm]; }
    00178     virtual Primitive* getMainPrimitive() const { return object[hand]; }
    00179
    00180     /** creates vehicle at desired pose
    00181         @param pose 4x4 pose matrix
    00182     */
    00183     virtual void create(const osg::Matrix& pose);
    00184
    00185     /** destroys vehicle and space
    00186      */
    00187     virtual void destroy();
    00188
    00189     static void mycallback(void *data, dGeomID o1, dGeomID o2);
    00190
    00191     double dBodyGetPositionAll ( dBodyID basis , int para );
    00192     double dGeomGetPositionAll ( dGeomID basis , int para );
    00193
    00194     void BodyCreate(int n, dMass m, dReal x, dReal y, dReal z,
    00195                     dReal qx, dReal qy, dReal qz, dReal qangle);
    00196
    00197     MuscledArmConf conf;
    00198
    00199     static const int  armanzahl= 3;
    00200
    00201
    00202     Primitive* object[NUMParts];
    00203     Joint* joint[NUMJoints];
    00204
    00205     Position old_dist[NUMParts]; // used for damping
    00206
    00207     paramval factorMotors;
    00208     paramval factorSensors;
    00209     paramval damping;
    00210     paramval print;
    00211
    00212     int segmentsno;    // number of motorsvehicle segments
    00213
    00214
    00215
    00216     double gelenkabstand;
    00217     double SOCKEL_LAENGE;
    00218     double SOCKEL_BREITE;
    00219     double SOCKEL_HOEHE;
    00220     double SOCKEL_MASSE;
    00221
    00222     int sensorno;      //number of sensors
    00223     int motorno;       // number of motors
    00224
    00225     bool created;      // true if robot was created
    00226
    00227
    00228
    00229     dSpaceID parentspace;
    00230
    00231     int printed;
    00232
    00233     double max_l;
    00234     double max_r, min_l, min_r;
    00235
    00236     double base_width;
    00237     double base_length;
    00238     double upperArm_width;
    00239     double upperArm_length;
    00240     double lowerArm_width;
    00241     double lowerArm_length;
    00242     double joint_offset;
    00243     double mainMuscle_width;
    00244     double mainMuscle_length;
    00245     double smallMuscle_width;
    00246     double smallMuscle_length;
    00247
    00248   };
    00249
    00250 }
    00251 #endif