limited_proxy.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
36 #include <algorithm>
37 #include <cmath>
38 #include <cstdlib>
39 
40 namespace control_toolbox {
41 
42 
43 // CONTINUOUS SECOND ORDER PROXY DYNAMICS
44 //
45 // Calculate the 2nd order dynamics with which the proxy will
46 // converge. In particular, this calculates the acceleration a as a
47 // function of position p and velocity v:
48 //
49 // a = a(p,v)
50 //
51 // It also calculates the partial dervatives da/dp and dq/dv. The
52 // parameters are
53 //
54 // lam Bandwidth of convergence
55 // acon Acceleration available for convergence
56 //
57 // The dynamics are split into a local region (small positions) with
58 // classic linear dynamics and a global region (large positions) with
59 // nonlinear dynamics. The regions and nonlinear dynamics are
60 // designed to allow convergence without overshoot with a limited
61 // acceleration of acon: the system will converge asymptotically along
62 // the critical line
63 //
64 // v^2 = 2*acon * ( abs(p) - acon/lam^2 )
65 //
66 // which uses a constant acceleration of
67 //
68 // a = - acon * sign(p)
69 
70 static void calcDynamics2ndorder(double &a, double &dadp, double &dadv,
71  double p, double v, double lam, double acon)
72 {
73  double lam2 = lam*lam; // Lambda squared
74 
75  // Separate 3 regions: large positive, small, and large negative positions
76  if (lam2*p > 3*acon)
77  {
78  // Large position position: Nonlinear dynamics
79  a = - 2.0*lam*v - sqrt(8.0*acon*(+lam2*p-acon)) + acon;
80  dadv = - 2.0*lam;
81  dadp = - lam2 * sqrt(2.0*acon/(+lam2*p-acon));
82  }
83  else if (lam2*p > -3*acon)
84  {
85  // Small position: Use linear dynamics
86  a = - 2.0*lam*v - lam2*p;
87  dadv = - 2.0*lam;
88  dadp = - lam2;
89  }
90  else
91  {
92  // Large negative position: Nonlinear dynamics
93  a = - 2.0*lam*v + sqrt(8.0*acon*(-lam2*p-acon)) - acon;
94  dadv = - 2.0*lam;
95  dadp = - lam2 * sqrt(2.0*acon/(-lam2*p-acon));
96  }
97 
98  // Note we don't explicitly limit the acceleration to acon. First
99  // such limits are more effectively imposed by force saturations.
100  // Second, if the proxy starts with very large velocities, higher
101  // accelerations may be helpful. The dynamics are simply designed
102  // not to need more than acon to avoid overshoot.
103  return;
104 }
105 
106 
107 
108 // CONTINUOUS FIRST ORDER PROXY DYNAMICS
109 //
110 // Calculate the 1st order dynamics with which the proxy will
111 // converge. In particular, this calculates the acceleration a as a
112 // function of velocity v (no position dependence).
113 //
114 // a = a(v)
115 //
116 // It also calculates the partial dervative dq/dv. The parameters are
117 //
118 // lam Bandwidth of convergence
119 // acon Acceleration available for convergence
120 //
121 // This uses basic linear dynamics, ignoring acon, as no overshoot can
122 // occur in a first order system.
123 
124 static void calcDynamics1storder(double &a, double &dadv,
125  double v, double lam, double /*acon*/)
126 {
127  a = - lam*v;
128  dadv = - lam;
129 
130  return;
131 }
132 
133 
134 // CONTROLLER RESET
135 //
136 // Reset the state of the controller.
137 
138 void LimitedProxy::reset(double pos_act, double vel_act)
139 {
140  // Place the proxy at the actual position, which sets the error to zero.
141  last_proxy_pos_ = pos_act;
142  last_proxy_vel_ = vel_act;
143  last_proxy_acc_ = 0.0;
144 
145  last_vel_error_ = 0.0;
146  last_pos_error_ = 0.0;
147  last_int_error_ = 0.0;
148 }
149 
150 
151 // CONTROLLER UPDATE
152 //
153 // Adjust the proxy and compute the controller force. We use a
154 // trapezoidal integration scheme for maximal stability and best
155 // accuracy.
156 
157 double LimitedProxy::update(double pos_des, double vel_des, double acc_des,
158  double pos_act, double vel_act, double dt)
159 {
160  // Get the parameters. This ensures that they can not change during
161  // the calculations and are non-negative!
162  double mass = abs(mass_); // Estimate of the joint mass
163  double Kd = abs(Kd_); // Damping gain
164  double Kp = abs(Kp_); // Position gain
165  double Ki = abs(Ki_); // Integral gain
166  double Ficl = abs(Ficl_); // Integral force clamp
167  double Flim = abs(effort_limit_); // Limit on output force
168  double vlim = abs(vel_limit_); // Limit on velocity
169  //double pmax = pos_upper_limit_; // Upper position bound. NOTE: Unused
170  //double pmin = pos_lower_limit_; // Lower position bound. NOTE: Unused
171  double lam = abs(lambda_proxy_); // Bandwidth of proxy reconvergence
172  double acon = abs(acc_converge_); // Acceleration of proxy reconvergence
173 
174  // For numerical stability, upper bound the bandwidth by 2/dt.
175  // Note this is safe for dt==0.
176  if (lam * dt > 2.0)
177  lam = 2.0/dt;
178 
179  // Other useful terms.
180  double dt2 = dt * dt; // Time step squared
181  double dt3 = dt * dt * dt; // Time step cubed
182 
183  // State values: current and last cycle.
184  double pos_pxy; // Current proxy position
185  double vel_pxy; // Current proxy velocity
186  double acc_pxy; // Current proxy acceleration
187  double vel_err; // Current velocity error
188  double pos_err; // Current position error
189  double int_err; // Current integral error
190 
191  double last_pos_pxy = last_proxy_pos_;
192  double last_vel_pxy = last_proxy_vel_;
193  double last_acc_pxy = last_proxy_acc_;
194  //double last_vel_err = last_vel_error_;. NOTE: Unused
195  double last_pos_err = last_pos_error_;
196  double last_int_err = last_int_error_;
197 
198  // Output value
199  double force; // Controller output force
200 
201 
202  // Step 1: Have the proxy track (and reconverge to) the desired
203  // motion. We use the bandwidth lambda as a general enable switch:
204  // Only implement the full proxy behavior if lambda is positive.
205  // (Would not make sense if the bandwidth is zero..)
206  if (lam > 0.0)
207  {
208  double pnom; // Nominal position (for lineariztion)
209  double vnom; // Nominal velocity (for lineariztion)
210  double anom; // Nominal/linearized acceleration value
211  double dadp; // Partial derivative w.r.t. position
212  double dadv; // Partial derivative w.r.t. velocity
213 
214  double acc_hi; // Upper acceleration boundary value
215  double acc_lo; // Lower acceleration boundary value
216 
217  // Using trapezoidal integration for the proxy, we need to solve
218  // the implicit equation for the acceleration. To do so, we
219  // have to linearize the equation around zero acceleration. And
220  // for this, we need to determine a new (nominal) proxy position
221  // and velocity assuming zero acceleration.
222  vnom = last_vel_pxy + dt/2 * (last_acc_pxy + 0.0 );
223  pnom = last_pos_pxy + dt/2 * (last_vel_pxy + vnom);
224 
225  // Calculate the proxy acceleration to track the desired
226  // trajectory = desired position/velocity/acceleration.
227  // Appropriate to trapezoidal integration, first compute the
228  // linearized dynamics at the nominal position/velocity, then
229  // solve the solve the implicit equation. Note the partial
230  // derivatives are negative, so the denominator is guaranteed to
231  // be positive.
232  calcDynamics2ndorder(anom, dadp, dadv, pnom-pos_des, vnom-vel_des, lam, acon);
233  acc_pxy = (acc_des + anom) / (1.0 - dadv*dt/2 - dadp*dt2/4);
234 
235  // Limit the new proxy position (if a non-zero position range is
236  // given). Calculate the acceleration that would be needed to
237  // stop at the upper and lower position limits. To stop in
238  // time, we should never apply more acceleration than the first
239  // or less than the second. Hence saturate the proxy
240  // acceleration accordingly.
241 #if 0
242  // Comment out preferred by Stu to avoid parameters pmin/pmax.
243  if (pmax - pmin > 0.0)
244  {
245  // Upper limit.
246  calcDynamics2ndorder(anom, dadp, dadv, pnom-pmax, vnom, lam, acon);
247  acc_hi = anom / (1.0 - dadv*dt/2 - dadp*dt2/4);
248 
249  // Lower limit.
250  calcDynamics2ndorder(anom, dadp, dadv, pnom-pmin, vnom, lam, acon);
251  acc_lo = anom / (1.0 - dadv*dt/2 - dadp*dt2/4);
252 
253  // Saturate between the lower and upper values.
254  acc_pxy = std::min(std::max(acc_pxy, acc_lo), acc_hi);
255  }
256 #endif
257 
258  // Limit the new proxy velocity (if a velocity limit is given).
259  // Calculate the acceleration that would be needed to converge
260  // to the upper and lower velocity limit. To avoid the limits,
261  // we should never apply more than the first or less than the
262  // second, hence saturate the proxy accordingly.
263  if (vlim > 0.0)
264  {
265  // Upper limit.
266  calcDynamics1storder(anom, dadv, vnom-vlim, lam, acon);
267  acc_hi = anom / (1.0 - dadv*dt/2);
268 
269  // Lower limit.
270  calcDynamics1storder(anom, dadv, vnom+vlim, lam, acon);
271  acc_lo = anom / (1.0 - dadv*dt/2);
272 
273  // Saturate between the lower and upper values.
274  acc_pxy = std::min(std::max(acc_pxy, acc_lo), acc_hi);
275  }
276 
277  // Do not limit the new proxy acceleration to any constant max
278  // acceleration value. If the acceleration is high, it may
279  // cause force saturations which Step 3 detects and corrects.
280  // As a result, accelerations can be higher for joint
281  // configurations with low inertia and vice versa.
282 
283  // Finally integrate the proxy over the time step using the
284  // (nonzero) computed proxy acceleration.
285  vel_pxy = last_vel_pxy + dt/2 * (last_acc_pxy + acc_pxy);
286  pos_pxy = last_pos_pxy + dt/2 * (last_vel_pxy + vel_pxy);
287  }
288  else
289  {
290  // The proxy dynamics are turned off, so just set it to track
291  // the desired exactly.
292  acc_pxy = acc_des;
293  vel_pxy = vel_des;
294  pos_pxy = pos_des;
295  }
296 
297 
298  // Step 2: Calculate the controller based on the proxy motion.
299  // First compute the velocity, position, and integral errors. Note
300  // we do NOT limit the integration or integral error until after the
301  // below adjustments!
302  vel_err = vel_act - vel_pxy;
303  pos_err = pos_act - pos_pxy;
304  int_err = last_int_err + dt/2 * (last_pos_err + pos_err);
305 
306  // Calculate the controller force. This includes an acceleration
307  // feedforward term (so the actual robot will track the proxy as
308  // best possible), a regular PD, and an integral term which is
309  // clamped to a maximum value.
310  force = mass*acc_pxy - Kd*vel_err - Kp*pos_err - std::min(std::max(Ki*int_err, -Ficl), Ficl);
311 
312 
313  // Step 3: If the controller force were to exceed the force limits,
314  // adjust the proxy to reduce the required force. This effectively
315  // drags the proxy with the actual when the controller can not
316  // create the forces required for tracking. We use the force
317  // limit as an enable switch: only adjust if the limit is positive.
318  // (A force limit of zero would make no sense.)
319  if (Flim > 0.0)
320  {
321  double Fpd; // PD force from the un-adjusted errors
322  double Fi; // Unclamped and un-adjusted integral force
323 
324  // Saturate the force to the known force limits.
325  force = std::min(std::max(force, -Flim), Flim);
326 
327  // Calculate the PD force and the unclamped (unsaturated)
328  // integral force which the un-adjusted proxy would provide.
329  Fpd = mass*acc_pxy - Kd*vel_err - Kp*pos_err;
330  Fi = - Ki*int_err;
331 
332  // If the mass is non-zero, calculate an acceleration-based
333  // proxy adjustment.
334  if (mass > 0.0)
335  {
336  double da; // Acceleration delta (change)
337 
338  // Compute the acceleration delta assuming the integral term
339  // does not saturate.
340  da = (force - Fpd - Fi) / (mass + Kd*dt/2 + Kp*dt2/4 + Ki*dt3/8);
341 
342  // Check for clamping/saturation on the adjusted integral
343  // force and re-compute the delta appropriately. There is
344  // no need to re-check for clamping after recomputation, as
345  // the new delta will only increase and can not undo the
346  // saturation.
347  if (Fi+da*Ki*dt3/8 > Ficl) da=(force-Fpd-Ficl)/(mass+Kd*dt/2+Kp*dt2/4);
348  else if (Fi+da*Ki*dt3/8 < -Ficl) da=(force-Fpd+Ficl)/(mass+Kd*dt/2+Kp*dt2/4);
349 
350  // Adjust the acceleration, velocity, position, and integral
351  // states.
352  acc_pxy += da;
353  vel_pxy += da * dt/2;
354  pos_pxy += da * dt2/4;
355 
356  vel_err -= da * dt/2;
357  pos_err -= da * dt2/4;
358  int_err -= da * dt3/8;
359  }
360 
361  // If the mass is zero and the damping gain is nonzero, we have
362  // to adjust the force by shifting the proxy velocity.
363  else if (Kd > 0.0)
364  {
365  double dv; // Velocity delta (change)
366 
367  // Compute the velocity delta assuming the integral term
368  // does not saturate.
369  dv = (force - Fpd - Fi) / (Kd + Kp*dt/2 + Ki*dt2/4);
370 
371  // Check for clamping/saturation on the adjusted integral
372  // force and re-compute the delta appropriately. There is
373  // no need to re-check for clamping after recomputation, as
374  // the new delta will only increase and can not undo the
375  // saturation.
376  if (Fi+dv*Ki*dt2/4 > Ficl) dv=(force-Fpd-Ficl)/(Kd+Kp*dt/2);
377  else if (Fi+dv*Ki*dt2/4 < -Ficl) dv=(force-Fpd+Ficl)/(Kd+Kp*dt/2);
378 
379  // Adjust the velocity, position, and integral states. Do
380  // not alter the acceleration.
381  vel_pxy += dv;
382  pos_pxy += dv * dt/2;
383 
384  vel_err -= dv;
385  pos_err -= dv * dt/2;
386  int_err -= dv * dt2/4;
387  }
388 
389  // If the mass and damping gain are both zero and the position
390  // gain is nonzero, we have to adjust the force by shifting the
391  // proxy position.
392  else if (Kp > 0.0)
393  {
394  double dp; // Position delta (change)
395 
396  // Compute the velocity delta assuming the integral term
397  // does not saturate.
398  dp = (force - Fpd - Fi) / (Kp + Ki*dt/2);
399 
400  // Check for clamping/saturation on the adjusted integral
401  // force and re-compute the delta appropriately. There is
402  // no need to re-check for clamping after recomputation, as
403  // the new delta will only increase and can not undo the
404  // saturation.
405  if (Fi+dp*Ki*dt/2 > Ficl) dp=(force-Fpd-Ficl)/(Kp);
406  else if (Fi+dp*Ki*dt/2 < -Ficl) dp=(force-Fpd+Ficl)/(Kp);
407 
408  // Adjust the position and integral states. Do not alter
409  // the acceleration or velocity.
410  pos_pxy += dp;
411 
412  pos_err -= dp;
413  int_err -= dp * dt/2;
414  }
415 
416  // If the mass, damping, and position gain are all zero, there
417  // isn't much we can do...
418  }
419 
420 
421  // Step 4: Clean up
422  // (a) Stop the position error integration (limit the integral error) if
423  // the integrator clamp is in effect. Note this is safe for Ki==0.
424  if (Ki * int_err > Ficl) int_err = Ficl / Ki;
425  else if (Ki * int_err < -Ficl) int_err = -Ficl / Ki;
426 
427  // (b) Remember the state.
428  last_proxy_pos_ = pos_pxy;
429  last_proxy_vel_ = vel_pxy;
430  last_proxy_acc_ = acc_pxy;
431  last_vel_error_ = vel_err;
432  last_pos_error_ = pos_err;
433  last_int_error_ = int_err;
434 
435  // (c) Return the controller force.
436  return(force);
437 }
438 
439 } // namespace
static void calcDynamics2ndorder(double &a, double &dadp, double &dadv, double p, double v, double lam, double acon)
static void calcDynamics1storder(double &a, double &dadv, double v, double lam, double)
void reset(double pos_act, double vel_act)
double update(double pos_des, double vel_des, double acc_des, double pos_act, double vel_act, double dt)


control_toolbox
Author(s): Melonee Wise, Sachin Chitta, John Hsu
autogenerated on Mon Jun 10 2019 12:56:31