pid_tests.cpp
Go to the documentation of this file.
1 
2 #include <ros/ros.h>
3 
4 #include <gtest/gtest.h>
5 
6 #include <control_toolbox/pid.h>
7 
8 #include <boost/math/special_functions/fpclassify.hpp>
9 
10 using namespace control_toolbox;
11 
12 TEST(ParameterTest, ITermBadIBoundsTest)
13 {
14  RecordProperty("description","This test checks that the integral contribution is robust to bad i_bounds specification (i.e. i_min > i_max).");
15 
16  // Check that the output is not a non-sense if i-bounds are bad, i.e. i_min > i_max
17  Pid pid(1.0, 1.0, 1.0, -1.0, 1.0);
18  double cmd = 0.0;
19  double pe,ie,de;
20 
21  cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
22  pid.getCurrentPIDErrors(&pe,&ie,&de);
23  EXPECT_FALSE(boost::math::isinf(ie));
24  EXPECT_FALSE(boost::math::isnan(cmd));
25 
26  cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
27  pid.getCurrentPIDErrors(&pe,&ie,&de);
28  EXPECT_FALSE(boost::math::isinf(ie));
29  EXPECT_FALSE(boost::math::isnan(cmd));
30 }
31 
32 TEST(ParameterTest, integrationClampTest)
33 {
34  RecordProperty("description","This test succeeds if the integral contribution is clamped when the integral gain is non-zero.");
35 
36  Pid pid(0.0, 1.0, 0.0, 1.0, -1.0);
37 
38  double cmd = 0.0;
39 
40  // Test lower limit
41  cmd = pid.computeCommand(-10.03, ros::Duration(1.0));
42  EXPECT_EQ(-1.0, cmd);
43 
44  // Test upper limit
45  cmd = pid.computeCommand(30.0, ros::Duration(1.0));
46  EXPECT_EQ(1.0, cmd);
47 
48 }
49 
50 TEST(ParameterTest, integrationClampZeroGainTest)
51 {
52  RecordProperty("description","This test succeeds if the integral contribution is clamped when the integral gain is zero. If the integral contribution is not clamped while it is disabled, it can cause sudden jumps to the minimum or maximum bound in control command when re-enabled.");
53 
54  double i_gain = 0.0;
55  double i_min = -1.0;
56  double i_max = 1.0;
57  Pid pid(0.0, i_gain, 0.0, i_max, i_min);
58 
59  double cmd = 0.0;
60  double pe,ie,de;
61 
62  cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
63  pid.getCurrentPIDErrors(&pe,&ie,&de);
64  EXPECT_LE(i_min, cmd);
65  EXPECT_LE(cmd, i_max);
66  EXPECT_EQ(0.0, cmd);
67 
68  cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
69  EXPECT_LE(i_min, cmd);
70  EXPECT_LE(cmd, i_max);
71  EXPECT_EQ(0.0, cmd);
72 }
73 
74 TEST(ParameterTest, integrationAntiwindupTest)
75 {
76  RecordProperty("description","This test succeeds if the integral error is prevented from winding up when i_gain > 0");
77 
78  double i_gain = 2.0;
79  double i_min = -1.0;
80  double i_max = 1.0;
81  Pid pid(0.0, i_gain, 0.0, i_max, i_min, true);
82 
83  double cmd = 0.0;
84  double pe,ie,de;
85 
86  cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
87  EXPECT_EQ(-1.0, cmd);
88 
89  cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
90  EXPECT_EQ(-1.0, cmd);
91 
92  cmd = pid.computeCommand(0.5, ros::Duration(1.0));
93  EXPECT_EQ(0.0, cmd);
94 
95  cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
96  EXPECT_EQ(-1.0, cmd);
97 }
98 
99 TEST(ParameterTest, negativeIntegrationAntiwindupTest)
100 {
101  RecordProperty("description","This test succeeds if the integral error is prevented from winding up when i_gain < 0");
102 
103  double i_gain = -2.5;
104  double i_min = -0.2;
105  double i_max = 0.5;
106  Pid pid(0.0, i_gain, 0.0, i_max, i_min, true);
107 
108  double cmd = 0.0;
109  double pe,ie,de;
110 
111  cmd = pid.computeCommand(0.1, ros::Duration(1.0));
112  EXPECT_EQ(-0.2, cmd);
113 
114  cmd = pid.computeCommand(0.1, ros::Duration(1.0));
115  EXPECT_EQ(-0.2, cmd);
116 
117  cmd = pid.computeCommand(-0.05, ros::Duration(1.0));
118  EXPECT_EQ(-0.075, cmd);
119 
120  cmd = pid.computeCommand(0.1, ros::Duration(1.0));
121  EXPECT_EQ(-0.2, cmd);
122 }
123 
124 TEST(ParameterTest, gainSettingCopyPIDTest)
125 {
126  RecordProperty("description","This test succeeds if a PID object has its gain set at different points in time then the values are get-ed and still remain the same, as well as when PID is copied.");
127 
128  // Test values
129  double p_gain = rand() % 100;
130  double i_gain = rand() % 100;
131  double d_gain = rand() % 100;
132  double i_max = rand() % 100;
133  double i_min = -1 * rand() % 100;
134  bool antiwindup = false;
135 
136  // Initialize the default way
137  Pid pid1(p_gain, i_gain, d_gain, i_max, i_min, antiwindup);
138 
139  // Test return values -------------------------------------------------
140  double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return;
141  bool antiwindup_return;
142  pid1.getGains(p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return);
143 
144  EXPECT_EQ(p_gain, p_gain_return);
145  EXPECT_EQ(i_gain, i_gain_return);
146  EXPECT_EQ(d_gain, d_gain_return);
147  EXPECT_EQ(i_max, i_max_return);
148  EXPECT_EQ(i_min, i_min_return);
149  EXPECT_EQ(antiwindup, antiwindup_return);
150 
151  // Test return values using struct -------------------------------------------------
152 
153  // New values
154  p_gain = rand() % 100;
155  i_gain = rand() % 100;
156  d_gain = rand() % 100;
157  i_max = rand() % 100;
158  i_min = -1 * rand() % 100;
159  pid1.setGains(p_gain, i_gain, d_gain, i_max, i_min, antiwindup);
160 
161  Pid::Gains g1 = pid1.getGains();
162  EXPECT_EQ(p_gain, g1.p_gain_);
163  EXPECT_EQ(i_gain, g1.i_gain_);
164  EXPECT_EQ(d_gain, g1.d_gain_);
165  EXPECT_EQ(i_max, g1.i_max_);
166  EXPECT_EQ(i_min, g1.i_min_);
167  EXPECT_EQ(antiwindup, g1.antiwindup_);
168 
169  // \todo test initParam() -------------------------------------------------
170 
171 
172  // \todo test bool init(const ros::NodeHandle &n); -----------------------------------
173 
174 
175  // Send update command to populate errors -------------------------------------------------
176  pid1.setCurrentCmd(10);
177  pid1.computeCommand(20, ros::Duration(1.0));
178 
179  // Test copy constructor -------------------------------------------------
180  Pid pid2(pid1);
181 
182  pid2.getGains(p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return);
183 
184  EXPECT_EQ(p_gain, p_gain_return);
185  EXPECT_EQ(i_gain, i_gain_return);
186  EXPECT_EQ(d_gain, d_gain_return);
187  EXPECT_EQ(i_max, i_max_return);
188  EXPECT_EQ(i_min, i_min_return);
189  EXPECT_EQ(antiwindup, antiwindup_return);
190 
191  // Test that errors are zero
192  double pe2, ie2, de2;
193  pid2.getCurrentPIDErrors(&pe2, &ie2, &de2);
194  EXPECT_EQ(0.0, pe2);
195  EXPECT_EQ(0.0, ie2);
196  EXPECT_EQ(0.0, de2);
197 
198  // Test assignment constructor -------------------------------------------------
199  Pid pid3;
200  pid3 = pid1;
201 
202  pid3.getGains(p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return);
203 
204  EXPECT_EQ(p_gain, p_gain_return);
205  EXPECT_EQ(i_gain, i_gain_return);
206  EXPECT_EQ(d_gain, d_gain_return);
207  EXPECT_EQ(i_max, i_max_return);
208  EXPECT_EQ(i_min, i_min_return);
209  EXPECT_EQ(antiwindup, antiwindup_return);
210 
211  // Test that errors are zero
212  double pe3, ie3, de3;
213  pid3.getCurrentPIDErrors(&pe3, &ie3, &de3);
214  EXPECT_EQ(0.0, pe3);
215  EXPECT_EQ(0.0, ie3);
216  EXPECT_EQ(0.0, de3);
217 
218  // Test the reset() function, it should clear errors and command
219  pid1.reset();
220 
221  double pe1, ie1, de1;
222  pid1.getCurrentPIDErrors(&pe1, &ie1, &de1);
223  EXPECT_EQ(0.0, pe1);
224  EXPECT_EQ(0.0, ie1);
225  EXPECT_EQ(0.0, de1);
226 
227  double cmd1 = pid1.getCurrentCmd();
228  EXPECT_EQ(0.0, cmd1);
229 }
230 
231 TEST(CommandTest, proportionalOnlyTest)
232 {
233  RecordProperty("description","This test checks that a command is computed correctly using the proportional contribution only.");
234 
235  // Set only proportional gain
236  Pid pid(1.0, 0.0, 0.0, 0.0, 0.0);
237  double cmd = 0.0;
238 
239  // If initial error = 0, p-gain = 1, dt = 1
240  cmd = pid.computeCommand(-0.5, ros::Duration(1.0));
241  // Then expect command = error
242  EXPECT_EQ(-0.5, cmd);
243 
244  // If call again
245  cmd = pid.computeCommand(-0.5, ros::Duration(1.0));
246  // Then expect the same as before
247  EXPECT_EQ(-0.5, cmd);
248 
249  // If call again doubling the error
250  cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
251  // Then expect the command doubled
252  EXPECT_EQ(-1.0, cmd);
253 
254  // If call with positive error
255  cmd = pid.computeCommand(0.5, ros::Duration(1.0));
256  // Then expect always command = error
257  EXPECT_EQ(0.5, cmd);
258 }
259 
260 TEST(CommandTest, integralOnlyTest)
261 {
262  RecordProperty("description","This test checks that a command is computed correctly using the integral contribution only (ATTENTION: this test depends on the integration scheme).");
263 
264  // Set only integral gains with enough limits to test behavior
265  Pid pid(0.0, 1.0, 0.0, 5.0, -5.0);
266  double cmd = 0.0;
267 
268  // If initial error = 0, i-gain = 1, dt = 1
269  cmd = pid.computeCommand(-0.5, ros::Duration(1.0));
270  // Then expect command = error
271  EXPECT_EQ(-0.5, cmd);
272 
273  // If call again with same arguments
274  cmd = pid.computeCommand(-0.5, ros::Duration(1.0));
275  // Then expect the integral part to double the command
276  EXPECT_EQ(-1.0, cmd);
277 
278  // Call again with no error
279  cmd = pid.computeCommand(0.0, ros::Duration(1.0));
280  // Expect the integral part to keep the previous command because it ensures error = 0
281  EXPECT_EQ(-1.0, cmd);
282 
283  // Double check that the integral contribution keep the previous command
284  cmd = pid.computeCommand(0.0, ros::Duration(1.0));
285  EXPECT_EQ(-1.0, cmd);
286 
287  // Finally call again with positive error to see if the command changes in the opposite direction
288  cmd = pid.computeCommand(1.0, ros::Duration(1.0));
289  // Expect that the command is cleared since error = -1 * previous command, i-gain = 1, dt = 1
290  EXPECT_EQ(0.0, cmd);
291 }
292 
293 TEST(CommandTest, derivativeOnlyTest)
294 {
295  RecordProperty("description","This test checks that a command is computed correctly using the derivative contribution only with own differentiation (ATTENTION: this test depends on the differentiation scheme).");
296 
297  // Set only derivative gain
298  Pid pid(0.0, 0.0, 1.0, 0.0, 0.0);
299  double cmd = 0.0;
300 
301  // If initial error = 0, d-gain = 1, dt = 1
302  cmd = pid.computeCommand(-0.5, ros::Duration(1.0));
303  // Then expect command = error
304  EXPECT_EQ(-0.5, cmd);
305 
306  // If call again with same error
307  cmd = pid.computeCommand(-0.5, ros::Duration(1.0));
308  // Then expect command = 0 due to no variation on error
309  EXPECT_EQ(0.0, cmd);
310 
311  // If call again with same error and smaller control period
312  cmd = pid.computeCommand(-0.5, ros::Duration(0.1));
313  // Then expect command = 0 again
314  EXPECT_EQ(0.0, cmd);
315 
316  // If the error increases, with dt = 1
317  cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
318  // Then expect the command = change in dt
319  EXPECT_EQ(-0.5, cmd);
320 
321  // If error decreases, with dt = 1
322  cmd = pid.computeCommand(-0.5, ros::Duration(1.0));
323  // Then expect always the command = change in dt (note the sign flip)
324  EXPECT_EQ(0.5, cmd);
325 }
326 
327 TEST(CommandTest, completePIDTest)
328 {
329  RecordProperty("description","This test checks that a command is computed correctly using a complete PID controller (ATTENTION: this test depends on the integral and differentiation schemes).");
330 
331  Pid pid(1.0, 1.0, 1.0, 5.0, -5.0);
332  double cmd = 0.0;
333 
334  // All contributions are tested, here few tests check that they sum up correctly
335  // If initial error = 0, all gains = 1, dt = 1
336  cmd = pid.computeCommand(-0.5, ros::Duration(1.0));
337  // Then expect command = 3x error
338  EXPECT_EQ(-1.5, cmd);
339 
340  // If call again with same arguments, no error change, but integration do its part
341  cmd = pid.computeCommand(-0.5, ros::Duration(1.0));
342  // Then expect command = 3x error again
343  EXPECT_EQ(-1.5, cmd);
344 
345  // If call again increasing the error
346  cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
347  // Then expect command equals to p = -1, i = -2.0 (i.e. - 0.5 - 0.5 - 1.0), d = -0.5
348  EXPECT_EQ(-3.5, cmd);
349 }
350 
351 int main(int argc, char** argv) {
352  testing::InitGoogleTest(&argc, argv);
353  return RUN_ALL_TESTS();
354 }
void getCurrentPIDErrors(double *pe, double *ie, double *de)
Return PID error terms for the controller.
Definition: pid.cpp:401
void getGains(double &p, double &i, double &d, double &i_max, double &i_min)
Get PID gains for the controller.
Definition: pid.cpp:206
void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup=false)
Set PID gains for the controller.
Definition: pid.cpp:229
int main(int argc, char **argv)
Definition: pid_tests.cpp:351
TEST(ParameterTest, ITermBadIBoundsTest)
Definition: pid_tests.cpp:12
double computeCommand(double error, ros::Duration dt)
Set the PID error and compute the PID command with nonuniform time step size. The derivative error is...
Definition: pid.cpp:298
Store gains in a struct to allow easier realtime buffer usage.
Definition: pid.h:124
A basic pid class.
Definition: pid.h:117
void setCurrentCmd(double cmd)
Set current command for this PID controller.
Definition: pid.cpp:391
double getCurrentCmd()
Return current command for this PID controller.
Definition: pid.cpp:396
void reset()
Reset the state of this PID controller.
Definition: pid.cpp:197


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