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

Method perform_test

dlib/test/mpc.cpp:336–410  ·  view source on GitHub ↗

Source from the content-addressed store, hash-verified

334 {}
335
336 void perform_test (
337 )
338 {
339 // a basic position + velocity model
340 matrix<double,2,2> A;
341 A = 1, 1,
342 0, 1;
343 matrix<double,2,1> B, C;
344 B = 0,
345 1;
346
347 C = 0.02,0.1; // no constant bias
348
349 matrix<double,2,1> Q;
350 Q = 2, 0; // only care about getting the position right
351 matrix<double,1,1> R, lower, upper;
352 R = 1;
353
354 lower = -0.2;
355 upper = 0.2;
356
357 std::vector<matrix<double,1,1> > controls(30);
358 std::vector<matrix<double,2,1> > target(30);
359 for (unsigned long i = 0; i < controls.size(); ++i)
360 {
361 controls[i] = 0;
362 target[i] = 0;
363 }
364
365 mpc<2,1,30> solver(A,B,C,Q,R,lower,upper);
366 solver.set_epsilon(0.00000001);
367 solver.set_max_iterations(10000);
368 matrix<double,2,1> initial_state;
369 initial_state = 0;
370 initial_state(0) = 5;
371 for (int i = 0; i < 30; ++i)
372 {
373 print_spinner();
374 matrix<double,1,1> control = solver(initial_state);
375
376 for (unsigned long i = 1; i < controls.size(); ++i)
377 controls[i-1] = controls[i];
378
379 // Compute the correct control via SMO and make sure it matches.
380 solve_linear_mpc(A,B,C,Q,R,lower,upper, target, initial_state, controls);
381 dlog << LINFO << "ERROR: " << length(control-controls[0]);
382 DLIB_TEST(length(control-controls[0]) < 1e-7);
383
384 initial_state = A*initial_state + B*control + C;
385 //cout << control(0) << "\t" << trans(initial_state);
386 }
387
388 {
389 // also just generally test our QP solver.
390 matrix<double,20,20> Q = gaussian_randm(20,20,5);
391 Q = Q*trans(Q);
392
393 matrix<double,20,1> b = randm(20,1)-0.5;

Callers

nothing calls this directly

Calls 14

print_spinnerFunction · 0.85
solve_linear_mpcFunction · 0.85
gaussian_randmFunction · 0.85
solve_qp_box_using_smoFunction · 0.85
solve_qp_box_constrainedFunction · 0.85
absFunction · 0.85
randmFunction · 0.70
lengthFunction · 0.50
transFunction · 0.50
maxFunction · 0.50
sizeMethod · 0.45

Tested by

no test coverage detected