Robotsystem from Robot Group Leipzig: schlangevelocity.cpp Source File

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

- File List
- File Members

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