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