MCPcopy Create free account
hub / github.com/davisking/dlib / test_with_positive_target_error_thresh

Function test_with_positive_target_error_thresh

dlib/test/mpc.cpp:256–324  ·  view source on GitHub ↗

Source from the content-addressed store, hash-verified

254 }
255
256 void test_with_positive_target_error_thresh()
257 {
258 // a basic position + velocity model
259 matrix<double,2,2> A;
260 A = 1, 1,
261 0, 1;
262 matrix<double,2,1> B, C;
263 B = 0,
264 1;
265
266 C = 0.0,0.0; // no constant bias
267
268 matrix<double,2,1> Q;
269 Q = 2, 0; // only care about getting the position right
270 matrix<double,1,1> R, lower, upper;
271 R = 0.001;
272
273 // We set it up so that the controller that only cares about getting to the target but
274 // doesn't care about what happens after can just give large controls of -1. It will fly
275 // past the target once it gets there, but it doesn't care about that. The solver_normal
276 // however wants to come to rest at the target. So it will have to give a much smaller
277 // control to get it moving towards the target and then slowly decelerate over time to end
278 // up stopping. So it will take longer to get to the target, which we test below.
279 lower = -1.0;
280 upper = 0.05;
281
282 mpc<2,1,30> solver_normal(A,B,C,Q,R,lower,upper);
283 solver_normal.set_epsilon(0.00000001);
284 solver_normal.set_max_iterations(10000);
285
286 mpc<2,1,30> solver_target_thresh = solver_normal;
287 solver_target_thresh.set_target_error_threshold(1.0);
288
289 matrix<double,2,1> state_normal;
290 state_normal = 10, 0;
291 matrix<double,2,1> state_target_thresh = state_normal;
292
293 // Run the control law with and without set_target_error_threshold() called. We should
294 // observe that the controller using set_target_error_threshold(1) drives us to the target
295 // state faster, however, only the normal solver results in a rest state at the target
296 // state.
297 int time_at_target_normal = -1;
298 int time_at_target_other = -1;
299 for (int i = 0; i < 30; ++i)
300 {
301 print_spinner();
302 const double normal_control = solver_normal(state_normal);
303 state_normal = A*state_normal + B*normal_control + C;
304
305 const double target_thresh_control = solver_target_thresh(state_target_thresh);
306 state_target_thresh = A*state_target_thresh + B*target_thresh_control + C;
307
308 const double normal_error = trans(state_normal)*diagm(Q)*state_normal;
309 const double target_error = trans(state_target_thresh)*diagm(Q)*state_target_thresh;
310 if (normal_error < 1.0 && time_at_target_normal == -1)
311 time_at_target_normal = i;
312 if (target_error < 1.0 && time_at_target_other == -1)
313 time_at_target_other = i;

Callers 1

perform_testMethod · 0.85

Calls 7

print_spinnerFunction · 0.85
diagmFunction · 0.85
transFunction · 0.50
lengthFunction · 0.50
set_epsilonMethod · 0.45
set_max_iterationsMethod · 0.45

Tested by

no test coverage detected