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

# oneaxisservo.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 __SERVO1_H
    00025 #define __SERVO1_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 to achieve position control
    00035    */
    00036   class OneAxisServo {
    00037   public:
    00038     /** min and max values are understood as travel bounds. Min should be less than 0.*/
    00039
    00040     OneAxisServo(OneAxisJoint* joint, double _min, double _max,
    00041                  double power, double damp=0.2, double integration=2, double maxVel=10.0,
    00042                  double jointLimit = 1.3, bool minmaxCheck = true)
    00043       : joint(joint), pid(power, integration, damp), maxVel(maxVel), jointLimit(jointLimit) {
    00044       assert(joint);
    00045       setMinMax(_min,_max);
    00046       assert(min<max);
    00047       assert(!minmaxCheck || min <= 0);
    00048       assert(!minmaxCheck || max >= 0);
    00049       assert(power>=0 && damp >=0 && integration >=0);
    00050     }
    00051
    00052     virtual ~OneAxisServo(){}
    00053
    00054     /** sets the set point of the servo.
    00055         Position must be between -1 and 1. It is scaled to fit into min, max
    00056     */
    00057     virtual void set(double pos){
    00058       pos = clip(pos, -1.0, 1.0);
    00059       if(pos > 0){
    00060         pos *= max;
    00061       }else{
    00062         pos *= -min;
    00063       }
    00064       pid.setTargetPosition(pos);
    00065
    00066       double force = pid.step(joint->getPosition1(), joint->odeHandle.getTime());
    00067       force = std::min(pid.KP, std::max(-pid.KP,force));// limit force to 1*KP
    00068       joint->addForce1(force);
    00069       if(maxVel>0){
    00070         joint->getPart1()->limitLinearVel(maxVel);
    00071         joint->getPart2()->limitLinearVel(maxVel);
    00072       }
    00073     }
    00074
    00075     /** returns the position of the slider in ranges [-1, 1] (scaled by min, max)*/
    00076     virtual double get(){
    00077       double pos =  joint->getPosition1();
    00078       if(pos > 0){
    00079         pos /= max;
    00080       }else{
    00081         pos /= -min;
    00082       }
    00083       return pos;
    00084     }
    00085
    00086     virtual void setMinMax(double _min, double _max){
    00087       min=_min;
    00088       max=_max;
    00089       joint->setParam(dParamLoStop, min  - abs(min) * (jointLimit-1));
    00090       joint->setParam(dParamHiStop, max  + abs(max) * (jointLimit-1));
    00091     }
    00092
    00093     /** adjusts the power of the servo*/
    00094     virtual void setPower(double power) {
    00095       pid.KP = power;
    00096     };
    00097
    00098     /** returns the power of the servo*/
    00099     virtual double getPower() {
    00100       return pid.KP;
    00101     };
    00102
    00103     /** returns the damping of the servo*/
    00104     virtual double getDamping() {
    00105       return pid.KD;
    00106     }
    00107     /** sets the damping of the servo*/
    00108     virtual void setDamping(double damp) {
    00109       pid.KD = damp;
    00110     };
    00111
    00112     /** returns the integration term of the PID controller of the servo*/
    00113     virtual double& offsetCanceling() {
    00114       return pid.KI;
    00115     };
    00116
    00117     /** adjusts maximal speed of servo*/
    00118     virtual void setMaxVel(double maxVel) {
    00119       this->maxVel = maxVel;
    00120     };
    00121     /** adjusts maximal speed of servo*/
    00122     virtual double getMaxVel() {
    00123       return maxVel;
    00124     };
    00125
    00126
    00127   protected:
    00128     OneAxisJoint* joint;
    00129     double min;
    00130     double max;
    00131     PID pid;
    00132     double maxVel;
    00133     double jointLimit; ///< joint limit with respect to servo limit
    00134   };
    00135
    00136   typedef OneAxisServo SliderServo;
    00137   typedef OneAxisServo HingeServo;
    00138   typedef OneAxisServo Hinge2Servo;
    00139
    00140
    00141
    00142   /** general servo motor to achieve position control with zero position centered
    00143    */
    00144   class OneAxisServoCentered : public OneAxisServo {
    00145   public:
    00146     /** min and max values are understood as travel bounds.
    00147         The zero position is (max-min)/2
    00148     */
    00149     OneAxisServoCentered(OneAxisJoint* joint, double _min, double _max,
    00150                          double power, double damp=0.2, double integration=2,
    00151                          double maxVel=10.0, double jointLimit = 1.3)
    00152       : OneAxisServo(joint, _min, _max, power, damp, integration, maxVel, jointLimit, false){
    00153     }
    00154     virtual ~OneAxisServoCentered(){}
    00155
    00156     /** sets the set point of the servo.
    00157         Position must be between -1 and 1. It is scaled to fit into min, max,
    00158         however 0 is just in the center of min and max
    00159     */
    00160     virtual void set(double pos){
    00161       pos = clip(pos, -1.0, 1.0);
    00162       pos = (pos+1)*(max-min)/2 + min;
    00163
    00164       pid.setTargetPosition(pos);
    00165       double force = pid.stepNoCutoff(joint->getPosition1(), joint->odeHandle.getTime());
    00166       force = clip(force,-10*pid.KP, 10*pid.KP); // limit force to 10*KP
    00167       joint->addForce1(force);
    00168       if(maxVel>0){
    00169         joint->getPart1()->limitLinearVel(maxVel);
    00170         joint->getPart2()->limitLinearVel(maxVel);
    00171       }
    00172     }
    00173     /** returns the position of the slider in ranges [-1, 1] (scaled by min, max, centered)*/
    00174     virtual double get(){
    00175       double pos =  joint->getPosition1();
    00176
    00177       return 2*(pos-min)/(max-min) - 1;
    00178     }
    00179
    00180   };
    00181
    00182   /** general servo motor to achieve position control.
    00183    *  It internally controls the velocity of the motor (much more stable)
    00184    *  with centered zero position.
    00185    *  The amount of body feeling can be adjusted by the damping parameter
    00186    *   which is understood as a stiffness parameter
    00187    */
    00188   class OneAxisServoVel : public OneAxisServo {
    00189   public:
    00190     /** min and max values are understood as travel bounds.
    00191         The zero position is (max-min)/2
    00192         @param power is the maximal torque the servo can generate
    00193         @param maxVel is understood as a speed parameter of the servo.
    00194         @param damp adjusts the power of the servo in dependence of the distance
    00195          to the set point. This regulates the stiffness and the body feeling
    00196           0: the servo has no power at the set point (maximal body feeling);
    00197           1: is servo has full power at the set point: perfectly damped.
    00198
    00199     */
    00200     OneAxisServoVel(const OdeHandle& odeHandle,
    00201                     OneAxisJoint* joint, double _min, double _max,
    00202                     double power, double damp=0.05, double maxVel=20,
    00203                     double jointLimit = 1.3)
    00204       : OneAxisServo(joint, _min, _max, maxVel/2, 0, 0, 0, jointLimit, false),
    00205         // don't wonder! It is correct to give maxVel as a power parameter to the parent.
    00206         motor(odeHandle, joint, power), power(power), damp(clip(damp,0.0,1.0))
    00207     {
    00208     }
    00209
    00210     virtual ~OneAxisServoVel(){}
    00211
    00212     /** adjusts the power of the servo*/
    00213     virtual void setPower(double _power) {
    00214       power=_power;
    00215       motor.setPower(power);
    00216     };
    00217     /** returns the power of the servo*/
    00218     virtual double getPower() {
    00219       return power;
    00220     };
    00221     virtual double getDamping() {
    00222       return damp;
    00223     };
    00224     virtual void setDamping(double _damp) {
    00225       damp = clip(_damp,0.0,1.0);
    00226     };
    00227     /** offetCanceling does not exist for this type of servo */
    00228     virtual double& offsetCanceling() {
    00229       dummy=0;
    00230       return dummy;
    00231     };
    00232
    00233     /** adjusts maximal speed of servo*/
    00234     virtual void setMaxVel(double maxVel) {
    00235       this->maxVel = maxVel;
    00236       pid.KP=maxVel/2;
    00237     };
    00238     /** adjusts maximal speed of servo*/
    00239     virtual double getMaxVel() {
    00240       return maxVel;
    00241     };
    00242
    00243
    00244     /** sets the set point of the servo.
    00245         Position must be between -1 and 1. It is scaled to fit into min, max,
    00246         however 0 is just in the center of min and max
    00247     */
    00248     virtual void set(double pos){
    00249       pos = clip(pos, -1.0, 1.0);
    00250       pos = (pos+1)*(max-min)/2 + min;
    00251       pid.setTargetPosition(pos);
    00252       double vel = pid.stepVelocity(joint->getPosition1(), joint->odeHandle.getTime());
    00253       double e   = fabs(2.0*(pid.error)/(max-min)); // distance from set point
    00254       motor.set(0, vel);
    00255       // calculate power of servo depending on the damping and distance from set point and
    00256       // sigmoid ramping of power for damping < 1
    00257       //      motor.setPower(((1.0-damp)*tanh(e)+damp) * power);
    00258       motor.setPower(tanh(e+damp) * power);
    00259     }
    00260
    00261     /** returns the position of the servo in ranges [-1, 1] (scaled by min, max, centered)*/
    00262     virtual double get(){
    00263       double pos =  joint->getPosition1();
    00264       return 2*(pos-min)/(max-min) - 1;
    00265     }
    00266   protected:
    00267     AngularMotor1Axis motor;
    00268     double dummy;
    00269     double power;
    00270     double damp;
    00271   };
    00272
    00273 }
    00274 #endif