Robotsystem from Robot Group Leipzig: angularmotor.cpp Source File

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

- File List
- File Members

# angularmotor.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  *                                                                         *
    00023  *  DESCRIPTION                                                            *
    00024  *                                                                         *
    00025  *   $Log: angularmotor.cpp,v $
    00026  *   Revision 1.1.2.3  2006/02/23 18:05:30  martius
    00027  *   setPower (on all axis the same)
    00028  *
    00029  *   Revision 1.1.2.2  2006/02/07 15:51:56  martius
    00030  *   axis, setpower
    00031  *
    00032  *   Revision 1.1.2.1  2005/12/21 15:38:32  martius
    00033  *   Angular motors nicely wrapped
    00034  *
    00035  *   Revision 1.1.2.1  2005/12/06 17:38:21  martius
    00036  *   *** empty log message ***
    00037  *
    00038  *                                                                 *
    00039  ***************************************************************************/
    00040
    00041 #include <ode/objects.h>
    00042 #include "mathutils.h"
    00043 #include "angularmotor.h"
    00044
    00045
    00046 namespace lpzrobots {
    00047
    00048   AngularMotor::AngularMotor(const OdeHandle& odeHandle, Joint* joint){
    00049     const Primitive* p1 = joint->getPart1();
    00050     const Primitive* p2 = joint->getPart2();
    00051     motor = dJointCreateAMotor(odeHandle.world, 0);
    00052     dJointAttach(motor, p1->getBody(), p2->getBody());
    00053   }
    00054
    00055   /** sets the desired speed of all motor.
    00056       @param velocities double array with desired velocities
    00057       @param len length of the given array
    00058       @return number actually set velocities
    00059   */
    00060   int AngularMotor::set(const double* velocities, int len){
    00061     int n = min(len, getNumberOfAxes());
    00062     for(int i=0; i< n; i++){
    00063       set(i, velocities[i]);
    00064     }
    00065     return n;
    00066   }
    00067
    00068   /** returns the speed (PositionRate) of all axis
    00069       @param velocities double array to fill in the velocities
    00070       @param len length of the given array
    00071       @return number actually returned velocities
    00072   */
    00073   int AngularMotor::get(double* velocities, int len){
    00074     int n = min(len, getNumberOfAxes());
    00075     for(int i=0; i< n; i++){
    00076       velocities[i] = get(i);
    00077     }
    00078     return n;
    00079   }
    00080
    00081   /*******************************************************************************/
    00082
    00083   /** Constuct a motor attached to a OneAxisJoint. It will its axis of course.
    00084       @param power The maximum force or torque that the motor will use to achieve the desired velocity.
    00085       This must always be greater than or equal to zero.
    00086       Setting this to zero (the default value) turns off the motor.
    00087   */
    00088   AngularMotor1Axis::AngularMotor1Axis(const OdeHandle& odeHandle, OneAxisJoint* joint, double power)
    00089     : AngularMotor(odeHandle, joint), joint(joint) {
    00090     dJointSetAMotorNumAxes(motor, 1);
    00091     const Axis& a =joint->getAxis1();
    00092     dJointSetAMotorAxis(motor, 0, 0, a.x(), a.y(), a.z() );
    00093     dJointSetAMotorParam(motor, dParamFMax, power);
    00094     dJointSetAMotorParam(motor, dParamVel, 0);
    00095   }
    00096
    00097
    00098   /** sets the desired speed of the motor at the given axis.
    00099       @param axisNumber is ignored because have only one axis
    00100       @param velocity Desired motor velocity (this will be an angular or linear velocity).
    00101   */
    00102   void AngularMotor1Axis::set(int axisNumber, double velocity){
    00103     dJointSetAMotorParam(motor, dParamVel, velocity);
    00104   }
    00105
    00106   /** returns the speed (PositionRate) at the given axis, or zero if the axis is out of range
    00107       @param axisNumber is ignored because have only one axis
    00108    */
    00109   double AngularMotor1Axis::get(int axisNumber){
    00110     return joint->getPosition1Rate();
    00111   }
    00112
    00113   /**  sets the maximal force the motor has
    00114    */
    00115   void AngularMotor1Axis::setPower(double power){
    00116     dJointSetAMotorParam(motor, dParamFMax, power);
    00117   }
    00118
    00119   /*******************************************************************************/
    00120
    00121   /** Constuct a motor attached to a TwoAxisJoint. It will its two axis of course.
    00122       @param power The maximum force or torque that the motor will use to achieve the desired velocity.
    00123       This must always be greater than or equal to zero.
    00124       Setting this to zero (the default value) turns off the motor.
    00125   */
    00126   AngularMotor2Axis::AngularMotor2Axis(const OdeHandle& odeHandle, TwoAxisJoint* joint,
    00127                                        double power1, double power2)
    00128     : AngularMotor(odeHandle, joint), joint(joint) {
    00129
    00130     dJointSetAMotorNumAxes(motor, 2);
    00131     const Axis& a0 =joint->getAxis1();
    00132     dJointSetAMotorAxis(motor, 0, 0, a0.x(), a0.y(), a0.z() );
    00133     const Axis& a1 =joint->getAxis2();
    00134     dJointSetAMotorAxis(motor, 1, 0, a1.x(), a1.y(), a1.z() );
    00135
    00136
    00137     dJointSetAMotorParam(motor, dParamFMax, power1);
    00138     dJointSetAMotorParam(motor, dParamFMax2, power2);
    00139
    00140     dJointSetAMotorParam(motor, dParamVel, 0);
    00141     dJointSetAMotorParam(motor, dParamVel2, 0);
    00142   }
    00143
    00144   /** sets the desired speed of the motor at the given axis.
    00145       @param axisNumber either 0 or 1
    00146       @param velocity Desired motor velocity (this will be an angular or linear velocity).
    00147   */
    00148   void AngularMotor2Axis::set(int axisNumber, double velocity){
    00149     int param;
    00150     switch (axisNumber) {
    00151     case 0:
    00152       param = dParamVel;
    00153       break;
    00154     case 1:
    00155       param = dParamVel2;
    00156       break;
    00157     }
    00158     dJointSetAMotorParam(motor, param, velocity);
    00159   }
    00160
    00161   /** returns the speed (PositionRate) at the given axis, or zero if the axis is out of range*/
    00162   double AngularMotor2Axis::get(int axisNumber){
    00163     switch (axisNumber) {
    00164     case 0:
    00165       return joint->getPosition1Rate();
    00166     case 1:
    00167       return joint->getPosition2Rate();
    00168     default:
    00169       return 0;
    00170     }
    00171   }
    00172
    00173   /**  sets the maximal force the motor has
    00174    */
    00175   void AngularMotor2Axis::setPower(double power){
    00176     dJointSetAMotorParam(motor, dParamFMax, power);
    00177     dJointSetAMotorParam(motor, dParamFMax2, power);
    00178   }
    00179
    00180
    00181   /*******************************************************************************/
    00182
    00183   /** Constuct a motor attached to any Joint (not Sliders!).
    00184       The axis have to be provided by the user.
    00185       @axis list of axis vector and power If empty then it motor is disabled.
    00186       Power is the maximum force or torque that the motor will use to achieve the desired velocity.
    00187       This must always be greater than or equal to zero.
    00188       Setting this to zero (the default value) turns off the motor.
    00189   */
    00190   AngularMotorNAxis::AngularMotorNAxis(const OdeHandle& odeHandle, Joint* joint,
    00191                                        std::list<std::pair<double, Axis > > axis)
    00192     : AngularMotor(odeHandle, joint), joint(joint) {
    00193
    00194     int len = axis.size();
    00195
    00196     dJointSetAMotorNumAxes(motor, len);
    00197     int j=0;
    00198     for(std::list<std::pair<double, Axis > >::iterator i = axis.begin();
    00199         i!= axis.end(); i++, j++){
    00200       const Axis& a = (*i).second;
    00201       dJointSetAMotorAxis(motor, j, 0, a.x(), a.y(), a.z() );
    00202       double pwr = (*i).first;
    00203       int param;
    00204       switch (j) {
    00205       case 0:
    00206         param = dParamFMax; break;
    00207       case 1:
    00208         param = dParamFMax2; break;
    00209       case 2:
    00210         param = dParamFMax3; break;
    00211       default:
    00212         continue;
    00213       }
    00214       dJointSetAMotorParam(motor, param, pwr);
    00215
    00216       switch (j) {
    00217       case 0:
    00218         param = dParamVel; break;
    00219       case 1:
    00220         param = dParamVel2; break;
    00221       case 2:
    00222         param = dParamVel3; break;
    00223       default:
    00224         continue;
    00225       }
    00226       dJointSetAMotorParam(motor, param, 0);
    00227     }
    00228   }
    00229
    00230   /// returns the number of Axis of this Motor
    00231   int AngularMotorNAxis::getNumberOfAxes(){
    00232     return dJointGetAMotorNumAxes(motor);
    00233   }
    00234
    00235   /** sets the desired speed of the motor at the given axis.
    00236       @param axisNumber either 0 or 1
    00237       @param velocity Desired motor velocity (this will be an angular or linear velocity).
    00238   */
    00239   void AngularMotorNAxis::set(int axisNumber, double velocity){
    00240     int param;
    00241     switch (axisNumber) {
    00242     case 0:
    00243       param = dParamVel;
    00244       break;
    00245     case 1:
    00246       param = dParamVel2;
    00247       break;
    00248     case 2:
    00249       param = dParamVel3;
    00250       break;
    00251     default:
    00252       return;
    00253     }
    00254     dJointSetAMotorParam(motor, param, velocity);
    00255   }
    00256
    00257   /** returns the speed (PositionRate) at the given axis, or zero if the axis is out of range*/
    00258   double AngularMotorNAxis::get(int axisNumber){
    00259     int param;
    00260     switch (axisNumber) {
    00261     case 0:
    00262       param = dParamVel;
    00263       break;
    00264     case 1:
    00265       param = dParamVel2;
    00266       break;
    00267     case 2:
    00268       param = dParamVel3;
    00269       break;
    00270     }
    00271     return dJointGetAMotorParam(motor, param);
    00272   }
    00273
    00274   /**  sets the maximal force the motor has
    00275    */
    00276   void AngularMotorNAxis::setPower(double power){
    00277     int param;
    00278     for(int i; i<getNumberOfAxes(); i++) {
    00279       switch (i) {
    00280       case 0:
    00281         param = dParamFMax; break;
    00282       case 1:
    00283         param = dParamFMax2; break;
    00284       case 2:
    00285         param = dParamFMax3; break;
    00286       default:
    00287         continue;
    00288       }
    00289       dJointSetAMotorParam(motor, param, power);
    00290     }
    00291   }
    00292
    00293   /*******************************************************************************/
    00294
    00295     /** Constuct a motor attached to a BallJoint.
    00296         @param axis1 axis relative to body 1
    00297         @param axis3 axis relative to body 2 (must be perpendicular to axis1
    00298         (the axis 2 is calculated automatically)
    00299         @param power The maximum force or torque that the motor will use to achieve the desired velocity.
    00300         This must always be greater than or equal to zero.
    00301         Setting this to zero (the default value) turns off the motor.
    00302     */
    00303   AngularMotor3AxisEuler::AngularMotor3AxisEuler(const OdeHandle& odeHandle, BallJoint* joint,
    00304                            const Axis& axis1, const Axis& axis3, double power)
    00305       : AngularMotor(odeHandle, joint), joint(joint) {
    00306
    00307     dJointSetAMotorNumAxes(motor, 3);
    00308     dJointSetAMotorMode (motor, dAMotorEuler);
    00309
    00310     dJointSetAMotorAxis(motor, 0, 1, axis1.x(), axis1.y(), axis1.z() );
    00311     dJointSetAMotorAxis(motor, 2, 2, axis3.x(), axis3.y(), axis3.z() );
    00312
    00313
    00314     dJointSetAMotorParam(motor, dParamFMax, power);
    00315     dJointSetAMotorParam(motor, dParamFMax2, power);
    00316     dJointSetAMotorParam(motor, dParamFMax3, power);
    00317   }
    00318
    00319   /** sets the desired speed of the motor at the given axis.
    00320       @param axisNumber either 0 or 1
    00321       @param velocity Desired motor velocity (this will be an angular or linear velocity).
    00322   */
    00323   void AngularMotor3AxisEuler::set(int axisNumber, double velocity){
    00324     int param;
    00325     switch (axisNumber) {
    00326     case 0:
    00327       param = dParamVel;
    00328       break;
    00329     case 1:
    00330       param = dParamVel2;
    00331       break;
    00332     case 2:
    00333       param = dParamVel3;
    00334       break;
    00335     default:
    00336       return;
    00337     }
    00338     dJointSetAMotorParam(motor, param, velocity);
    00339   }
    00340
    00341   /** returns the speed (PositionRate) at the given axis, or zero if the axis is out of range*/
    00342   double AngularMotor3AxisEuler::get(int axisNumber){
    00343     switch (axisNumber) {
    00344     case 0:
    00345       return dJointGetAMotorAngleRate(motor, 0);
    00346     case 1:
    00347       return dJointGetAMotorAngleRate(motor, 1);
    00348     case 2:
    00349       return dJointGetAMotorAngleRate(motor, 2);
    00350     default:
    00351       return 0;
    00352     }
    00353   }
    00354
    00355   /**  sets the maximal force the motor has
    00356    */
    00357   void AngularMotor3AxisEuler::setPower(double power){
    00358     dJointSetAMotorParam(motor, dParamFMax, power);
    00359     dJointSetAMotorParam(motor, dParamFMax2, power);
    00360     dJointSetAMotorParam(motor, dParamFMax3, power);
    00361   }
    00362
    00363
    00364 }
    00365
    00366