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

# twoaxisservo.h

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005-2011 LpzRobots development team                    *
    00003  *    Georg Martius  <georg dot martius at web dot de>                     *
    00004  *    Frank Guettler <guettler at informatik dot uni-leipzig dot de        *
    00005  *    Frank Hesse    <frank at nld dot ds dot mpg dot de>                  *
    00006  *    Ralf Der       <ralfder at mis dot mpg dot de>                       *
    00007  *                                                                         *
    00008  *   This program is free software; you can redistribute it and/or modify  *
    00009  *   it under the terms of the GNU General Public License as published by  *
    00010  *   the Free Software Foundation; either version 2 of the License, or     *
    00011  *   (at your option) any later version.                                   *
    00012  *                                                                         *
    00013  *   This program is distributed in the hope that it will be useful,       *
    00014  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00015  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00016  *   GNU General Public License for more details.                          *
    00017  *                                                                         *
    00018  *   You should have received a copy of the GNU General Public License     *
    00019  *   along with this program; if not, write to the                         *
    00020  *   Free Software Foundation, Inc.,                                       *
    00021  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00022  *                                                                         *
    00023  ***************************************************************************/
    00024 #ifndef __SERVO2_H
    00025 #define __SERVO2_H
    00026
    00027 #include "joint.h"
    00028 #include "pid.h"
    00029 #include "angularmotor.h"
    00030 #include <selforg/controller_misc.h>
    00031
    00032 namespace lpzrobots {
    00033
    00034   /** general servo motor for 2 axis joints
    00035    */
    00036   class TwoAxisServo {
    00037   public:
    00038     /** min and max values are understood as travel bounds. Min should be less than 0.*/
    00039
    00040     TwoAxisServo(TwoAxisJoint* joint, double _min1, double _max1, double power1,
    00041                  double _min2, double _max2, double power2,
    00042                  double damp=0.2, double integration=2, double maxVel=10.0,
    00043                  double jointLimit = 1.3, bool minmaxCheck=true)
    00044       : joint(joint),
    00045         pid1(power1, integration, damp),
    00046         pid2(power2, integration, damp),
    00047         maxVel(maxVel), jointLimit(jointLimit) {
    00048       assert(joint);
    00049       setMinMax1(_min1,_max1);
    00050       setMinMax2(_min2,_max2);
    00051       assert(min1<max1); assert(min2<max2);
    00052       assert(!minmaxCheck || min1 <= 0); assert(!minmaxCheck || min2 <= 0);
    00053       assert(!minmaxCheck || max1 >= 0); assert(!minmaxCheck || max2 >= 0);
    00054       assert(power1 >=0 && power2 >=0 && damp >=0 && integration >=0);
    00055     }
    00056     virtual ~TwoAxisServo(){}
    00057
    00058     /** sets the set point of the servo.
    00059         Position must be between -1 and 1. It is scaled to fit into min, max
    00060     */
    00061     virtual void set(double pos1, double pos2){
    00062       if(pos1 > 0){
    00063         pos1 *= max1;
    00064       }else{
    00065         pos1 *= -min1;
    00066       }
    00067       pid1.setTargetPosition(pos1);
    00068       // double force1 = pid1.stepWithD(joint->getPosition1(), joint->getPosition1Rate());
    00069       double force1 = pid1.step(joint->getPosition1(), joint->odeHandle.getTime());
    00070       // limit force to 1*KP
    00071       force1 = std::min(pid1.KP, std::max(-pid1.KP,force1));// limit force to 1*KP
    00072
    00073       if(pos2 > 0){
    00074         pos2 *= max2;
    00075       }else{
    00076         pos2 *= -min2;
    00077       }
    00078       pid2.setTargetPosition(pos2);
    00079       //      double force2 = pid2.stepWithD(joint->getPosition2(), joint->getPosition2Rate());
    00080       double force2 = pid2.step(joint->getPosition2(), joint->odeHandle.getTime());
    00081       // limit force to 1*KP
    00082       force2 = std::min(pid2.KP, std::max(-pid2.KP,force2));// limit force to 1*KP
    00083       joint->addForces(force1, force2);
    00084       if(maxVel >0 ){
    00085         joint->getPart1()->limitLinearVel(maxVel);
    00086         joint->getPart2()->limitLinearVel(maxVel);
    00087       }
    00088     }
    00089
    00090     /** returns the position of the servo (joint) of 1. axis in ranges [-1, 1] (scaled by min1, max1)*/
    00091     virtual double get1(){
    00092       double pos =  joint->getPosition1();
    00093       if(pos > 0){
    00094         pos /= max1;
    00095       }else{
    00096         pos /= -min1;
    00097       }
    00098       return pos;
    00099     }
    00100
    00101     /** returns the position of the servo (joint) of 2. axis in ranges [-1, 1] (scaled by min2, max2)*/
    00102     virtual double get2(){
    00103       double pos =  joint->getPosition2();
    00104       if(pos > 0){
    00105         pos /= max2;
    00106       }else{
    00107         pos /= -min2;
    00108       }
    00109       return pos;
    00110     }
    00111
    00112     /** returns the positions of the joint in ranges [-1, 1] (scaled by min, max)*/
    00113     void get(double& p1, double& p2){
    00114       p1=get1();
    00115       p2=get2();
    00116     }
    00117
    00118
    00119     /** adjusts the power of the servo*/
    00120     virtual void setPower(double power1, double power2) {
    00121       pid1.KP = power1;
    00122       pid2.KP = power2;
    00123     };
    00124
    00125     /** returns the power of the servo*/
    00126     virtual void setPower1(double power1) {
    00127       pid1.KP = power1;
    00128     };
    00129
    00130     /** returns the power of the servo*/
    00131     virtual void setPower2(double power2) {
    00132       pid2.KP = power2;
    00133     };
    00134
    00135     /** returns the power of the servo*/
    00136     virtual double getPower1() {
    00137       return pid1.KP;
    00138     };
    00139     /** returns the power of the servo*/
    00140     virtual double getPower2() {
    00141       return pid2.KP;
    00142     };
    00143
    00144     /** returns the damping of the servo (axis 1) */
    00145     virtual double getDamping1() {
    00146       return pid1.KD;
    00147     };
    00148
    00149     /** returns the damping of the servo (axis 2) */
    00150     virtual double getDamping2() {
    00151       return pid2.KD;
    00152     };
    00153
    00154     /** sets the damping of the servo (axis 1) */
    00155     virtual void setDamping1(double damp) {
    00156       pid1.KD = damp;
    00157     };
    00158
    00159     /** sets the damping of the servo (axis 1) */
    00160     virtual void setDamping2(double damp) {
    00161       pid2.KD = damp;
    00162     };
    00163
    00164     /** sets the damping of the servo (both axis) */
    00165     virtual void setDamping(double _damp) {
    00166       setDamping1(_damp);
    00167       setDamping2(_damp);
    00168     };
    00169
    00170     /** returns the damping of the servo*/
    00171     virtual double& offsetCanceling() {
    00172       return pid1.KI;
    00173     };
    00174
    00175     virtual void setMinMax1(double _min, double _max){
    00176       min1=_min;
    00177       max1=_max;
    00178       joint->setParam(dParamLoStop, _min  - abs(_min) * (jointLimit-1));
    00179       joint->setParam(dParamHiStop, _max  + abs(_max) * (jointLimit-1));
    00180     }
    00181
    00182     virtual void setMinMax2(double _min, double _max){
    00183       min2=_min;
    00184       max2=_max;
    00185       joint->setParam(dParamLoStop2, _min  - abs(_min) * (jointLimit-1));
    00186       joint->setParam(dParamHiStop2, _max  + abs(_max) * (jointLimit-1));
    00187     }
    00188
    00189     /** adjusts maximal speed of servo*/
    00190     virtual void setMaxVel(double maxVel) {
    00191       this->maxVel = maxVel;
    00192     };
    00193     /** adjusts maximal speed of servo*/
    00194     virtual double getMaxVel() {
    00195       return maxVel;
    00196     };
    00197
    00198
    00199   protected:
    00200     TwoAxisJoint* joint;
    00201     double min1;
    00202     double max1;
    00203     double min2;
    00204     double max2;
    00205     PID pid1;
    00206     PID pid2;
    00207     double maxVel;
    00208     double jointLimit;
    00209   };
    00210
    00211   typedef TwoAxisServo UniversalServo;
    00212
    00213
    00214   /** general servo motor for 2 axis joints with zero position centered
    00215    */
    00216   class TwoAxisServoCentered : public TwoAxisServo {
    00217   public:
    00218     /** min and max values are understood as travel bounds.
    00219         The zero position is (max-min)/2
    00220     */
    00221     TwoAxisServoCentered(TwoAxisJoint* joint, double _min1, double _max1, double power1,
    00222                          double _min2, double _max2, double power2,
    00223                          double damp=0.2, double integration=2, double maxVel=10.0,
    00224                          double jointLimit = 1.3)
    00225       : TwoAxisServo(joint, _min1, _max1, power1, _min2, _max2, power2,
    00226                      damp, integration, maxVel, jointLimit, false){
    00227     }
    00228     virtual ~TwoAxisServoCentered(){}
    00229
    00230     /** sets the set point of the servo.
    00231         Position must be between -1 and 1. It is scaled to fit into min, max,
    00232         however 0 is just in the center of min and max
    00233     */
    00234     virtual void set(double pos1, double pos2){
    00235       pos1 = clip(pos1, -1.0, 1.0);
    00236       pos2 = clip(pos2, -1.0, 1.0);
    00237       pos1 = (pos1+1)*(max1-min1)/2 + min1;
    00238
    00239       pid1.setTargetPosition(pos1);
    00240       double force1 = pid1.stepNoCutoff(joint->getPosition1(), joint->odeHandle.getTime());
    00241       // limit force to 10*KP
    00242       force1 = clip(force1,-10*pid1.KP, 10*pid1.KP);
    00243
    00244       pos2 = (pos2+1)*(max2-min2)/2 + min2;
    00245       pid2.setTargetPosition(pos2);
    00246       double force2 = pid2.stepNoCutoff(joint->getPosition2(), joint->odeHandle.getTime());
    00247       // limit force to 10*KP
    00248       force2 = clip(force2,-10*pid2.KP, 10*pid2.KP);
    00249       joint->addForces(force1, force2);
    00250       if(maxVel >0 ){
    00251         joint->getPart1()->limitLinearVel(maxVel);
    00252         joint->getPart2()->limitLinearVel(maxVel);
    00253       }
    00254     }
    00255
    00256     /** returns the position of the servo (joint) of 1. axis in ranges [-1, 1]
    00257         (scaled by min1, max1, centered)*/
    00258     virtual double get1(){
    00259       double pos =  joint->getPosition1();
    00260       return 2*(pos-min1)/(max1-min1) - 1;
    00261     }
    00262
    00263
    00264     /** returns the position of the servo (joint) of 2. axis in ranges [-1, 1]
    00265         (scaled by min2, max2, centered)*/
    00266     virtual double get2(){
    00267       double pos =  joint->getPosition2();
    00268       return 2*(pos-min2)/(max2-min2) - 1;
    00269     }
    00270
    00271   };
    00272
    00273
    00274   /** general servo motor to achieve position control for 2 axis joints
    00275    *  that internally controls the velocity of the motor (much more stable)
    00276    *  with centered zero position.
    00277    *  The amount of body feeling can be adjusted by the damping parameter
    00278    *   which is understood as a stiffness parameter
    00279    */
    00280   class TwoAxisServoVel : public TwoAxisServoCentered {
    00281   public:
    00282     /** min and max values are understood as travel bounds.
    00283         The zero position is (max-min)/2
    00284         @param power is the maximal torque the servo can generate
    00285         @param maxVel is understood as a speed parameter of the servo.
    00286         @param damp adjusts the power of the servo in dependence of the distance
    00287          to the set point. This regulates the stiffness and the body feeling
    00288           0: the servo has no power at the set point (maximal body feeling);
    00289           1: is servo has full power at the set point: perfectly damped and stiff.
    00290     */
    00291     TwoAxisServoVel(const OdeHandle& odeHandle,
    00292                     TwoAxisJoint* joint, double _min1, double _max1, double power1,
    00293                     double _min2, double _max2, double power2,
    00294                     double damp=0.05, double maxVel=10.0, double jointLimit = 1.3)
    00295       : TwoAxisServoCentered(joint, _min1, _max1, maxVel/2, _min2, _max2, maxVel/2,
    00296                              0, 0, 0, jointLimit),
    00297         // don't wonder! It is correct to give maxVel as a power parameter to the normal
    00298         //  servo (PID).
    00299         motor(odeHandle, joint, power1, power2),
    00300         damp(clip(damp,0.0,1.0)), power1(power1), power2(power2)
    00301     {
    00302     }
    00303     virtual ~TwoAxisServoVel(){}
    00304
    00305     virtual void setPower(double _power1, double _power2) {
    00306       motor.setPower(_power1,_power2);
    00307       power1=_power1;
    00308       power2=_power2;
    00309     };
    00310     virtual void setPower1(double _power1) {
    00311       power1=_power1;
    00312       motor.setPower(power1,motor.getPower2());
    00313     };
    00314     virtual void setPower2(double _power2) {
    00315       power2=_power2;
    00316       motor.setPower(motor.getPower(),power2);
    00317     };
    00318     virtual double getPower1() {
    00319       return power1;
    00320     };
    00321     virtual double getPower2() {
    00322       return power2;
    00323     };
    00324
    00325     virtual double getDamping1() {
    00326       return damp;
    00327     };
    00328     virtual double getDamping2() {
    00329       return damp;
    00330     };
    00331     virtual void setDamping1(double _damp) {
    00332       damp = clip(_damp,0.0,1.0);
    00333     };
    00334     virtual void setDamping2(double _damp) {
    00335       setDamping1(_damp);
    00336     };
    00337
    00338     /** offetCanceling does not exist for this type of servo */
    00339     virtual double& offsetCanceling() {
    00340       dummy=0;
    00341       return dummy;
    00342     };
    00343
    00344     /** adjusts maximal speed of servo*/
    00345     virtual void setMaxVel(double maxVel) {
    00346       this->maxVel = maxVel;
    00347       pid1.KP=maxVel/2;
    00348       pid2.KP=maxVel/2;
    00349     };
    00350     /** adjusts maximal speed of servo*/
    00351     virtual double getMaxVel() {
    00352       return maxVel;
    00353     };
    00354
    00355
    00356     /** sets the set point of the servo.
    00357         Position must be between -1 and 1. It is scaled to fit into min, max,
    00358         however 0 is just in the center of min and max
    00359     */
    00360     virtual void set(double pos1, double pos2){
    00361       pos1 = clip(pos1, -1.0, 1.0);
    00362       pos2 = clip(pos2, -1.0, 1.0);
    00363
    00364       pos1 = (pos1+1.0)*(max1-min1)/2.0 + min1;
    00365       pid1.setTargetPosition(pos1);
    00366       double vel1 = pid1.stepVelocity(joint->getPosition1(), joint->odeHandle.getTime());
    00367       double e1 = fabs(2.0*(pid1.error)/(max1-min1)); // distance from set point
    00368
    00369       pos2 = (pos2+1.0)*(max2-min2)/2.0 + min2;
    00370       pid2.setTargetPosition(pos2);
    00371       double vel2 = pid2.stepVelocity(joint->getPosition2(), joint->odeHandle.getTime());
    00372       double e2 = fabs(2.0*(pid2.error)/(max2-min2)); // distance from set point
    00373
    00374       motor.set(0, vel1);
    00375       motor.set(1, vel2);
    00376       // calculate power of servo depending on distance from setpoint and damping
    00377       // sigmoid ramping of power for damping < 1
    00378       //      motor.setPower(((1.0-damp)*tanh(e1)+damp) * power1,
    00379       //                     ((1.0-damp)*tanh(e2)+damp) * power2);
    00380       motor.setPower(tanh(e1+damp) * power1,
    00381                      tanh(e2+damp) * power2);
    00382
    00383     }
    00384
    00385   protected:
    00386     AngularMotor2Axis motor;
    00387     double dummy;
    00388     double damp;
    00389     double power1;
    00390     double power2;
    00391   };
    00392
    00393
    00394 }
    00395 #endif