Robotsystem from Robot Group Leipzig: arm2segm.cpp Source File

- Main Page
- Modules
- Namespaces
- Classes
- Files
- Directories
- Examples

- File List
- File Members

# arm2segm.cpp

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: arm2segm.cpp,v $
    00023  *   Revision 1.6.4.6  2006/01/05 13:50:31  fhesse
    00024  *   started to add tip at the end
    00025  *
    00026  *   Revision 1.6.4.5  2006/01/04 14:45:10  fhesse
    00027  *   hingejoint axis multiplied with pose (in hinge joiunt constructor)
    00028  *
    00029  *   Revision 1.6.4.4  2006/01/03 13:18:51  fhesse
    00030  *   cleaned up
    00031  *   TODO: in the long run robot disappears (huge sensorvalues)
    00032  *
    00033  *   Revision 1.6.4.3  2006/01/03 10:01:46  fhesse
    00034  *   moved to osg
    00035  *
    00036  *   Revision 1.6.4.2  2005/11/15 12:29:25  martius
    00037  *   new selforg structure and OdeAgent, OdeRobot ...
    00038  *
    00039  *   Revision 1.6.4.1  2005/11/14 17:37:16  martius
    00040  *   moved to selforg
    00041  *
    00042  *   Revision 1.6  2005/11/09 13:24:42  martius
    00043  *   added GPL
    00044  *
    00045  ***************************************************************************/
    00046
    00047 #include "arm2segm.h"
    00048
    00049 namespace lpzrobots{
    00050
    00051   Arm2Segm::Arm2Segm(const OdeHandle& odeHandle, const OsgHandle& osgHandle, const Arm2SegmConf armConf):
    00052     OdeRobot(odeHandle, osgHandle), conf(armConf){
    00053
    00054     // prepare name;
    00055     Configurable::insertCVSInfo(name, "$RCSfile: arm2segm.cpp,v $",
    00056                                 "$Revision: 1.6.4.6 $");
    00057     parentspace=odeHandle.space;
    00058
    00059     speed=1.0;
    00060     factorSensors=2.0;
    00061     sensorno=conf.segmentsno;
    00062     motorno=conf.segmentsno;
    00063     this->osgHandle.color = Color(1, 0, 0, 1.0f);
    00064
    00065     created=false;
    00066   };
    00067
    00068   /** sets actual motorcommands
    00069       @param motors motors scaled to [-1,1]
    00070       @param motornumber length of the motor array
    00071   */
    00072   void Arm2Segm::setMotors(const motor* motors, int motornumber){
    00073     assert(created); // robot must exist
    00074     // the number of controlled motors is minimum of
    00075     // "number of motorcommands" (motornumber) and
    00076     // "number of motors inside the robot" (motorno)
    00077     int len = (motornumber < motorno)? motornumber : motorno;
    00078
    00079     // for each motor the motorcommand (between -1 and 1) multiplied with speed
    00080     // is set (maximal force defined by amotors, see create() below)
    00081     for (int i=0; i<len; i++){
    00082       amotors[i]->set(1, motors[i]*speed);
    00083     }
    00084
    00085     // another possibility is to set half of the difference between last set speed
    00086     // and the actual desired speed as new speed;
    00087     /*
    00088     double tmp;
    00089     for (int i=0; i<len; i++){
    00090       tmp=amotors[i]->get(1);
    00091       amotors[i]->set(1,tmp + 0.5*(motors[i]*speed-tmp) );
    00092     }
    00093     */
    00094   };
    00095
    00096
    00097   /** returns actual sensorvalues
    00098       @param sensors sensors scaled to [-1,1] (more or less)
    00099       @param sensornumber length of the sensor array
    00100       @return number of actually written sensors
    00101   */
    00102   int Arm2Segm::getSensors(sensor* sensors, int sensornumber){
    00103     assert(created); // robot must exist
    00104
    00105     // the number of sensors to read is the minimum of
    00106     // "number of sensors requested" (sensornumber) and
    00107     // "number of sensors inside the robot" (sensorno)
    00108     int len = (sensornumber < sensorno)? sensornumber : sensorno;
    00109
    00110     // for each sensor the anglerate of the joint is red and scaled with 1/speed
    00111     for (int i=0; i<len; i++){
    00112       sensors[i]=amotors[i]->get(1);  // is equal to: ((HingeJoint*)joints[i])->getPosition1Rate()
    00113       // or read angle of each joint:
    00114       // sensors[i]=((HingeJoint*)joints[i])->getPosition1();
    00115       sensors[i]/=speed;  //scaling
    00116     }
    00117     // the number of red sensors is returned
    00118     return len;
    00119   };
    00120
    00121   /** sets the vehicle to position pos, sets color to c, and creates robot if necessary
    00122       @params pos desired position of the robot in struct Position
    00123       @param c desired color for the robot in struct Color
    00124   */
    00125   void Arm2Segm::place(const osg::Matrix& pose){
    00126     // the position of the robot is the center of the base
    00127     // to set it on the ground when the z component of the position is 0
    00128     // base_length*0.5 is added (without this half of the base will be in the ground)
    00129     osg::Matrix p2;
    00130     p2 = pose
    00131       // TODO: create is not robust enough to endure this pose !!
    00132       //      * osg::Matrix::rotate(M_PI/2, 0, 0, 1)
    00133       * osg::Matrix::translate(osg::Vec3(0, 0, conf.base_length* 0.5));
    00134     create(p2);
    00135
    00136
    00137       // p->setPose(osg::Matrix::rotate(M_PI/2, 0, 0, 1) *
    00138 //               osg::Matrix::translate((n-half)*conf.segmLength, 0 , conf.segmDia/2) *
    00139 //               pose);
    00140
    00141   };
    00142
    00143
    00144   /** returns a vector with the positions of all segments of the robot
    00145       @param poslist vector of positions (of all robot segments)
    00146       @return length of the list
    00147   */
    00148   int Arm2Segm::getSegmentsPosition(vector<Position> &poslist){
    00149     for (int i=0; i<conf.segmentsno; i++){
    00150       Pos p = objects[i]->getPosition();
    00151       poslist.push_back(p.toPosition());
    00152     }
    00153     return conf.segmentsno;
    00154   };
    00155
    00156
    00157   void Arm2Segm::update(){
    00158     assert(created); // robot must exist
    00159     for (vector<Primitive*>::iterator i = objects.begin(); i!=objects.end(); i++) {
    00160       if (*i) (*i)->update();
    00161     }
    00162     for (vector<Joint*>::iterator i = joints.begin(); i!=joints.end(); i++) {
    00163       if (*i) (*i)->update();
    00164     }
    00165   };
    00166
    00167
    00168   void Arm2Segm::doInternalStuff(const GlobalData& globalData){
    00169   };
    00170
    00171   void Arm2Segm::mycallback(void *data, dGeomID o1, dGeomID o2){
    00172     // no internal collisions in this robot
    00173   };
    00174
    00175   bool Arm2Segm::collisionCallback(void *data, dGeomID o1, dGeomID o2){
    00176     //checks if one of the collision objects is part of the robot
    00177     if( o1 == (dGeomID)odeHandle.space || o2 == (dGeomID)odeHandle.space){
    00178
    00179       // the rest is for collisions of some elements of the robot with the rest of the world
    00180       int i,n;
    00181       const int N = 10;
    00182       dContact contact[N];
    00183
    00184       n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
    00185       for (i=0; i<n; i++){
    00186         contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
    00187           dContactSoftERP | dContactSoftCFM | dContactApprox1;
    00188         contact[i].surface.slip1 = 0.005;
    00189         contact[i].surface.slip2 = 0.005;
    00190         contact[i].surface.mu = 0.5;
    00191         contact[i].surface.soft_erp = 0.9;
    00192         contact[i].surface.soft_cfm = 0.001;
    00193         dJointID c = dJointCreateContact( odeHandle.world, odeHandle.jointGroup, &contact[i]);
    00194         dJointAttach ( c , dGeomGetBody(contact[i].geom.g1) , dGeomGetBody(contact[i].geom.g2)) ;
    00195       }
    00196       return true;
    00197     }
    00198     return false;
    00199   };
    00200
    00201
    00202   Primitive* Arm2Segm::getMainPrimitive() const{
    00203     // at the moment returning position of last arm,
    00204     // better would be a tip at the end of this arm, as in muscledArm
    00205     return objects[conf.segmentsno-1];
    00206   }
    00207
    00208
    00209   /** creates vehicle at desired position
    00210       @param pos struct Position with desired position
    00211   */
    00212   void Arm2Segm::create(const osg::Matrix& pose){
    00213     if (created) {
    00214       destroy();
    00215     }
    00216
    00217     // create vehicle space and add it to parentspace
    00218     odeHandle.space = dSimpleSpaceCreate (parentspace);
    00219
    00220     // create base
    00221     Primitive* o = new Box(conf.base_length, conf.base_width, conf.base_length);
    00222     o -> init(odeHandle, conf.base_mass, osgHandle);
    00223     o->setPose( pose); // set base to given pose
    00224     //o->getOSGPrimitive()->setTexture("Images/wood.rgb");
    00225     objects.push_back(o);
    00226
    00227     // create arms
    00228     for (int i=0; i<(conf.segmentsno-1); i++){
    00229       o = new Box(conf.arm_length, conf.arm_width, conf.arm_width);
    00230       o -> init(odeHandle, conf.arm_mass, osgHandle);
    00231       // move arms to desired places
    00232       o -> setPose(osg::Matrix::translate(osg::Vec3(// shift (armlength -JOINT_OFFSET) to have overlapp
    00233                                                     (i+0.5)*conf.arm_length - (i+1)*conf.joint_offset,
    00234                                                     // shift to have place between arms
    00235                                                     (i+1)*conf.arm_offset
    00236                                                     + (i+0.5)*conf.arm_width + 0.5*conf.base_width,
    00237                                                     // height is ok, no shift
    00238                                                     0) ) * pose);
    00239       objects.push_back(o);
    00240     }
    00241
    00242     // hinge joint and angular motor to connect base with world
    00243     Pos p1(objects[0]->getPosition());
    00244     HingeJoint* j = new HingeJoint(0, objects[0], p1, osg::Vec3(0,0,1) /** pose*/);
    00245     j -> init(odeHandle, osgHandle,/*withVisual*/true);
    00246     joints.push_back(j);
    00247     AngularMotor1Axis* a=new AngularMotor1Axis(odeHandle, (OneAxisJoint *)joints[0], conf.max_force);
    00248     amotors.push_back(a);
    00249
    00250     // hinge joint and angular motor to connect base with first arm
    00251     Pos p2(objects[1]->getPosition());
    00252     p1[1]=(p1[1]+p2[1])/2;
    00253     j = new HingeJoint(objects[0], objects[1], p1, osg::Vec3(0,1,0) /** pose*/);
    00254     j -> init(odeHandle, osgHandle,/*withVisual*/true);
    00255     joints.push_back(j);
    00256     a=new AngularMotor1Axis(odeHandle, (OneAxisJoint *)joints[1], conf.max_force);
    00257     amotors.push_back(a);
    00258
    00259     // hinge joint and angular motor to connect arms
    00260     for (int i=2; i<conf.segmentsno; i++) {
    00261       Pos po1(objects[i-1]->getPosition());
    00262       Pos po2(objects[i]->getPosition());
    00263       Pos po3( (po1+po2)/2);
    00264       j = new HingeJoint(objects[i-1], objects[i], po3, osg::Vec3(0,1,0) /** pose*/);
    00265       j -> init(odeHandle, osgHandle,/*withVisual*/true);
    00266       joints.push_back(j);
    00267       a=new AngularMotor1Axis(odeHandle, (OneAxisJoint *)joints[i], conf.max_force);
    00268       amotors.push_back(a);
    00269     }
    00270
    00271 // --------------
    00272 // TODO: add tip at the end of arm for easier getPosition; temporarily positioning of transform object does not work
    00273     osg::Matrix ps;
    00274     ps.makeIdentity();
    00275     Primitive* o1 = new Sphere(conf.arm_width*0.8);
    00276     Primitive* o2 = new Transform(objects[objects.size()-1], o1, osg::Matrix::translate(0, conf.arm_length*0.5, 0) * ps);
    00277     o2->init(odeHandle, /*mass*/0, osgHandle, /*withBody*/ false);
    00278 // --------------
    00279
    00280     created=true;
    00281   };
    00282
    00283
    00284   /** destroys vehicle and space
    00285    */
    00286   void Arm2Segm::destroy(){
    00287     if (created){
    00288       for (vector<Primitive*>::iterator i=objects.begin(); i!=objects.end(); i++){
    00289         if (*i) delete *i;
    00290       }
    00291       objects.clear();
    00292       for (vector<Joint*>::iterator i=joints.begin(); i!=joints.end(); i++){
    00293         if (*i) delete *i;
    00294       }
    00295       joints.clear();
    00296       for (vector<AngularMotor1Axis*>::iterator i=amotors.begin(); i!=amotors.end(); i++){
    00297         if (*i) delete *i;
    00298       }
    00299       amotors.clear();
    00300       dSpaceDestroy(odeHandle.space);
    00301     }
    00302     created=false;
    00303   };
    00304
    00305
    00306
    00307   Configurable::paramlist Arm2Segm::getParamList() const{
    00308     paramlist list;
    00309     list.push_back(pair<paramkey, paramval> (string("speed"), speed));
    00310     list.push_back(pair<paramkey, paramval> (string("factorSensors"), factorSensors));
    00311     return list;
    00312   }
    00313
    00314   Configurable::paramval Arm2Segm::getParam(const paramkey& key) const{
    00315     if(key == "speed") return speed;
    00316     else if(key == "factorSensors") return factorSensors;
    00317     else  return Configurable::getParam(key) ;
    00318   };
    00319
    00320   bool Arm2Segm::setParam(const paramkey& key, paramval val){
    00321     if(key == "speed") speed=val;
    00322     else if(key == "factorSensors") factorSensors = val;
    00323     else return Configurable::setParam(key, val);
    00324     return true;
    00325   };
    00326
    00327 }