optional) mean of state
%   S          (optional) covariance of state
%
%
% Copyright (C) 2008-2013 by
% Marc Deisenroth, Andrew McHutchon, Joe Hall, and Carl Edward Rasmussen.
%
% Last modified: 2013-03-08


function draw_pendubot(theta1, theta2, force, cost, text1, text2, M, S)
%% Code

l = 0.6;
xmin = -2*l; 
xmax = 2*l;    
umax = 2;
height = 0;

% Draw double pendulum
clf; hold on
sth1 = sin(theta1); sth2 = sin(theta2);
cth1 = cos(theta1); cth2 = cos(theta2);
pendulum1 = [0, 0; -l*sth1, l*cth1];
pendulu