ros2_control - rolling
limited_proxy.hpp
1 // Copyright (c) 2008, Willow Garage, Inc.
2 // All rights reserved.
3 //
4 // Software License Agreement (BSD License 2.0)
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions
8 // are met:
9 //
10 // * Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 // * Redistributions in binary form must reproduce the above
13 // copyright notice, this list of conditions and the following
14 // disclaimer in the documentation and/or other materials provided
15 // with the distribution.
16 // * Neither the name of the Willow Garage nor the names of its
17 // contributors may be used to endorse or promote products derived
18 // from this software without specific prior written permission.
19 //
20 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 // POSSIBILITY OF SUCH DAMAGE.
32 
33 #ifndef CONTROL_TOOLBOX__LIMITED_PROXY_HPP_
34 #define CONTROL_TOOLBOX__LIMITED_PROXY_HPP_
35 
36 namespace control_toolbox
37 {
39 {
40 public:
41  // Controller parameter values
42  double mass_; // Estimate of the joint mass
43  double Kd_; // Damping gain
44  double Kp_; // Position gain
45  double Ki_; // Integral gain
46  double Ficl_; // Integral force clamp
47  double effort_limit_; // Limit on output force
48  double vel_limit_; // Limit on velocity
49  double pos_upper_limit_; // Upper position bound
50  double pos_lower_limit_; // Lower position bound
51  double lambda_proxy_; // Bandwidth of proxy reconvergence
52  double acc_converge_; // Acceleration of proxy reconvergence
53 
54  LimitedProxy()
55  : mass_(0.0),
56  Kd_(0.0),
57  Kp_(0.0),
58  Ki_(0.0),
59  Ficl_(0.0),
60  effort_limit_(0.0),
61  vel_limit_(0.0),
62  pos_upper_limit_(0.0),
63  pos_lower_limit_(0.0),
64  lambda_proxy_(0.0),
65  acc_converge_(0.0)
66  {
67  }
68 
69  void reset(double pos_act, double vel_act);
70 
71  double update(
72  double pos_des, double vel_des, double acc_des, double pos_act, double vel_act, double dt);
73 
74 private:
75  // Controller state values
76  double last_proxy_pos_; // Proxy position
77  double last_proxy_vel_; // Proxy velocity
78  double last_proxy_acc_; // Proxy acceleration
79 
80  double last_vel_error_; // Velocity error
81  double last_pos_error_; // Position error
82  double last_int_error_; // Integral error
83 };
84 
85 } // namespace control_toolbox
86 
87 #endif // CONTROL_TOOLBOX__LIMITED_PROXY_HPP_
Definition: limited_proxy.hpp:39
Definition: dither.hpp:46