Robotsystem from Robot Group Leipzig: schlangeforce.cpp Source File

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

- File List
- File Members

# schlangeforce.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: schlangeforce.cpp,v $
    00023  *   Revision 1.12.4.5  2006/02/23 18:05:05  martius
    00024  *   friction with angularmotor
    00025  *
    00026  *   Revision 1.12.4.4  2006/02/01 18:33:40  martius
    00027  *   use Axis type for Joint axis. very important, since otherwise Vec3 * pose is not the right direction vector anymore
    00028  *
    00029  *   Revision 1.12.4.3  2006/01/10 14:24:32  fhesse
    00030  *   second motor config
    00031  *
    00032  *   Revision 1.12.4.2  2006/01/04 14:46:00  fhesse
    00033  *   inherits from Schlange; moved to osg
    00034  *
    00035  *   Revision 1.12.4.1  2005/12/12 22:36:28  martius
    00036  *   indentation
    00037  *
    00038  *   Revision 1.12  2005/11/09 13:24:42  martius
    00039  *   added GPL
    00040  *
    00041  ***************************************************************************/
    00042
    00043 #include "schlangeforce.h"
    00044
    00045
    00046 namespace lpzrobots {
    00047
    00048   SchlangeForce::SchlangeForce ( const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00049                                  const SchlangeConf& conf, const char* n)
    00050     : Schlange(odeHandle, osgHandle, conf, n)
    00051   {
    00052     Configurable::insertCVSInfo(name, "$RCSfile: schlangeforce.cpp,v $",
    00053                                 "$Revision: 1.12.4.5 $");
    00054   }
    00055
    00056   SchlangeForce::~SchlangeForce() { }
    00057
    00058
    00059   /**
    00060    *Reads the actual motor commands from an array, and sets all motors (forces) of the snake to this values.
    00061    *It is an linear allocation.
    00062    *@param motors pointer to the array, motor values are scaled to [-1,1]
    00063    *@param motornumber length of the motor array
    00064    **/
    00065   void SchlangeForce::setMotors ( const motor* motors, int motornumber )
    00066   {
    00067     assert(created);
    00068     // there will always be an even number of motors
    00069     // (two sensors/motors per joint)
    00070     int len = min(motornumber/2, (int)joints.size());
    00071     // controller output as torques; friction added
    00072     for (int i = 0; i < len; i++){
    00073       // motorcommand
    00074       // use all motors
    00075       ((UniversalJoint*)joints[i])->addTorques(conf.motorPower * motors[2*i],
    00076                                                conf.motorPower * motors[2*i+1]);
    00077
    00078       // or use only one motor at a joint (alternating between motor 1 and motor 2)
    00079       //       if (i%2==0){
    00080       //        ((UniversalJoint*)joints[i])->addTorques(conf.motorPower * motors[2*i],0);
    00081       //       }
    00082       //       else{
    00083       //        ((UniversalJoint*)joints[i])->addTorques(0, conf.motorPower * motors[2*i+1]);
    00084       //       }
    00085     }
    00086   }
    00087
    00088   /**
    00089    *Writes the sensor values to an array in the memory.
    00090    *@param sensor* pointer to the arrays
    00091    *@param sensornumber length of the sensor array
    00092    *@return number of actually written sensors
    00093    **/
    00094   int SchlangeForce::getSensors ( sensor* sensors, int sensornumber )
    00095   {
    00096     assert(created);
    00097     // there will always be an even number of senors
    00098     // (two sensors/motors per joint)
    00099     int len = min(sensornumber/2, (int)joints.size());
    00100     // reading angle of joints
    00101     /*
    00102       for (int n = 0; n < len; n++) {
    00103       sensors[2*n]   = joints[n]->getPosition1();
    00104       sensors[2*n+1] = joints[n]->getPosition2();
    00105       }
    00106     */
    00107     // or reading anglerate of joints
    00108     for (int n = 0; n < len; n++) {
    00109       sensors[2*n]   = conf.sensorFactor * ((UniversalJoint*)joints[n])->getPosition1Rate();
    00110       //      sensors[2*n]   = factor_sensors * ((UniversalJoint*)joints[n])->getPosition1();
    00111       sensors[2*n+1] = conf.sensorFactor * ((UniversalJoint*)joints[n])->getPosition2Rate();
    00112       //      sensors[2*n+1] = factor_sensors * ((UniversalJoint*)joints[n])->getPosition2();
    00113     }
    00114     return len*2;
    00115   }
    00116
    00117
    00118   /** creates vehicle at desired position
    00119       @param pos struct Position with desired position
    00120   */
    00121   void SchlangeForce::create(const osg::Matrix& pose){
    00122     Schlange::create(pose);
    00123
    00124     //*****************joint definition***********
    00125     for ( int n = 0; n < conf.segmNumber-1; n++ ) {
    00126
    00127       Pos p1(objects[n]->getPosition());
    00128       Pos p2(objects[n+1]->getPosition());
    00129
    00130       UniversalJoint* j = new UniversalJoint(objects[n], objects[n+1],
    00131                                              (p1+p2)/2,
    00132                                              Axis(0,0,1)*pose, Axis(0,1,0)*pose);
    00133       j->init(odeHandle, osgHandle, true, conf.segmDia * 1.02);
    00134
    00135       // setting stops at universal joints
    00136       j->setParam(dParamLoStop, -conf.jointLimit);
    00137       j->setParam(dParamHiStop,  conf.jointLimit);
    00138       j->setParam(dParamLoStop2, -conf.jointLimit);
    00139       j->setParam(dParamHiStop2,  conf.jointLimit);
    00140
    00141       // making stops bouncy
    00142       j->setParam (dParamBounce, 0.9 );
    00143       j->setParam (dParamBounce2, 0.9 ); // universal
    00144
    00145       joints.push_back(j);
    00146
    00147       frictionmotors.push_back(new AngularMotor2Axis(odeHandle, j,
    00148                                                      conf.frictionJoint, conf.frictionJoint)
    00149                                );
    00150     }
    00151   }
    00152
    00153
    00154   /** destroys vehicle and space
    00155    */
    00156   void SchlangeForce::destroy(){
    00157     Schlange::destroy();
    00158   }
    00159
    00160 }