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


control_toolbox
Author(s): Melonee Wise, Sachin Chitta, John Hsu
autogenerated on Fri Apr 15 2016 15:39:29