├── .github
    └── workflows
    │   └── ci.yml
├── .gitignore
├── README.md
├── docs
    └── mkdocs.yml
├── matlab_implementation
    ├── Animate.m
    ├── MPPI_Pendulum.m
    ├── Pendulam_Dynamics.m
    ├── animatePendulumCart.m
    ├── car
    │   ├── CarDynamicModel.m
    │   ├── CostFunction_car.m
    │   └── GetState.m
    ├── cost_function.m
    ├── cost_function_updat.m
    ├── plotPendulumCart.m
    ├── res
    │   ├── anim.gif
    │   └── states.png
    └── totalEntropy.m
├── mkdocs.yml
├── python_implementation
    └── MPPI_test.py
└── report
    ├── presentation_MPPI.pdf
    └── report_MPPI.pdf


/.github/workflows/ci.yml:
--------------------------------------------------------------------------------
 1 | name: mppi
 2 | on:
 3 |   push:
 4 |     branches: 
 5 |       - master
 6 |       
 7 | jobs:
 8 |   deploy:
 9 |     runs-on: ubuntu-latest
10 |     steps:
11 |       - uses: actions/checkout@v2
12 |       - uses: actions/setup-python@v2
13 |         with:
14 |           python-version: 3.x
15 |       - run: pip install mkdocs-material 
16 |       - run: mkdocs gh-deploy --force
17 | 
18 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | .idea/
2 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # controls-project
 2 | 
 3 | ### Study of PI based stochastic optimal control and Implementation of MPPI for Aggressive Driving
 4 | 
 5 | This project was submitted as a course project for RBE502 - Robot Control  in Spring 2018 at Worcester Polytechnic Institute. 
 6 | 
 7 | Course page - [https://users.wpi.edu/~jfu2/rbe502/index.html](https://users.wpi.edu/~jfu2/rbe502/index.html)
 8 | 
 9 | The project is to conduct a study on Model Predictive Path Integral (MPPI) Control and implement the same for aggressive driving for a car-like robot. This is primarily based on work presented in 
10 | 
11 | - [Aggressive driving with model predictive path integral control](https://ieeexplore.ieee.org/document/7487277/)
12 | - [Model Predictive Path Integral Control: From Theory to Parallel Computation](https://arc.aiaa.org/doi/abs/10.2514/1.G001921)
13 | - [Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving](https://arxiv.org/abs/1707.02342)
14 | 
15 | For more details on the work, check this [report](https://github.com/vvrs/MPPIController/tree/master/report/report_MPPI.pdf).
16 | <br>
17 | #### How to run
18 | ---------------
19 | 
20 | Goto _matlab\_implementation_ directory in MATLAB and run
21 | ```
22 | MPPI_Pendulum.m
23 | ```
24 | 
25 | <br>
26 | 
27 | #### Result
28 | -----------
29 | 
30 | <div align='center'>
31 |   <img src="https://raw.githubusercontent.com/vvrs/MPPIController/master/matlab_implementation/res/anim.gif"/>
32 | </div>
33 | 
34 | <div align='center'>
35 |   <img src="https://raw.githubusercontent.com/vvrs/MPPIController/master/matlab_implementation/res/states.png"/>
36 | </div>
37 | 


--------------------------------------------------------------------------------
/docs/mkdocs.yml:
--------------------------------------------------------------------------------
1 | theme:
2 |   name: material
3 | 


--------------------------------------------------------------------------------
/matlab_implementation/Animate.m:
--------------------------------------------------------------------------------
 1 | function Animate(t,x,P)
 2 | %Animate(t,x,P)
 3 | %
 4 | %FUNCTION:
 5 | %   Animate is used to animate a system with state x at the times in t.
 6 | %
 7 | %INPUTS:
 8 | %   t = [1xM] vector of times, Must be monotonic: t(k) < t(k+1)
 9 | %   x = [NxM] matrix of states, corresponding to times in t
10 | %   P = animation parameter struct, with field:
11 | %     plotFunc = @(t,x) = function handle to create a plot
12 | %       t = a scalar time
13 | %       x = [Nx1] state vector
14 | %     speed = scalar multiple of time, for playback speed 
15 | %
16 | %OUTPUTS: 
17 | %   Animation based on data in t and x.
18 | %
19 | 
20 | if ~isfield(P,'figNum')
21 |     P.figNum=1000;  %Default to figure 1000
22 | end
23 | figure(P.figNum)
24 | 
25 | 
26 | Loop_Time = 0;    %store how long has the simulation been running
27 | T_end = t(end);   %Ending time of one step
28 | 
29 | t = t-min(t);  %Force things to start at t=0;
30 | 
31 | % f = getframe;
32 | % [im,map] = rgb2ind(f.cdata,65536,'nodither');
33 | % im(1,1,1,length(t)+1) = 0;
34 | % 
35 | % l = 1;
36 | 
37 | tic;    %Start a timer
38 | while Loop_Time < T_end  %Loop while the CPU time is less than the end of the simulation's time
39 |     
40 |     %Interpolate to get the new point:
41 |     xNow = interp1(t',x',Loop_Time,'pchip','extrap')';
42 |     
43 |     %Call the plot command
44 |    feval(P.plotFunc,Loop_Time,xNow);
45 |    drawnow;
46 |    
47 | %    f = getframe;
48 | %    im(:,:,1,l+1) = rgb2ind(f.cdata,map,'nodither');
49 |    
50 |    
51 |        %The next few lines pull the time step that is closest to the real time
52 |     RealTime = toc;
53 |     Loop_Time = RealTime*P.speed;   %Get the current time  (Taking slow motion into accunt if desired)
54 |     
55 | %     l = l+1;
56 | end
57 | 
58 | % imwrite(im,map,'Cart_Pole_MPPI.gif','DelayTime',0,'LoopCount',inf)
59 | end


--------------------------------------------------------------------------------
/matlab_implementation/MPPI_Pendulum.m:
--------------------------------------------------------------------------------
  1 | clear all;
  2 | clc;
  3 | 
  4 | 
  5 | 
  6 | K = 1000;
  7 | N = 100;
  8 | iteration = 500;
  9 | param.dt = 0.02;
 10 | 
 11 | % System Paramters Given in paper
 12 | param.mc = 1;
 13 | param.mp = 0.01;
 14 | param.l = 1;
 15 | param.g = 9.81;
 16 | 
 17 | % Variance and Lamda
 18 | param.lambda = 10;
 19 | param.variance = 10;
 20 | param.R = 1;
 21 | 
 22 | % param.lambda = 1/(param.variance*K);
 23 | 
 24 | % Initial State
 25 | x_init = [0 0 0 0];
 26 | 
 27 | % Final state for Cart Pole
 28 | x_fin = [0 0 pi 0];
 29 | 
 30 | % Variables To store the system state
 31 | X_sys = zeros(4,iteration+1);
 32 | U_sys = zeros(1,iteration);
 33 | cost  = zeros(1,iteration);
 34 | cost_avg = zeros(1,iteration);
 35 | 
 36 | X_sys(:,1) = x_init;
 37 | 
 38 | % Initialization of Variables
 39 | x = zeros(4,N);
 40 | delta_u = zeros(N,K);
 41 | u_init = 1;
 42 | 
 43 | X_sys(:,1) = x_init;
 44 | 
 45 | % Initialization of input for N time horizone
 46 | u = zeros(1,N);
 47 | 
 48 | % MPPI Loop
 49 | for j = 1: iteration
 50 |     % Initialization of Cost for K Samples
 51 |     Stk = zeros(1,K);
 52 |     
 53 |     % Calculating cost for K samples and N finite horizone
 54 |     for k = 1:K
 55 |         x(:,1) = x_init;
 56 |         for i = 1:N-1
 57 |             delta_u(i,k) = param.variance*(randn(1));
 58 |             x(:,i+1) = x(:,i) + Pendulam_Dynamics(x(1,i), x(2,i), x(3,i),...
 59 |                 x(4,i), (u(i)+delta_u(i,k)), param)*param.dt;
 60 |                 Stk(k) = Stk(k) + cost_function(x(1,i+1), x(2,i+1), x(3,i+1), ...
 61 |                     x(4,i+1),(u(i)+ delta_u(i,k)),param);
 62 |                 
 63 |            % implementing updated cost function given in paper Aggressive
 64 |            % Driving with MPPI control
 65 | %             Stk(k) = Stk(k) + cost_function_updat(x(1,i+1), x(2,i+1), x(3,i+1), ...
 66 | %                 x(4,i+1),u(i),delta_u(i,k),param);
 67 |         end
 68 |         delta_u(N,k) = param.variance*(randn(1));
 69 |         
 70 |     end
 71 |     
 72 |     % Average cost over iteration
 73 |     cost_avg(j) = sum(Stk)/K;
 74 |     
 75 |     % Updating lambda as function of R which Lambad = 1/R here R = Cost
 76 |     % function
 77 | %     param.lambda = 1000*cost(j) + 1;
 78 | 
 79 |     % Updating the control input according to the expectency over K sample
 80 |     % trajectories
 81 |     for i = 1:N
 82 |         u(i) = u(i) + totalEntropy(Stk(:) , delta_u(i,:),param);
 83 |     end
 84 |     
 85 |     % Input to the system 
 86 |     U_sys(j) = u(1);
 87 |     
 88 |     % System Updatation because of input
 89 |     X_sys(:,j+1) = X_sys(:,j) + Pendulam_Dynamics(X_sys(1, j), X_sys(2,j),...
 90 |         X_sys(3,j), X_sys(4,j), u(1), param)*param.dt;
 91 |     
 92 |     % Calculating state cost function
 93 |     cost(j+1) = cost_function(X_sys(1,j+1), X_sys(2,j+1), X_sys(3,j+1),...
 94 |         X_sys(4,j+1),(u(i)+delta_u(i,k)),param);
 95 |     
 96 |     % Input updatation for next time step
 97 |     for i = 1:N-1
 98 |         u(i) = u(i+1);
 99 |     end
100 |     
101 |     % Updating the input in Last time step
102 |     u(N) = u_init;
103 |     
104 |     % initial state for calculating the expectency over trajectory in next
105 |     % step
106 |     x_init = X_sys(:,j+1);  
107 | end
108 | 
109 | %% plot the cart pole state X and theta
110 | figure
111 | plot(X_sys(1,:))
112 | hold on
113 | plot(X_sys(3,:))
114 | title('X and Theta');
115 | ylabel('X and Theta');
116 | xlabel('Iteration');
117 | legend('X--', 'Theta');
118 | 
119 | 
120 | % Animation of Cart Pole
121 | figure;
122 | animatePendulumCart;


--------------------------------------------------------------------------------
/matlab_implementation/Pendulam_Dynamics.m:
--------------------------------------------------------------------------------
 1 | function dX = Pendulam_Dynamics(x, x_dot, theta, theta_dot, u, param)
 2 | 
 3 | mc = param.mc;
 4 | mp = param.mp;
 5 | l = param.l;
 6 | g = param.g;
 7 | 
 8 | % States of the Pendulum dX(1) is X_dot dX(2) is X double dor, dX(3) is
 9 | % Theta_dot and dX(4) is theta double dot
10 | dX(1) = x_dot;
11 | 
12 | dX(2) = (u + mp*sin(theta)*(l*theta_dot^2 + g*cos(theta)))/(mc+mp*sin(theta)^2);
13 | 
14 | dX(3) = theta_dot;
15 | 
16 | dX(4) = (-u*cos(theta) - mp*l*theta_dot^2*cos(theta)*sin(theta)...
17 |     -(mc+mp)*g*sin(theta))/(mc+mp*sin(theta)^2);
18 | 
19 | 
20 | dX = dX';
21 | end
22 | 
23 | 


--------------------------------------------------------------------------------
/matlab_implementation/animatePendulumCart.m:
--------------------------------------------------------------------------------
 1 | %Unpack the data
 2 | pos = X_sys(1,:);
 3 | angle = X_sys(3,:);
 4 | 
 5 | %Position of the cart over time:
 6 | Cart = [pos;zeros(size(pos))];
 7 | 
 8 | %Position of the pendulum bob over time:  (Assume that length==1)
 9 | Bob = Cart + param.l*[sin(angle); -cos(angle)];
10 | 
11 | %Pack up for plotting:
12 | t = zeros(1,iteration+1);
13 | x = [Cart;Bob];
14 | for i = 1:iteration
15 |     t(i+1) = param.dt*i;
16 | end
17 | 
18 | 
19 | %Figure out the extents of the axis
20 | horizontalAll = [Cart(1,:), Bob(1,:)];
21 | verticalAll = [Cart(2,:), Bob(2,:)];
22 | param.axis = [min(horizontalAll),max(horizontalAll),...
23 |   min(verticalAll),max(verticalAll)];
24 | param.clearFigure = true;
25 | 
26 | %Pass the trace of the Bob's path to the plotting function.
27 | param.Bob = Bob;
28 | 
29 | %Set up parameters for animation:
30 | P.plotFunc = @(t,x)plotPendulumCart(t,x,param);
31 | P.speed = 0.25;
32 | P.figNum = gcf;
33 | 
34 | %Call the animation function
35 | Animate(t,x,P)


--------------------------------------------------------------------------------
/matlab_implementation/car/CarDynamicModel.m:
--------------------------------------------------------------------------------
 1 | function X_dot = CarModel(t,x,u)
 2 | 
 3 | 	% State (x)
 4 | 	% vx,vy,phi_dot(angular velocity)
 5 | 
 6 | 	vx = x(1);
 7 | 	vy = x(2);
 8 | 	phi_dot = x(3);
 9 | 
10 | 	% input (u)
11 | 	% Frx, delta	
12 | 	Frx = u(1);
13 | 	delta = u(2);
14 | 
15 | 	% parameters
16 | 	theta_z = 2.78*10^-5 % kgm^2
17 | 	m = 40.1
18 | 	lr = 0.029
19 | 	lf = 0.033
20 | 
21 | 	Bf = 4.1
22 | 	Cf = 1.1
23 | 	Df = 0.22
24 | 
25 | 	Br = 3.8609
26 | 	Cr = 1.4
27 | 	Dr = 0.1643
28 | 
29 | 	alpha_f = -atan((phi_dot*lf + vy)/vx) + delta;
30 | 	alpha_r = atan((phi_dot*lr-vy)/vx);
31 | 
32 | 	Ffy = Df*sin(Cf*atan(Bf*alpha_f));
33 | 	Fry = Dr*sin(Cr*atan(Br*alpha_r));
34 | 
35 | 	vx_dot = (1/m)*(Frx - Ffy*sin(delta)+m*vy*phi_dot);
36 | 	vy_dot = (1/m)*(Fry - Ffx*cos(delta)-m*vx*phi_dot);
37 | 	phi_dot_dot = (1/theta_z)*(Ffy*lf*cos(delta)-Fry*lr);
38 | 
39 | 	X_dot = [vx_dot;
40 | 			 vy_dot;
41 | 			 phi_dot_dot];
42 | end


--------------------------------------------------------------------------------
/matlab_implementation/car/CostFunction_car.m:
--------------------------------------------------------------------------------
1 | function cost = CostFunction_car(X,V,v_desired)
2 | 	% X - from GetState
3 | 	% V - from CarDynamicModel
4 | 	d = abs((X(1)/13)^2 + (X(2)/6)^2 - 1);
5 | 
6 | 	cost = 100*d^2 + (V(1)-v_desired)^2;
7 | end


--------------------------------------------------------------------------------
/matlab_implementation/car/GetState.m:
--------------------------------------------------------------------------------
 1 | function X = GetState(velocity,current_state,dt)
 2 | 
 3 | 	vx = velocity(1);
 4 | 	vy = velocity(2);
 5 | 	phi_dot = velocity(3);
 6 | 
 7 | 	current_x = current_state(1);
 8 | 	current_y = current_state(2);
 9 | 	current_phi = current_state(3);
10 | 
11 | 	phi = current_phi + phi_dot*dt;
12 | 
13 | 	x_dot = vx*cos(phi) - vy*sin(phi);
14 | 	y_dot = vx*sin(phi) + vy*cos(phi);
15 | 
16 | 
17 | 	x = current_x + x_dot*dt;
18 | 	y = current_y + y_dot*dt;
19 | 
20 | 	X = [x;
21 | 		 y;
22 | 		 phi]
23 | end


--------------------------------------------------------------------------------
/matlab_implementation/cost_function.m:
--------------------------------------------------------------------------------
 1 | function [S] = cost_function(p, p_dot, theta, theta_dot, u, param)
 2 |     dt = param.dt;
 3 |     
 4 |     % P is position of Cart Pole Theta is angle theta_dot is change in
 5 |     % angle and p_dot is change in position
 6 |     S = (6*(p)^2 + 12*(1+cos(theta))^2 + 0.1*theta_dot^2 + 0.1*p_dot^2)*dt;
 7 |     
 8 |     % Cost function given in paper Model Predictive Path Integral Control
 9 |     % from theory to parallel computation
10 | %     S = (1*(p)^2 + 500*(1+cos(theta))^2 + 1*theta_dot^2 + 1*p_dot^2)*dt;
11 | 
12 | 
13 | end
14 | 


--------------------------------------------------------------------------------
/matlab_implementation/cost_function_updat.m:
--------------------------------------------------------------------------------
 1 | function [S] = cost_function_updat(p, p_dot, theta, theta_dot, u, delta_u, param)
 2 | 
 3 | % implementing updated cost function given in paper Aggressive Driving with MPPI control
 4 |     dt = param.dt;
 5 |     S = (6*(p)^2 + 12*(1+cos(theta))^2 + 0.1*theta_dot^2 + 0.1*p_dot^2)*dt;
 6 | %     S = (1*(p)^2 + 500*(1+cos(theta))^2 + 1*theta_dot^2 + 1*p_dot^2)*dt;
 7 | %     param.lambda
 8 | %     param.R
 9 | %     u
10 | %     param.variance
11 | %     delta_u
12 |     S = S + 0.5*param.R*u^2 + param.lambda*u*delta_u + 0.5*param.lambda*(1-(1/param.variance))*delta_u^2;
13 | 
14 | 
15 | end
16 | 


--------------------------------------------------------------------------------
/matlab_implementation/plotPendulumCart.m:
--------------------------------------------------------------------------------
  1 | % function plotPendulumCart(t,x,param)
  2 | % 
  3 | % Cart_Width = 0.2;
  4 | % Cart_Height = 0.1;
  5 | % % Limits = param.axis;
  6 | % Limits = [-2 2 -0.5 0.5];
  7 | % 
  8 | % 
  9 | % Cart = x(1:2);
 10 | % Bob = x(3:4);
 11 | % 
 12 | % if param.clearFigure    %THEN RUNNING ANIMATION
 13 | %     clf;
 14 | %     hold on;
 15 | %     
 16 | %     %Plot Cart
 17 | %     x = Cart(1) - 0.5*Cart_Width;
 18 | %     y = -0.5*Cart_Height;
 19 | %     w = Cart_Width;
 20 | %     h = Cart_Height;
 21 | %     h = rectangle('Position',[x,y,w,h],'LineWidth',4,'Curvature',[0.3,0.3]);
 22 | %     set(h,'EdgeColor',[0.1,0.8,0.1])
 23 | %     
 24 | %     %Plot Pendulum
 25 | %     Rod_X = [Cart(1), Bob(1)];
 26 | %     Rod_Y = [Cart(2), Bob(2)];
 27 | %     plot(Rod_X,Rod_Y,'k-','LineWidth',4)
 28 | %     
 29 | %     %Plot Bob
 30 | %     plot(Bob(1),Bob(2),'k.','MarkerSize',50)
 31 | %     
 32 | %     %Plot Rails
 33 | %     plot([Limits(1) Limits(2)],-0.5*Cart_Height*[1,1],'k-','LineWidth',2)
 34 | %     
 35 | %     %Title
 36 | %     title(['Simulation Time: ' num2str(t) ' s'])
 37 | %     xlabel(['X = : ' num2str(Cart(1)) 's '])
 38 | %     
 39 | %     %These commands keep the window from automatically rescaling in funny ways.
 40 | % %     axis(Limits);
 41 | % %     axis('equal');
 42 | % %     axis manual;
 43 | % %     axis on;
 44 | % %     
 45 | %     axis(Limits);
 46 | % %     axis('equal');
 47 | %     axis ([-2 2 -0.5 0.5]);
 48 | %     axis on;
 49 | %     
 50 | %     
 51 | % else    %THEN RUNNING STOP ACTION
 52 | %     
 53 | %     hold on;
 54 | %     
 55 | %     %Plot Cart
 56 | %     x = Cart(1) - 0.5*Cart_Width;
 57 | %     y = -0.5*Cart_Height;
 58 | %     w = Cart_Width;
 59 | %     h = Cart_Height;
 60 | %     rectangle('Position',[x,y,w,h],'LineWidth',2);
 61 | %     
 62 | %     %Plot Pendulum
 63 | %     Rod_X = [Cart(1), Bob(1)];
 64 | %     Rod_Y = [Cart(2), Bob(2)];
 65 | %     plot(Rod_X,Rod_Y,'k-','LineWidth',2)
 66 | %     
 67 | %     %Plot Bob and hinge
 68 | %     plot(Bob(1),Bob(2),'k.','MarkerSize',30)
 69 | %     plot(Cart(1),Cart(2),'k.','MarkerSize',30)
 70 | %     
 71 | %     %Plot Rails
 72 | %     plot([Limits(1) Limits(2)],-0.5*Cart_Height*[1,1],'k-','LineWidth',2)
 73 | %     
 74 | %     %Plot trace of the Bob's path:
 75 | %     plot(param.Bob(1,:),param.Bob(2,:),'k:','LineWidth',1)
 76 | %     
 77 | %     
 78 | %     %These commands keep the window from automatically rescaling in funny ways.
 79 | %     axis(Limits);
 80 | % %     axis('equal');
 81 | %     axis ([-2 2 -0.5 0.5]);
 82 | %     axis on;
 83 | %     
 84 | %     
 85 | % 
 86 | % end
 87 | % end
 88 | 
 89 | 
 90 | 
 91 | 
 92 | function plotPendulumCart(t,x,param)
 93 | 
 94 | Cart_Width = 0.3;
 95 | Cart_Height = 0.15;
 96 | Limits = [-2 2 -2 2];
 97 | Cart = x(1:2);
 98 | Bob = x(3:4);
 99 | 
100 | if param.clearFigure    %THEN RUNNING ANIMATION
101 |     clf;
102 |     hold on;
103 |     
104 |     %Plot Cart
105 |     x = Cart(1) - 0.5*Cart_Width;
106 |     y = -0.5*Cart_Height;
107 |     w = Cart_Width;
108 |     h = Cart_Height;
109 |     h = rectangle('Position',[x,y,w,h],'LineWidth',4,'Curvature',[0.3,0.3]);
110 |     set(h,'EdgeColor',[0.1,0.8,0.1])
111 |     
112 |     %Plot Pendulum
113 |     Rod_X = [Cart(1), Bob(1)];
114 |     Rod_Y = [Cart(2), Bob(2)];
115 |     plot(Rod_X,Rod_Y,'k-','LineWidth',4)
116 |     
117 |     %Plot Bob
118 |     plot(Bob(1),Bob(2),'k.','MarkerSize',50)
119 |     
120 |     %Plot Rails
121 |     plot([Limits(1) Limits(2)],-0.5*Cart_Height*[1,1],'k-','LineWidth',2)
122 |     
123 |     %Title
124 |     title(['Simulation Time: ' num2str(t) ' s'])
125 |     
126 |     
127 |     %These commands keep the window from automatically rescaling in funny ways.
128 |     axis(Limits);
129 |     axis('equal');
130 |     axis manual;
131 |     axis off;
132 |     
133 |     
134 | else    %THEN RUNNING STOP ACTION
135 |     
136 |     hold on;
137 |     
138 |     %Plot Cart
139 |     x = Cart(1) - 0.5*Cart_Width;
140 |     y = -0.5*Cart_Height;
141 |     w = Cart_Width;
142 |     h = Cart_Height;
143 |     rectangle('Position',[x,y,w,h],'LineWidth',2);
144 |     
145 |     %Plot Pendulum
146 |     Rod_X = [Cart(1), Bob(1)];
147 |     Rod_Y = [Cart(2), Bob(2)];
148 |     plot(Rod_X,Rod_Y,'k-','LineWidth',2)
149 |     
150 |     %Plot Bob and hinge
151 |     plot(Bob(1),Bob(2),'k.','MarkerSize',30)
152 |     plot(Cart(1),Cart(2),'k.','MarkerSize',30)
153 |     
154 |     %Plot Rails
155 |     plot([Limits(1) Limits(2)],-0.5*Cart_Height*[1,1],'k-','LineWidth',2)
156 |     
157 |     %Plot trace of the Bob's path:
158 |     plot(param.Bob(1,:),param.Bob(2,:),'k:','LineWidth',1)
159 |     
160 |     
161 |     %These commands keep the window from automatically rescaling in funny ways.
162 |     axis(Limits);
163 |     axis('equal');
164 |     axis manual;
165 |     axis off;
166 |     
167 | end
168 | end


--------------------------------------------------------------------------------
/matlab_implementation/res/anim.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/vvrs/MPPIController/bdad9758039e8ec7572516596f465fd164c3ecb2/matlab_implementation/res/anim.gif


--------------------------------------------------------------------------------
/matlab_implementation/res/states.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/vvrs/MPPIController/bdad9758039e8ec7572516596f465fd164c3ecb2/matlab_implementation/res/states.png


--------------------------------------------------------------------------------
/matlab_implementation/totalEntropy.m:
--------------------------------------------------------------------------------
 1 | function [entropy] = totalEntropy(Sk , del_uk, param)
 2 |     % Calculation of expectency over trajectory 
 3 |     
 4 |     % Normalization of cost function 
 5 | %     Sk = Sk./sum(Sk);
 6 | 
 7 | 
 8 |     n = length(Sk);
 9 |     lambda = param.lambda;
10 |     sum1 = 0;
11 |     sum2 = 0;
12 |     for i = 1:n
13 |         sum1 = sum1 + exp(-(1/lambda)*Sk(i))*del_uk(i);
14 |         sum2 = sum2 + exp(-(1/lambda)*Sk(i));
15 |     end
16 |     entropy = sum1/sum2;
17 | 
18 | end


--------------------------------------------------------------------------------
/mkdocs.yml:
--------------------------------------------------------------------------------
 1 | site_name: MPPI
 2 | # theme:
 3 | #   name: null
 4 | #   custom_dir: mkdocs-material/material
 5 | 
 6 | #   # 404 page
 7 | #   static_templates:
 8 | #     - 404.html
 9 | 
10 | #   # Necessary for search to work properly
11 | #   include_search_page: false
12 | #   search_index_only: true
13 | 
14 | #   # Default values, taken from mkdocs_theme.yml
15 | #   language: en
16 | #   font:
17 | #     text: Roboto
18 | #     code: Roboto Mono
19 | #   favicon: assets/favicon.png
20 | #   icon:
21 | #     logo: logo
22 | 


--------------------------------------------------------------------------------
/python_implementation/MPPI_test.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | 
  3 | 
  4 | 
  5 | l = 0.25
  6 | g = 9.81
  7 | 
  8 | 
  9 | def pendulum_dynamics(x, x_dot, theta, theta_dot, u, mc, mp):
 10 |     dx = [x_dot,
 11 |           (u + mp * np.sin(theta) * (l * theta_dot ** 2 + g * np.cos(theta))) / (mc + mp * np.sin(theta) ** 2),
 12 |           theta_dot,
 13 |           (-u * np.cos(theta) - mp * l * theta_dot ** 2 * np.cos(theta) * np.sin(theta) - (mc + mp) * g * np.sin(
 14 |               theta)) / (mc + mp * np.sin(theta) ** 2)]
 15 |     return np.array(dx)
 16 | 
 17 | 
 18 | 
 19 | 
 20 | def cost_function(p, p_dot, theta, theta_dot, u):
 21 |     dt = 0.02
 22 |     S = (20 * np.abs(p) ** 2 + 100 * (1 + np.cos(theta)) ** 2 + 1 * theta_dot ** 2 + 1 * p_dot ** 2) * dt / 10
 23 |     if p > 8:
 24 |         S += 0
 25 |     return S
 26 | 
 27 | 
 28 | 
 29 | 
 30 | def total_entropy(Sk, del_uk):
 31 |     n = len(Sk)
 32 |     lambda_ = 10
 33 |     sum1 = 0
 34 |     sum2 = 0
 35 |     for i in range(n):
 36 |         sum1 += np.exp(-(1 / lambda_) * Sk[i]) * del_uk[i]
 37 |         sum2 += np.exp(-(1 / lambda_) * Sk[i])
 38 | 
 39 |     entropy = sum1 / sum2
 40 |     return entropy
 41 | 
 42 | 
 43 | 
 44 | 
 45 | def main_loop():
 46 |     K = 1000
 47 |     N = 50
 48 |     iteration = 200
 49 |     dt = 0.02
 50 | 
 51 |     mc = 1
 52 |     mp = 0.01
 53 |     l = 0.25
 54 |     g = 9.81
 55 |     lambda_ = 1
 56 |     variance = 100
 57 |     x_init = np.array([0, 0, 0, 0])
 58 |     x_fin = np.array([0, 0, np.pi, 0])
 59 | 
 60 |     X_sys = np.zeros((4, iteration+1))
 61 | 
 62 |     U_sys = np.zeros(iteration)
 63 |     cost = np.zeros(iteration)
 64 | 
 65 |     X_sys[:, 1] = x_init
 66 | 
 67 |     u = np.zeros(N)
 68 |     # x = np.zeros((4, N))
 69 |     delta_u = np.zeros((N, K))
 70 |     u_init = 1
 71 | 
 72 |     # MPPI LOOP
 73 |     x = np.zeros((4,N))
 74 | 
 75 |     for j in range(iteration):
 76 |         Stk = np.zeros(K)
 77 | 
 78 |         for k in range(K):
 79 |             x[:, 1] = x_init
 80 |             for i in range(N-1):
 81 |                 # print "i>>", i, " k>>", k
 82 |                 delta_u[i, k] = variance * float(np.random.normal(1, 1, 1))
 83 |                 x[:, i+1] = x[:, i] + pendulum_dynamics(x[0, i], x[1, i], x[2, i], x[3, i], (u[i] + delta_u[i, k]), mc, mp)*dt
 84 |                 # cost_val = cost_function(x[0, i+1], x[1, i+1], x[2, i+1], x[3, i+1], (u[i] + delta_u[i, k]))
 85 |                 # Stk[k] += cost_val
 86 |                 Stk[k] += cost_function(x[0, i+1], x[1, i+1], x[2, i+1], x[3, i+1], (u[i] + delta_u[i, k]))
 87 |             delta_u[N-1, k] = variance * float(np.random.normal(1,1,1))
 88 |         # print "-"*80
 89 |         for i in range(N):
 90 |             u[i] += total_entropy(Stk, delta_u[i, :])
 91 |         U_sys[j] = u[0]
 92 |         X_sys[:, j+1] = X_sys[:, j] + pendulum_dynamics(X_sys[0, j], X_sys[1, j], X_sys[2, j], X_sys[3, j], u[1],mc,mp)*dt
 93 |         cost[j + 1] = cost_function(X_sys[0, j+1], X_sys[1, j+1], X_sys[2, j+1], X_sys[3, j+1], (u[i]+delta_u[i, k]))
 94 |         for i in range(N-1):
 95 |             u[i] = u[i+1]
 96 |         u[N-1] = u_init
 97 |         x_init = X_sys[:, j+1]
 98 | 
 99 | 
100 | 
101 | 
102 | main_loop()
103 | 
104 | print("MPPI controller for inverted pendulum cart pole")
105 | 


--------------------------------------------------------------------------------
/report/presentation_MPPI.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/vvrs/MPPIController/bdad9758039e8ec7572516596f465fd164c3ecb2/report/presentation_MPPI.pdf


--------------------------------------------------------------------------------
/report/report_MPPI.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/vvrs/MPPIController/bdad9758039e8ec7572516596f465fd164c3ecb2/report/report_MPPI.pdf


--------------------------------------------------------------------------------