function cartpole % Parameters: mc = 10; mp = 1; l = 0.5; g = 9.8; %mc = 1; mp = 1; l = 1; g = 1; T = 15; plant_dt = 1e-3; display_dt = 0.1; % Initial Conditions: randn('state',sum(100*clock)) % x = [x,\theta,\dot{x},\dot\theta]^T % (set this to [0;0;0;0] to test true swing-up) x = 0.1*randn(4,1); %x = zeros(4,1); % Euler Integration Loop: last_display_t = -inf; for t=0:plant_dt:T u = control(x,t); if (t>last_display_t + display_dt) draw(x,t); last_display_t = t; % Energy calc (good way to verify eqs. of motion) % T = 0.5*(mc+mp)*x(3)^2 + mp*x(3)*x(4)*l*cos(x(2)) + 0.5*mp*l^2*x(4)^2; % U = -mp*g*l*cos(x(2)); % E = T+U end xdot = dynamics(x,u); x = x + plant_dt*xdot; end draw(x,t); function u = control(x,t) % TO DO: Fill in your control here u = 0; end function xdot = dynamics(x,u) s = sin(x(2)); c = cos(x(2)); % H = [mc+mp, mp*l*c; mp*l*c, mp*l^2]; % C = [0 -mp*x(4)*l*s; 0 0]; % G = [0; mp*g*l*s]; % B = [1; 0]; % xdot = [x(3:4); inv(H)*[B*u - C*x(3:4) - G]]; xddot = [u + mp*s*(l*x(4)^2 + g*c)]/[mc+mp*s^2]; tddot = [-u*c - mp*l*x(4)^2*c*s - (mc+mp)*g*s]/[l*(mc+mp*s^2)]; xdot = [x(3:4); xddot; tddot]; end function draw(x,t) persistent hFig base a1 raarm wb lwheel rwheel; if (isempty(hFig)) hFig = figure(25); set(hFig,'DoubleBuffer', 'on'); a1 = l+0.25; av = pi*[0:.05:1]; theta = pi*[0:0.05:2]; wb = .3; hb=.15; aw = .01; wheelr = 0.05; lwheel = [-wb/2 + wheelr*cos(theta); -hb-wheelr + wheelr*sin(theta)]'; base = [wb*[1 -1 -1 1]; hb*[1 1 -1 -1]]'; arm = [aw*cos(av-pi/2) -a1+aw*cos(av+pi/2) aw*sin(av-pi/2) aw*sin(av+pi/2)]'; raarm = [(arm(:,1).^2+arm(:,2).^2).^.5, atan2(arm(:,2),arm(:,1))]; end figure(hFig); cla; hold on; view(0,90); patch(x(1)+base(:,1), base(:,2),0*base(:,1),'b','FaceColor',[.3 .6 .4]) patch(x(1)+lwheel(:,1), lwheel(:,2), 0*lwheel(:,1),'k'); patch(x(1)+wb+lwheel(:,1), lwheel(:,2), 0*lwheel(:,1),'k'); patch(x(1)+raarm(:,1).*sin(raarm(:,2)+x(2)-pi),-raarm(:,1).*cos(raarm(:,2)+x(2)-pi), 1+0*raarm(:,1),'r','FaceColor',[.9 .1 0]) plot3(x(1)+l*sin(x(2)), -l*cos(x(2)),1, 'ko',... 'MarkerSize',10,'MarkerFaceColor','b') plot3(x(1),0,1.5,'k.') title(['t = ', num2str(t,'%.2f') ' sec']); set(gca,'XTick',[],'YTick',[]) axis image; axis([-2.5 2.5 -1.5*l 1.5*l]); drawnow; end end