limited_proxy.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <control_toolbox/limited_proxy.h>
00036 #include <algorithm>
00037 #include <cmath>
00038 
00039 namespace control_toolbox {
00040 
00041 
00042 // CONTINUOUS SECOND ORDER PROXY DYNAMICS
00043 //
00044 // Calculate the 2nd order dynamics with which the proxy will
00045 // converge.  In particular, this calculates the acceleration a as a
00046 // function of position p and velocity v:
00047 //
00048 //   a = a(p,v)
00049 //
00050 // It also calculates the partial dervatives da/dp and dq/dv.  The
00051 // parameters are
00052 //
00053 //   lam        Bandwidth of convergence
00054 //   acon       Acceleration available for convergence
00055 //
00056 // The dynamics are split into a local region (small positions) with
00057 // classic linear dynamics and a global region (large positions) with
00058 // nonlinear dynamics.  The regions and nonlinear dynamics are
00059 // designed to allow convergence without overshoot with a limited
00060 // acceleration of acon: the system will converge asymptotically along
00061 // the critical line
00062 //
00063 //   v^2 = 2*acon * ( abs(p) - acon/lam^2 )
00064 //
00065 // which uses a constant acceleration of
00066 //
00067 //   a = - acon * sign(p)
00068 
00069 static void calcDynamics2ndorder(double &a, double &dadp, double &dadv,
00070                                  double p, double v, double lam, double acon)
00071 {
00072   double lam2 = lam*lam;        // Lambda squared
00073 
00074   // Separate 3 regions: large positive, small, and large negative positions
00075   if (lam2*p > 3*acon)
00076   {
00077     // Large position position: Nonlinear dynamics
00078     a    = - 2.0*lam*v - sqrt(8.0*acon*(+lam2*p-acon)) + acon;
00079     dadv = - 2.0*lam;
00080     dadp =             - lam2 * sqrt(2.0*acon/(+lam2*p-acon));
00081   }
00082   else if (lam2*p > -3*acon)
00083   {
00084     // Small position: Use linear dynamics
00085     a    = - 2.0*lam*v - lam2*p;
00086     dadv = - 2.0*lam;
00087     dadp =             - lam2;
00088   }
00089   else
00090   {
00091     // Large negative position: Nonlinear dynamics
00092     a    = - 2.0*lam*v + sqrt(8.0*acon*(-lam2*p-acon)) - acon;
00093     dadv = - 2.0*lam;
00094     dadp =             - lam2 * sqrt(2.0*acon/(-lam2*p-acon));
00095   }
00096 
00097   // Note we don't explicitly limit the acceleration to acon.  First
00098   // such limits are more effectively imposed by force saturations.
00099   // Second, if the proxy starts with very large velocities, higher
00100   // accelerations may be helpful.  The dynamics are simply designed
00101   // not to need more than acon to avoid overshoot.
00102   return;
00103 }
00104 
00105 
00106 
00107 // CONTINUOUS FIRST ORDER PROXY DYNAMICS
00108 //
00109 // Calculate the 1st order dynamics with which the proxy will
00110 // converge.  In particular, this calculates the acceleration a as a
00111 // function of velocity v (no position dependence).
00112 //
00113 //   a = a(v)
00114 //
00115 // It also calculates the partial dervative dq/dv.  The parameters are
00116 //
00117 //   lam        Bandwidth of convergence
00118 //   acon       Acceleration available for convergence
00119 //
00120 // This uses basic linear dynamics, ignoring acon, as no overshoot can
00121 // occur in a first order system.
00122 
00123 static void calcDynamics1storder(double &a, double &dadv,
00124                                  double v, double lam, double acon)
00125 {
00126   a    = - lam*v;
00127   dadv = - lam;
00128 
00129   return;
00130 }
00131 
00132 
00133 // CONTROLLER RESET
00134 //
00135 // Reset the state of the controller.
00136 
00137 void LimitedProxy::reset(double pos_act, double vel_act)
00138 {
00139   // Place the proxy at the actual position, which sets the error to zero.
00140   last_proxy_pos_ = pos_act;
00141   last_proxy_vel_ = vel_act;
00142   last_proxy_acc_ = 0.0;
00143 
00144   last_vel_error_ = 0.0;
00145   last_pos_error_ = 0.0;
00146   last_int_error_ = 0.0;
00147 }
00148 
00149 
00150 // CONTROLLER UPDATE
00151 //
00152 // Adjust the proxy and compute the controller force.  We use a
00153 // trapezoidal integration scheme for maximal stability and best
00154 // accuracy.
00155 
00156 double LimitedProxy::update(double pos_des, double vel_des, double acc_des,
00157                             double pos_act, double vel_act, double dt)
00158 {
00159   // Get the parameters.  This ensures that they can not change during
00160   // the calculations and are non-negative!
00161   double mass = abs(mass_);             // Estimate of the joint mass
00162   double Kd   = abs(Kd_);               // Damping gain
00163   double Kp   = abs(Kp_);               // Position gain
00164   double Ki   = abs(Ki_);               // Integral gain
00165   double Ficl = abs(Ficl_);             // Integral force clamp
00166   double Flim = abs(effort_limit_);     // Limit on output force
00167   double vlim = abs(vel_limit_);        // Limit on velocity
00168   double pmax = pos_upper_limit_;       // Upper position bound
00169   double pmin = pos_lower_limit_;       // Lower position bound
00170   double lam  = abs(lambda_proxy_);     // Bandwidth of proxy reconvergence
00171   double acon = abs(acc_converge_);     // Acceleration of proxy reconvergence
00172 
00173   // For numerical stability, upper bound the bandwidth by 2/dt.
00174   // Note this is safe for dt==0.
00175   if (lam * dt > 2.0)
00176     lam = 2.0/dt;
00177 
00178   // Other useful terms.
00179   double dt2 = dt * dt;                 // Time step squared
00180   double dt3 = dt * dt * dt;            // Time step cubed
00181 
00182   // State values: current and last cycle.
00183   double pos_pxy;                       // Current proxy position
00184   double vel_pxy;                       // Current proxy velocity
00185   double acc_pxy;                       // Current proxy acceleration
00186   double vel_err;                       // Current velocity error
00187   double pos_err;                       // Current position error
00188   double int_err;                       // Current integral error
00189 
00190   double last_pos_pxy = last_proxy_pos_;
00191   double last_vel_pxy = last_proxy_vel_;
00192   double last_acc_pxy = last_proxy_acc_;
00193   double last_vel_err = last_vel_error_;
00194   double last_pos_err = last_pos_error_;
00195   double last_int_err = last_int_error_;
00196 
00197   // Output value
00198   double force;                         // Controller output force
00199 
00200 
00201   // Step 1: Have the proxy track (and reconverge to) the desired
00202   // motion.  We use the bandwidth lambda as a general enable switch:
00203   // Only implement the full proxy behavior if lambda is positive.
00204   // (Would not make sense if the bandwidth is zero..)
00205   if (lam > 0.0)
00206   {
00207     double pnom;                // Nominal position (for lineariztion)
00208     double vnom;                // Nominal velocity (for lineariztion)
00209     double anom;                // Nominal/linearized acceleration value
00210     double dadp;                // Partial derivative w.r.t. position
00211     double dadv;                // Partial derivative w.r.t. velocity
00212 
00213     double acc_hi;              // Upper acceleration boundary value
00214     double acc_lo;              // Lower acceleration boundary value
00215 
00216     // Using trapezoidal integration for the proxy, we need to solve
00217     // the implicit equation for the acceleration.  To do so, we
00218     // have to linearize the equation around zero acceleration.  And
00219     // for this, we need to determine a new (nominal) proxy position
00220     // and velocity assuming zero acceleration.
00221     vnom = last_vel_pxy + dt/2 * (last_acc_pxy + 0.0 );
00222     pnom = last_pos_pxy + dt/2 * (last_vel_pxy + vnom);
00223 
00224     // Calculate the proxy acceleration to track the desired
00225     // trajectory = desired position/velocity/acceleration.
00226     // Appropriate to trapezoidal integration, first compute the
00227     // linearized dynamics at the nominal position/velocity, then
00228     // solve the solve the implicit equation.  Note the partial
00229     // derivatives are negative, so the denominator is guaranteed to
00230     // be positive.
00231     calcDynamics2ndorder(anom, dadp, dadv, pnom-pos_des, vnom-vel_des, lam, acon);
00232     acc_pxy = (acc_des + anom) / (1.0 - dadv*dt/2 - dadp*dt2/4);
00233 
00234     // Limit the new proxy position (if a non-zero position range is
00235     // given).  Calculate the acceleration that would be needed to
00236     // stop at the upper and lower position limits.  To stop in
00237     // time, we should never apply more acceleration than the first
00238     // or less than the second.  Hence saturate the proxy
00239     // acceleration accordingly.
00240 #if 0
00241     // Comment out preferred by Stu to avoid parameters pmin/pmax.
00242     if (pmax - pmin > 0.0)
00243     {
00244       // Upper limit.
00245       calcDynamics2ndorder(anom, dadp, dadv, pnom-pmax, vnom, lam, acon);
00246       acc_hi = anom / (1.0 - dadv*dt/2 - dadp*dt2/4);
00247 
00248       // Lower limit.
00249       calcDynamics2ndorder(anom, dadp, dadv, pnom-pmin, vnom, lam, acon);
00250       acc_lo = anom / (1.0 - dadv*dt/2 - dadp*dt2/4);
00251 
00252       // Saturate between the lower and upper values.
00253       acc_pxy = std::min(std::max(acc_pxy, acc_lo), acc_hi);
00254     }
00255 #endif
00256 
00257     // Limit the new proxy velocity (if a velocity limit is given).
00258     // Calculate the acceleration that would be needed to converge
00259     // to the upper and lower velocity limit.  To avoid the limits,
00260     // we should never apply more than the first or less than the
00261     // second, hence saturate the proxy accordingly.
00262     if (vlim > 0.0)
00263     {
00264       // Upper limit.
00265       calcDynamics1storder(anom, dadv, vnom-vlim, lam, acon);
00266       acc_hi = anom / (1.0 - dadv*dt/2);
00267 
00268       // Lower limit.
00269       calcDynamics1storder(anom, dadv, vnom+vlim, lam, acon);
00270       acc_lo = anom / (1.0 - dadv*dt/2);
00271 
00272       // Saturate between the lower and upper values.
00273       acc_pxy = std::min(std::max(acc_pxy, acc_lo), acc_hi);
00274     }
00275 
00276     // Do not limit the new proxy acceleration to any constant max
00277     // acceleration value.  If the acceleration is high, it may
00278     // cause force saturations which Step 3 detects and corrects.
00279     // As a result, accelerations can be higher for joint
00280     // configurations with low inertia and vice versa.
00281 
00282     // Finally integrate the proxy over the time step using the
00283     // (nonzero) computed proxy acceleration.
00284     vel_pxy = last_vel_pxy + dt/2 * (last_acc_pxy + acc_pxy);
00285     pos_pxy = last_pos_pxy + dt/2 * (last_vel_pxy + vel_pxy);
00286   }
00287   else
00288   {
00289     // The proxy dynamics are turned off, so just set it to track
00290     // the desired exactly.
00291     acc_pxy = acc_des;
00292     vel_pxy = vel_des;
00293     pos_pxy = pos_des;
00294   }
00295 
00296 
00297   // Step 2: Calculate the controller based on the proxy motion.
00298   // First compute the velocity, position, and integral errors.  Note
00299   // we do NOT limit the integration or integral error until after the
00300   // below adjustments!
00301   vel_err = vel_act - vel_pxy;
00302   pos_err = pos_act - pos_pxy;
00303   int_err = last_int_err + dt/2 * (last_pos_err + pos_err);
00304 
00305   // Calculate the controller force.  This includes an acceleration
00306   // feedforward term (so the actual robot will track the proxy as
00307   // best possible), a regular PD, and an integral term which is
00308   // clamped to a maximum value.
00309   force = mass*acc_pxy - Kd*vel_err - Kp*pos_err - std::min(std::max(Ki*int_err, -Ficl), Ficl);
00310 
00311 
00312   // Step 3: If the controller force were to exceed the force limits,
00313   // adjust the proxy to reduce the required force.  This effectively
00314   // drags the proxy with the actual when the controller can not
00315   // create the forces required for tracking.  We use the force
00316   // limit as an enable switch: only adjust if the limit is positive.
00317   // (A force limit of zero would make no sense.)
00318   if (Flim > 0.0)
00319   {
00320     double Fpd;         // PD force from the un-adjusted errors
00321     double Fi;          // Unclamped and un-adjusted integral force
00322 
00323     // Saturate the force to the known force limits.
00324     force = std::min(std::max(force, -Flim), Flim);
00325 
00326     // Calculate the PD force and the unclamped (unsaturated)
00327     // integral force which the un-adjusted proxy would provide.
00328     Fpd = mass*acc_pxy - Kd*vel_err - Kp*pos_err;
00329     Fi  = - Ki*int_err;
00330 
00331     // If the mass is non-zero, calculate an acceleration-based
00332     // proxy adjustment.
00333     if (mass > 0.0)
00334     {
00335       double da;                // Acceleration delta (change)
00336 
00337       // Compute the acceleration delta assuming the integral term
00338       // does not saturate.
00339       da = (force - Fpd - Fi) / (mass + Kd*dt/2 + Kp*dt2/4 + Ki*dt3/8);
00340 
00341       // Check for clamping/saturation on the adjusted integral
00342       // force and re-compute the delta appropriately.  There is
00343       // no need to re-check for clamping after recomputation, as
00344       // the new delta will only increase and can not undo the
00345       // saturation.
00346       if      (Fi+da*Ki*dt3/8 >  Ficl)  da=(force-Fpd-Ficl)/(mass+Kd*dt/2+Kp*dt2/4);
00347       else if (Fi+da*Ki*dt3/8 < -Ficl)  da=(force-Fpd+Ficl)/(mass+Kd*dt/2+Kp*dt2/4);
00348 
00349       // Adjust the acceleration, velocity, position, and integral
00350       // states.
00351       acc_pxy += da;
00352       vel_pxy += da * dt/2;
00353       pos_pxy += da * dt2/4;
00354 
00355       vel_err -= da * dt/2;
00356       pos_err -= da * dt2/4;
00357       int_err -= da * dt3/8;
00358     }
00359 
00360     // If the mass is zero and the damping gain is nonzero, we have
00361     // to adjust the force by shifting the proxy velocity.
00362     else if (Kd > 0.0)
00363     {
00364       double dv;                // Velocity delta (change)
00365 
00366       // Compute the velocity delta assuming the integral term
00367       // does not saturate.
00368       dv = (force - Fpd - Fi) / (Kd + Kp*dt/2 + Ki*dt2/4);
00369 
00370       // Check for clamping/saturation on the adjusted integral
00371       // force and re-compute the delta appropriately.  There is
00372       // no need to re-check for clamping after recomputation, as
00373       // the new delta will only increase and can not undo the
00374       // saturation.
00375       if      (Fi+dv*Ki*dt2/4 >  Ficl)  dv=(force-Fpd-Ficl)/(Kd+Kp*dt/2);
00376       else if (Fi+dv*Ki*dt2/4 < -Ficl)  dv=(force-Fpd+Ficl)/(Kd+Kp*dt/2);
00377 
00378       // Adjust the velocity, position, and integral states.  Do
00379       // not alter the acceleration.
00380       vel_pxy += dv;
00381       pos_pxy += dv * dt/2;
00382 
00383       vel_err -= dv;
00384       pos_err -= dv * dt/2;
00385       int_err -= dv * dt2/4;
00386     }
00387 
00388     // If the mass and damping gain are both zero and the position
00389     // gain is nonzero, we have to adjust the force by shifting the
00390     // proxy position.
00391     else if (Kp > 0.0)
00392     {
00393       double dp;                // Position delta (change)
00394 
00395       // Compute the velocity delta assuming the integral term
00396       // does not saturate.
00397       dp = (force - Fpd - Fi) / (Kp + Ki*dt/2);
00398 
00399       // Check for clamping/saturation on the adjusted integral
00400       // force and re-compute the delta appropriately.  There is
00401       // no need to re-check for clamping after recomputation, as
00402       // the new delta will only increase and can not undo the
00403       // saturation.
00404       if      (Fi+dp*Ki*dt/2 >  Ficl)  dp=(force-Fpd-Ficl)/(Kp);
00405       else if (Fi+dp*Ki*dt/2 < -Ficl)  dp=(force-Fpd+Ficl)/(Kp);
00406 
00407       // Adjust the position and integral states.  Do not alter
00408       // the acceleration or velocity.
00409       pos_pxy += dp;
00410 
00411       pos_err -= dp;
00412       int_err -= dp * dt/2;
00413     }
00414 
00415     // If the mass, damping, and position gain are all zero, there
00416     // isn't much we can do...
00417   }
00418 
00419 
00420   // Step 4: Clean up
00421   // (a) Stop the position error integration (limit the integral error) if
00422   //     the integrator clamp is in effect.  Note this is safe for Ki==0.
00423   if      (Ki * int_err >  Ficl)   int_err =  Ficl / Ki;
00424   else if (Ki * int_err < -Ficl)   int_err = -Ficl / Ki;
00425 
00426   // (b) Remember the state.
00427   last_proxy_pos_ = pos_pxy;
00428   last_proxy_vel_ = vel_pxy;
00429   last_proxy_acc_ = acc_pxy;
00430   last_vel_error_ = vel_err;
00431   last_pos_error_ = pos_err;
00432   last_int_error_ = int_err;
00433 
00434   // (c) Return the controller force.
00435   return(force);
00436 }
00437 
00438 } // namespace


control_toolbox
Author(s): Melonee Wise, Sachin Chitta, John Hsu
autogenerated on Fri Jan 3 2014 11:41:22