mirror of https://github.com/davisking/dlib.git
90 lines
2.1 KiB
C++
90 lines
2.1 KiB
C++
|
// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
|
||
|
|
/*
|
||
|
|
|
||
|
|
*/
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
#include <dlib/gui_widgets.h>
|
||
|
|
#include <dlib/control.h>
|
||
|
|
#include <dlib/image_transforms.h>
|
||
|
|
|
||
|
|
|
||
|
|
using namespace std;
|
||
|
|
using namespace dlib;
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
|
||
|
|
int main()
|
||
|
|
{
|
||
|
|
// state is x, y, x_vel, y_vel
|
||
|
|
matrix<double,4,4> A;
|
||
|
|
A = 1, 0, 1, 0,
|
||
|
|
0, 1, 0, 1,
|
||
|
|
0, 0, 1, 0,
|
||
|
|
0, 0, 0, 1;
|
||
|
|
|
||
|
|
matrix<double,4,2> B;
|
||
|
|
B = 0, 0,
|
||
|
|
0, 0,
|
||
|
|
1, 0,
|
||
|
|
0, 1;
|
||
|
|
|
||
|
|
matrix<double,4,1> C;
|
||
|
|
C = 0,
|
||
|
|
0,
|
||
|
|
0,
|
||
|
|
0.1;
|
||
|
|
|
||
|
|
matrix<double,4,1> Q;
|
||
|
|
Q = 1, 1, 0, 0;
|
||
|
|
|
||
|
|
matrix<double,2,1> R, lower, upper;
|
||
|
|
R = 1, 1;
|
||
|
|
lower = -0.5, -0.5;
|
||
|
|
upper = 0.5, 0.5;
|
||
|
|
|
||
|
|
mpc<4,2,30> controller(A,B,C,Q,R,lower,upper);
|
||
|
|
|
||
|
|
dlib::rand rnd;
|
||
|
|
matrix<double,4,1> target;
|
||
|
|
target = rnd.get_random_double()*400,rnd.get_random_double()*400,0,0;
|
||
|
|
controller.set_target(target);
|
||
|
|
|
||
|
|
|
||
|
|
matrix<double,4,1> current_state;
|
||
|
|
current_state = 200,200,0,0;
|
||
|
|
|
||
|
|
matrix<rgb_pixel> world(400,400);
|
||
|
|
image_window win;
|
||
|
|
|
||
|
|
int iter = 0;
|
||
|
|
while(!win.is_closed())
|
||
|
|
{
|
||
|
|
matrix<double,2,1> action = controller(current_state);
|
||
|
|
|
||
|
|
assign_all_pixels(world, rgb_pixel(255,255,255));
|
||
|
|
const dpoint pos = point(current_state(0),current_state(1));
|
||
|
|
const dpoint goal = point(target(0),target(1));
|
||
|
|
draw_solid_circle(world, goal, 9, rgb_pixel(100,255,100));
|
||
|
|
draw_solid_circle(world, pos, 7, 0);
|
||
|
|
draw_line(world, pos, pos-50*action, rgb_pixel(255,0,0));
|
||
|
|
|
||
|
|
current_state = A*current_state + B*action + C;
|
||
|
|
win.set_image(world);
|
||
|
|
dlib::sleep(100);
|
||
|
|
|
||
|
|
// Every 100 iterations change the target to some other random location.
|
||
|
|
++iter;
|
||
|
|
if (iter > 100)
|
||
|
|
{
|
||
|
|
iter = 0;
|
||
|
|
target = rnd.get_random_double()*400,rnd.get_random_double()*400,0,0;
|
||
|
|
controller.set_target(target);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
|