Robot Simulator of the Robotics Group for Self-Organization of Control: twoaxisservo.h Source File

- Main Page
- Namespaces
- Classes
- Files
- Directories
- Related Pages
- Examples

- Search for |

- File List
- File Members

# twoaxisservo.h

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: twoaxisservo.h,v $
    00023  *   Revision 1.10  2009/08/12 10:28:48  der
    00024  *   Centered servos use stepNoCutoff which is much more stable
    00025  *
    00026  *   Revision 1.9  2009/08/10 14:46:41  der
    00027  *   power() functions removed because references are bad vor velocity servo
    00028  *   setPower() functions added
    00029  *
    00030  *   Revision 1.8  2009/05/11 17:04:49  martius
    00031  *   no velocity limit if maxVel=0
    00032  *
    00033  *   Revision 1.7  2009/05/11 15:43:22  martius
    00034  *   new velocity controlling servo motors
    00035  *
    00036  *   Revision 1.6  2009/02/04 09:37:05  martius
    00037  *   fixed offset of 2 in centered servos
    00038  *
    00039  *   Revision 1.5  2008/11/14 11:23:05  martius
    00040  *   added centered Servos! This is useful for highly nonequal min max values
    00041  *   skeleton has now also a joint in the back
    00042  *
    00043  *   Revision 1.4  2007/07/17 07:17:40  martius
    00044  *   joints limits are set
    00045  *   damping is accessable for both axis
    00046  *
    00047  *   Revision 1.3  2007/07/03 13:02:22  martius
    00048  *   maximum velocity check
    00049  *   new pid with stepsize
    00050  *
    00051  *   Revision 1.2  2007/04/03 16:31:02  der
    00052  *   use fixed version of pid
    00053  *   new default values
    00054  *
    00055  *   Revision 1.1  2007/02/12 13:28:20  martius
    00056  *   twoaxisservo and some minor changes
    00057  *
    00058  *
    00059  *
    00060  *                                                                 *
    00061  ***************************************************************************/
    00062 #ifndef __SERVO2_H
    00063 #define __SERVO2_H
    00064
    00065 #include "joint.h"
    00066 #include "pid.h"
    00067 #include "angularmotor.h"
    00068 #include <selforg/controller_misc.h>
    00069
    00070 namespace lpzrobots {
    00071
    00072   /** general servo motor for 2 axis joints
    00073    */
    00074   class TwoAxisServo {
    00075   public:
    00076     /** min and max values are understood as travel bounds. Min should be less than 0.*/
    00077
    00078     TwoAxisServo(TwoAxisJoint* joint, double _min1, double _max1, double power1,
    00079                  double _min2, double _max2, double power2,
    00080                  double damp=0.2, double integration=2, double maxVel=10.0,
    00081                  double jointLimit = 1.3, bool minmaxCheck=true)
    00082       : joint(joint),
    00083         pid1(power1, integration, damp),
    00084         pid2(power2, integration, damp),
    00085         maxVel(maxVel), jointLimit(jointLimit) {
    00086       assert(joint);
    00087       setMinMax1(_min1,_max1);
    00088       setMinMax2(_min2,_max2);
    00089       assert(min1<max1); assert(min2<max2);
    00090       assert(!minmaxCheck || min1 <= 0); assert(!minmaxCheck || min2 <= 0);
    00091       assert(!minmaxCheck || max1 >= 0); assert(!minmaxCheck || max2 >= 0);
    00092       assert(power1 >=0 && power2 >=0 && damp >=0 && integration >=0);
    00093     }
    00094     virtual ~TwoAxisServo(){}
    00095
    00096     /** sets the set point of the servo.
    00097         Position must be between -1 and 1. It is scaled to fit into min, max
    00098     */
    00099     virtual void set(double pos1, double pos2){
    00100       if(pos1 > 0){
    00101         pos1 *= max1;
    00102       }else{
    00103         pos1 *= -min1;
    00104       }
    00105       pid1.setTargetPosition(pos1);
    00106       // double force1 = pid1.stepWithD(joint->getPosition1(), joint->getPosition1Rate());
    00107       double force1 = pid1.step(joint->getPosition1(), joint->odeHandle.getTime());
    00108       // limit force to 1*KP
    00109       force1 = std::min(pid1.KP, std::max(-pid1.KP,force1));// limit force to 1*KP
    00110
    00111       if(pos2 > 0){
    00112         pos2 *= max2;
    00113       }else{
    00114         pos2 *= -min2;
    00115       }
    00116       pid2.setTargetPosition(pos2);
    00117       //      double force2 = pid2.stepWithD(joint->getPosition2(), joint->getPosition2Rate());
    00118       double force2 = pid2.step(joint->getPosition2(), joint->odeHandle.getTime());
    00119       // limit force to 1*KP
    00120       force2 = std::min(pid2.KP, std::max(-pid2.KP,force2));// limit force to 1*KP
    00121       joint->addForces(force1, force2);
    00122       if(maxVel >0 ){
    00123         joint->getPart1()->limitLinearVel(maxVel);
    00124         joint->getPart2()->limitLinearVel(maxVel);
    00125       }
    00126     }
    00127
    00128     /** returns the position of the servo (joint) of 1. axis in ranges [-1, 1] (scaled by min1, max1)*/
    00129     virtual double get1(){
    00130       double pos =  joint->getPosition1();
    00131       if(pos > 0){
    00132         pos /= max1;
    00133       }else{
    00134         pos /= -min1;
    00135       }
    00136       return pos;
    00137     }
    00138
    00139     /** returns the position of the servo (joint) of 2. axis in ranges [-1, 1] (scaled by min2, max2)*/
    00140     virtual double get2(){
    00141       double pos =  joint->getPosition2();
    00142       if(pos > 0){
    00143         pos /= max2;
    00144       }else{
    00145         pos /= -min2;
    00146       }
    00147       return pos;
    00148     }
    00149
    00150     /** returns the positions of the joint in ranges [-1, 1] (scaled by min, max)*/
    00151     void get(double& p1, double& p2){
    00152       p1=get1();
    00153       p2=get2();
    00154     }
    00155
    00156
    00157     /** adjusts the power of the servo*/
    00158     virtual void setPower(double power1, double power2) {
    00159       pid1.KP = power1;
    00160       pid2.KP = power2;
    00161     };
    00162
    00163     /** returns the power of the servo*/
    00164     virtual void setPower1(double power1) {
    00165       pid1.KP = power1;
    00166     };
    00167
    00168     /** returns the power of the servo*/
    00169     virtual void setPower2(double power2) {
    00170       pid2.KP = power2;
    00171     };
    00172
    00173     /** returns the power of the servo*/
    00174     virtual double getPower1() {
    00175       return pid1.KP;
    00176     };
    00177     /** returns the power of the servo*/
    00178     virtual double getPower2() {
    00179       return pid2.KP;
    00180     };
    00181
    00182     /*TODO remove this referenced interface*/
    00183     /** returns the damping of the servo*/
    00184     virtual double& damping1() {
    00185       return pid1.KD;
    00186     };
    00187
    00188     /** returns the damping of the servo*/
    00189     virtual double& damping2() {
    00190       return pid2.KD;
    00191     };
    00192
    00193     /** returns the damping of the servo*/
    00194     virtual double& offsetCanceling() {
    00195       return pid1.KI;
    00196     };
    00197
    00198     virtual void setMinMax1(double _min, double _max){
    00199       min1=_min;
    00200       max1=_max;
    00201       joint->setParam(dParamLoStop, _min * jointLimit);
    00202       joint->setParam(dParamHiStop, _max * jointLimit);
    00203     }
    00204
    00205     virtual void setMinMax2(double _min, double _max){
    00206       min2=_min;
    00207       max2=_max;
    00208       joint->setParam(dParamLoStop2, _min * jointLimit);
    00209       joint->setParam(dParamHiStop2, _max * jointLimit);
    00210     }
    00211
    00212     /** adjusts maximal speed of servo*/
    00213     virtual void setMaxVel(double maxVel) {
    00214       this->maxVel = maxVel;
    00215     };
    00216     /** adjusts maximal speed of servo*/
    00217     virtual double getMaxVel() {
    00218       return maxVel;
    00219     };
    00220
    00221
    00222   protected:
    00223     TwoAxisJoint* joint;
    00224     double min1;
    00225     double max1;
    00226     double min2;
    00227     double max2;
    00228     PID pid1;
    00229     PID pid2;
    00230     double maxVel;
    00231     double jointLimit;
    00232   };
    00233
    00234   typedef TwoAxisServo UniversalServo;
    00235
    00236
    00237   /** general servo motor for 2 axis joints with zero position centered
    00238    */
    00239   class TwoAxisServoCentered : public TwoAxisServo {
    00240   public:
    00241     /** min and max values are understood as travel bounds.
    00242         The zero position is max-min/2
    00243     */
    00244     TwoAxisServoCentered(TwoAxisJoint* joint, double _min1, double _max1, double power1,
    00245                          double _min2, double _max2, double power2,
    00246                          double damp=0.2, double integration=2, double maxVel=10.0,
    00247                          double jointLimit = 1.3)
    00248       : TwoAxisServo(joint, _min1, _max1, power1, _min2, _max2, power2,
    00249                      damp, integration, maxVel, jointLimit, false){
    00250     }
    00251     virtual ~TwoAxisServoCentered(){}
    00252
    00253     /** sets the set point of the servo.
    00254         Position must be between -1 and 1. It is scaled to fit into min, max,
    00255         however 0 is just in the center of min and max
    00256     */
    00257     virtual void set(double pos1, double pos2){
    00258       pos1 = clip(pos1, -1.0, 1.0);
    00259       pos2 = clip(pos2, -1.0, 1.0);
    00260       pos1 = (pos1+1)*(max1-min1)/2 + min1;
    00261
    00262       pid1.setTargetPosition(pos1);
    00263       double force1 = pid1.stepNoCutoff(joint->getPosition1(), joint->odeHandle.getTime());
    00264       // limit force to 10*KP
    00265       force1 = clip(force1,-10*pid1.KP, 10*pid1.KP);
    00266
    00267       pos2 = (pos2+1)*(max2-min2)/2 + min2;
    00268       pid2.setTargetPosition(pos2);
    00269       double force2 = pid2.stepNoCutoff(joint->getPosition2(), joint->odeHandle.getTime());
    00270       // limit force to 10*KP
    00271       force2 = clip(force2,-10*pid2.KP, 10*pid2.KP);
    00272       joint->addForces(force1, force2);
    00273       if(maxVel >0 ){
    00274         joint->getPart1()->limitLinearVel(maxVel);
    00275         joint->getPart2()->limitLinearVel(maxVel);
    00276       }
    00277     }
    00278
    00279     /** returns the position of the servo (joint) of 1. axis in ranges [-1, 1]
    00280         (scaled by min1, max1, centered)*/
    00281     virtual double get1(){
    00282       double pos =  joint->getPosition1();
    00283       return 2*(pos-min1)/(max1-min1) - 1;
    00284     }
    00285
    00286
    00287     /** returns the position of the servo (joint) of 2. axis in ranges [-1, 1]
    00288         (scaled by min2, max2, centered)*/
    00289     virtual double get2(){
    00290       double pos =  joint->getPosition2();
    00291       return 2*(pos-min2)/(max2-min2) - 1;
    00292     }
    00293
    00294   };
    00295
    00296
    00297   /** general servo motor to achieve position control for 2 axis joints
    00298    *  that that internally controls the velocity of the motor (much more stable)
    00299    *  with centered zero position
    00300    */
    00301   class TwoAxisServoVel : public TwoAxisServoCentered {
    00302   public:
    00303     /** min and max values are understood as travel bounds.
    00304         The zero position is max-min/2
    00305     */
    00306     TwoAxisServoVel(const OdeHandle& odeHandle,
    00307                     TwoAxisJoint* joint, double _min1, double _max1, double power1,
    00308                     double _min2, double _max2, double power2,
    00309                     double damp=0.1, double maxVel=10.0, double jointLimit = 1.3)
    00310       : TwoAxisServoCentered(joint, _min1, _max1, maxVel/2, _min2, _max2, maxVel/2,
    00311                              damp, 0, 0, jointLimit),
    00312         motor(odeHandle, joint, power1, power2)
    00313     {
    00314     }
    00315     virtual ~TwoAxisServoVel(){}
    00316
    00317     /** adjusts the power of the servo*/
    00318     virtual void setPower(double power1, double power2) {
    00319       motor.setPower(power1,power2);
    00320     };
    00321
    00322     /** returns the power of the servo*/
    00323     virtual void setPower1(double power1) {
    00324       pid1.KP = power1;
    00325     };
    00326
    00327     /** returns the power of the servo*/
    00328     virtual void setPower2(double power2) {
    00329       pid2.KP = power2;
    00330     };
    00331
    00332     /** returns the power of the servo*/
    00333     virtual double getPower1() {
    00334       return motor.getPower();
    00335     };
    00336     /** returns the power of the servo*/
    00337     virtual double getPower2() {
    00338       return motor.getPower2();
    00339     };
    00340
    00341     /** offetCanceling does not exist for this type of servo */
    00342     virtual double& offsetCanceling() {
    00343       dummy=0;
    00344       return dummy;
    00345     };
    00346
    00347     /** adjusts maximal speed of servo*/
    00348     virtual void setMaxVel(double maxVel) {
    00349       this->maxVel = maxVel;
    00350       pid1.KP=maxVel/2;
    00351       pid2.KP=maxVel/2;
    00352     };
    00353     /** adjusts maximal speed of servo*/
    00354     virtual double getMaxVel() {
    00355       return maxVel;
    00356     };
    00357
    00358
    00359     /** sets the set point of the servo.
    00360         Position must be between -1 and 1. It is scaled to fit into min, max,
    00361         however 0 is just in the center of min and max
    00362     */
    00363     virtual void set(double pos1, double pos2){
    00364       pos1 = clip(pos1, -1.0, 1.0);
    00365       pos2 = clip(pos2, -1.0, 1.0);
    00366
    00367       pos1 = (pos1+1)*(max1-min1)/2 + min1;
    00368       pid1.setTargetPosition(pos1);
    00369       double vel1 = pid1.stepNoCutoff(joint->getPosition1(), joint->odeHandle.getTime());
    00370       pos2 = (pos2+1)*(max2-min2)/2 + min2;
    00371       pid2.setTargetPosition(pos2);
    00372       double vel2 = pid2.stepNoCutoff(joint->getPosition2(), joint->odeHandle.getTime());
    00373       motor.set(0, vel1);
    00374       motor.set(1, vel2);
    00375     }
    00376
    00377   protected:
    00378     AngularMotor2Axis motor;
    00379     double dummy;
    00380
    00381   };
    00382
    00383
    00384 }
    00385 #endif