Robotsystem from Robot Group Leipzig: schlangeservo.cpp Source File

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

- File List
- File Members

# schlangeservo.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: schlangeservo.cpp,v $
    00023  *   Revision 1.5.4.6  2006/02/23 18:05:05  martius
    00024  *   friction with angularmotor
    00025  *
    00026  *   Revision 1.5.4.5  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.5.4.4  2005/12/30 22:52:52  martius
    00030  *   joint axis have right size
    00031  *
    00032  *   Revision 1.5.4.3  2005/12/29 16:46:52  martius
    00033  *   inherits from Schlange
    00034  *   moved to osg
    00035  *
    00036  *   Revision 1.5.4.2  2005/11/15 12:29:27  martius
    00037  *   new selforg structure and OdeAgent, OdeRobot ...
    00038  *
    00039  *   Revision 1.5.4.1  2005/11/14 17:37:18  martius
    00040  *   moved to selforg
    00041  *
    00042  *   Revision 1.5  2005/11/09 13:24:42  martius
    00043  *   added GPL
    00044  *
    00045  ***************************************************************************/
    00046
    00047 #include "schlangeservo.h"
    00048
    00049 namespace lpzrobots {
    00050
    00051 SchlangeServo::SchlangeServo ( const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00052                                const SchlangeConf& conf, const char* n)
    00053   : Schlange(odeHandle, osgHandle, conf, n)
    00054 {
    00055
    00056   // prepare name;
    00057   Configurable::insertCVSInfo(name, "$RCSfile: schlangeservo.cpp,v $",
    00058                               "$Revision: 1.5.4.6 $");
    00059 }
    00060
    00061 SchlangeServo::~SchlangeServo() { }
    00062
    00063
    00064 /**
    00065  *Reads the actual motor commands from an array, an sets all motors (forces) of the snake to this values.
    00066  *It is an linear allocation.
    00067  *@param motors pointer to the array, motor values are scaled to [-1,1]
    00068  *@param motornumber length of the motor array
    00069  **/
    00070 void SchlangeServo::setMotors ( const motor* motors, int motornumber )
    00071 {
    00072   assert(created);
    00073   int len = min(motornumber, (int)servos.size());
    00074   // controller output as torques
    00075   for (int i = 0; i < len; i++){
    00076     servos[i]->set(motors[i]);
    00077   }
    00078 }
    00079
    00080 /**
    00081  *Writes the sensor values to an array in the memory.
    00082  *@param sensor* pointer to the arrays
    00083
    00084  *@param sensornumber length of the sensor array
    00085  *@return number of actually written sensors
    00086  **/
    00087 int SchlangeServo::getSensors ( sensor* sensors, int sensornumber )
    00088 {
    00089   assert(created);
    00090   int len = min(sensornumber, getSensorNumber());
    00091
    00092   for (int n = 0; n < len; n++) {
    00093     sensors[n] = servos[n]->get();
    00094   }
    00095
    00096   return len;
    00097 }
    00098
    00099
    00100 /** creates vehicle at desired position
    00101     @param pos struct Position with desired position
    00102 */
    00103 void SchlangeServo::create(const osg::Matrix& pose){
    00104   Schlange::create(pose);
    00105
    00106   //*****************joint definition***********
    00107   for ( int n = 0; n < conf.segmNumber-1; n++ ) {
    00108
    00109     Pos p1(objects[n]->getPosition());
    00110     Pos p2(objects[n]->getPosition());
    00111     HingeJoint* j = new HingeJoint(objects[n], objects[n+1],
    00112                                    (objects[n]->getPosition() + objects[n+1]->getPosition())/2,
    00113                                    Axis(0,0,1)* pose);
    00114     j->init(odeHandle, osgHandle, true, conf.segmDia * 1.02);
    00115
    00116     // setting stops at universal joints
    00117     j->setParam(dParamLoStop, -conf.jointLimit*1.5);
    00118     j->setParam(dParamHiStop,  conf.jointLimit*1.5);
    00119
    00120     // making stops bouncy
    00121     //    j->setParam (dParamBounce, 0.9 );
    00122       //    j->setParam (dParamBounce2, 0.9 ); // universal
    00123
    00124     joints.push_back(j);
    00125
    00126     HingeServo* servo =  new HingeServo(j, -conf.jointLimit, conf.jointLimit, conf.motorPower);
    00127     servos.push_back(servo);
    00128
    00129     frictionmotors.push_back(new AngularMotor1Axis(odeHandle, j, conf.frictionJoint) );
    00130   }
    00131 }
    00132
    00133 bool SchlangeServo::setParam(const paramkey& key, paramval val){
    00134   bool rv = Schlange::setParam(key, val);
    00135   for (vector<HingeServo*>::iterator i = servos.begin(); i!= servos.end(); i++){
    00136     if(*i) (*i)->setPower(conf.motorPower);
    00137   }
    00138   return rv;
    00139 }
    00140
    00141
    00142 /** destroys vehicle and space
    00143  */
    00144 void SchlangeServo::destroy(){
    00145   if (created){
    00146     Schlange::destroy();
    00147     for (vector<HingeServo*>::iterator i = servos.begin(); i!= servos.end(); i++){
    00148       if(*i) delete *i;
    00149     }
    00150     servos.clear();
    00151   }
    00152 }
    00153
    00154 }