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

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