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

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

- Search for |

- File List
- File Members

# arm.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  ***************************************************************************/
    00023
    00024 #ifndef __ARM_H
    00025 #define __ARM_H
    00026
    00027 #include <selforg/inspectable.h>
    00028 #include <list>
    00029 #include <vector>
    00030 #include <cmath>
    00031
    00032 #include <string.h>
    00033 #include <iostream>
    00034
    00035 #include <selforg/matrix.h>
    00036 #include "oderobot.h"
    00037 // include primitives (box, spheres, cylinders ...)
    00038 #include "primitive.h"
    00039 #include "oneaxisservo.h"
    00040 #include "joint.h"
    00041 #include "sensor.h"
    00042
    00043
    00044 #include "osgprimitive.h"
    00045
    00046 using namespace matrix;
    00047
    00048 namespace lpzrobots{
    00049
    00050   /* Enumeration of different parts and joints */
    00051   enum parts
    00052     {
    00053       base,
    00054       shoulder1,
    00055       shoulder2,
    00056       upperArm,
    00057       foreArm,
    00058       hand
    00059     };
    00060
    00061   typedef struct
    00062   {
    00063     double body_mass;
    00064     double body_height;
    00065     double body_width;
    00066     double body_depth;
    00067
    00068     double shoulder_mass;
    00069     double shoulder_radius;
    00070     double joint_offset; // distance of shoulder components from each other
    00071
    00072     double upperarm_mass;
    00073     double upperarm_radius;
    00074     double upperarm_length;
    00075
    00076     double forearm_mass;
    00077     double forearm_radius;
    00078     double forearm_length;
    00079
    00080     double elevation_min;
    00081     double elevation_max;
    00082     double humeral_min;
    00083     double humeral_max;
    00084     double azimuthal_min;
    00085     double azimuthal_max;
    00086     double elbow_min;
    00087     double elbow_max;
    00088
    00089     double motorPower;
    00090     double damping;    // motor damping
    00091     double servoFactor; // reduces servo angle constraints to servoFactor percent of hingeJoint angle constraints
    00092     double scaleMotorElbow;
    00093
    00094     bool withContext; // if true context sensors are the effector positions
    00095     bool useJointSensors; // if true joint sensors otherwise effector positions
    00096
    00097     std::list<Sensor*> sensors; // list of additional sensors
    00098
    00099   } ArmConf;
    00100
    00101   class Arm : public OdeRobot
    00102   {
    00103   public:
    00104
    00105     Arm(const OdeHandle& odeHandle, const OsgHandle& osgHandle, const ArmConf& conf, const std::string& name);
    00106
    00107     static ArmConf getDefaultConf()
    00108     {
    00109       ArmConf conf;
    00110
    00111       conf.motorPower=5;//2-15;
    00112       conf.damping=0.2;//1.0;
    00113
    00114       conf.upperarm_radius = 0.05;//0.15; <- not beautiful
    00115       conf.forearm_radius = 0.05;//0.1; <- not beautiful TODO universelle Anordnung!
    00116
    00117       // body
    00118       conf.body_mass = 1.0;
    00119       conf.body_height = 5.0;
    00120       conf.body_width = 2.0;
    00121       conf.body_depth = 0.5;
    00122       // shoulder
    00123       conf.shoulder_mass=0.005;
    00124       conf.shoulder_radius=0.03; // 0.1
    00125       conf.joint_offset=0.005;
    00126       // upper arm
    00127       conf.upperarm_mass = 0.1; // 0.01
    00128       conf.upperarm_length = 1.5;
    00129       // forearm
    00130       conf.forearm_mass = 0.1; // 0.01
    00131       conf.forearm_length = 1.2;
    00132       // stops at hinge joints
    00133       conf.elevation_min=-M_PI/3;
    00134       conf.elevation_max=M_PI/3;
    00135       conf.humeral_min=-M_PI/4;
    00136       conf.humeral_max=M_PI/4;
    00137       conf.azimuthal_min=-M_PI/4;
    00138       conf.azimuthal_max=M_PI/3;
    00139       conf.elbow_min=-M_PI/3.5; // 50Deg. the hard limit is at about 60
    00140       conf.elbow_max=M_PI/3.5; // 50Deg. the hard limit is at about 60
    00141       conf.servoFactor=1;
    00142       conf.scaleMotorElbow=0.6;
    00143       conf.useJointSensors=true;
    00144       conf.withContext=false;
    00145
    00146       return conf;
    00147     }
    00148
    00149     virtual ~Arm(){};
    00150
    00151     virtual paramkey getName() const {return "Arm";};
    00152
    00153     /**
    00154      * sets the pose of the vehicle
    00155      * @param pose desired 4x4 pose matrix
    00156      */
    00157     virtual void place(const osg::Matrix& pose);
    00158
    00159     /**
    00160      * update the subcomponents
    00161      */
    00162     virtual void update();
    00163
    00164     /**
    00165      * returns actual sensorvalues
    00166      * @param sensors sensors scaled to [-1,1]
    00167      * @param sensornumber length of the sensor array
    00168      * @return number of actually written sensors
    00169      */
    00170     virtual int getSensors(sensor* sensors, int sensornumber);
    00171
    00172     /**
    00173      * sets actual motorcommands
    00174      * @param motors motors scaled to [-1,1]
    00175      * @param motornumber length of the motor array
    00176      */
    00177     virtual void setMotors(const motor* motors, int motornumber);
    00178
    00179     /**
    00180      * returns number of sensors
    00181      */
    00182     virtual int getSensorNumber()
    00183     {
    00184       return sensorno;
    00185     };
    00186
    00187     /**
    00188      * returns number of motors
    00189      */
    00190     virtual int getMotorNumber()
    00191     {
    00192       return motorno;
    00193     };
    00194
    00195     /**
    00196      * returns a vector with the positions of all segments of the robot
    00197      * @param poslist vector of positions (of all robot segments)
    00198      * @return length of the list
    00199      */
    00200     virtual int getSegmentsPosition(std::vector<Position> &poslist);
    00201
    00202     /**
    00203      * returns the position of the endeffector (hand)
    00204      * @param position vector position vector
    00205      */
    00206     void getEndeffectorPosition(double* position);
    00207
    00208     virtual bool collisionCallback(void *data, dGeomID o1, dGeomID o2);
    00209
    00210     /**
    00211      * this function is called in each timestep. It should perform robot-internal checks,
    00212      * like space-internal collision detection, sensor resets/update etc.
    00213      * @param globalData structure that contains global data from the simulation environment
    00214      */
    00215     virtual void doInternalStuff(GlobalData& globalData);
    00216
    00217     /**
    00218      * The list of all parameters with there value as allocated lists.
    00219      */
    00220     paramlist getParamList() const;
    00221
    00222     virtual paramval getParam(const paramkey& key) const;
    00223
    00224     virtual bool setParam(const paramkey& key, paramval val);
    00225
    00226     virtual Primitive* getMainObject() const{
    00227       return objects[base];
    00228     }
    00229
    00230     /**
    00231      * the main object of the robot, which is used for position and speed tracking
    00232      */
    00233     virtual Primitive* getMainPrimitive() const
    00234     {
    00235       return objects[hand];
    00236     }
    00237
    00238     void setDlearnTargetHack(double* post);
    00239     void setDmotorTargetHack(double* post);
    00240
    00241   protected:
    00242
    00243     /**
    00244      * creates vehicle at desired pose
    00245      * @param pose 4x4 pose matrix
    00246      * @param snowmanmode snowman body
    00247      */
    00248     virtual void create(const osg::Matrix& pose);
    00249
    00250     /**
    00251      * destroys vehicle and space
    00252      */
    00253     virtual void destroy();
    00254
    00255     static void mycallback(void *data, dGeomID o1, dGeomID o2);
    00256
    00257     void hitTarget();
    00258
    00259     double dBodyGetPositionAll ( dBodyID basis , int para );
    00260     double dGeomGetPositionAll ( dGeomID basis , int para );
    00261
    00262     void BodyCreate(int n, dMass m, dReal x, dReal y, dReal z, dReal qx, dReal qy, dReal qz, dReal qangle);
    00263
    00264     // inspectable interface
    00265     //virtual std::list<Inspectable::iparamkey> getInternalParamNames() const;
    00266     //virtual std::list<Inspectable::iparamval> getInternalParams() const;
    00267     //          virtual std::list<ILayer> getStructuralLayers() const;
    00268     //          virtual std::list<IConnection> getStructuralConnections() const;
    00269
    00270
    00271     ArmConf conf;
    00272     matrix::Matrix endeff;
    00273
    00274     paramval factorSensors;
    00275     paramval print;
    00276
    00277     std::vector <Primitive*> objects; // Primitive* object[NUMParts];
    00278     std::vector <Joint*> joints; // Joint* joint[NUMJoints];
    00279     std::vector <HingeServo*> hingeServos;
    00280
    00281     int sensorno;      // number of sensors
    00282     int motorno;       // number of motors
    00283
    00284     bool created;      // true if robot was created
    00285
    00286     dSpaceID parentspace;
    00287
    00288     int printed;
    00289
    00290   };
    00291 }
    00292 #endif