function slip % Spring Loaded Inverted Pendulum % % This code was written by Russ Tedrake . % Modified for 6.832 problem set by Rick Cory global m r0 theta0 k g plant_dt display_dt yd; m = 1; r0 = 1; % this is the touchdown angle for the passive system theta0 = pi/8; k = 300; g = 9.8; plant_dt = 1e-3; display_dt = 0.005; y0 = 1.5; yd = 1.5; E = 30; % simulate the passive system simulate(y0,E); end % ================================================================ % Simulates and draws the system. % YOU'LL NEED TO ADD CODE FOR THE DEADBEAT CONTROLLER. % =============================================================== function simulate(y,E) global m r0 theta0 k g plant_dt display_dt; %theta0 = .4; % this is a fixed touch down angle x = apex_to_state(y,E); t = 0; last_display_t = -inf; while (1) % x(5) below is just an indicator that tells you what mode you're in, % i.e. flight, flight-to-stance, stance-to-flight if (x(5)) x = x + plant_dt*flight_dynamics(x); if (x(2)r0 && x(3)>0) x = stance_to_flight(x); % check for falling if (x(3)<0 || x(4)<0), break; end end end t = t+plant_dt; if (t-last_display_t>display_dt) draw(x,t); last_display_t = t; end end end % === Returns the state for a given apex and energy ====== function x = apex_to_state(y,E) global m r0 theta0 k g; x = [0; y; sqrt(2/m*(E - m*g*y)); 0; 1]; end % === Flight-to-stance dynamics =================== function xp = flight_to_stance(x) % x = [x;y;xdot;ydot] % xp = [r,theta,rdot,thetadot] global m r0 theta0 k g; r = x(2)/cos(theta0); xp = [ r ; theta0; ... -x(3)*sin(theta0) + x(4)*cos(theta0); ... -(x(3)*cos(theta0) + x(4)*sin(theta0))/r; ... 0]; end % === Stance-to-flight dynamics ================ function x = stance_to_flight(xp) % x = [x;y;xdot;ydot] % xp = [r,theta,rdot,thetadot] global m r0 theta0 k g; x = [ -xp(1)*sin(xp(2));... xp(1)*cos(xp(2)); ... -xp(3)*sin(xp(2)) - xp(1)*xp(4)*cos(xp(2)); ... xp(3)*cos(xp(2)) - xp(1)*xp(4)*sin(xp(2)); ... 1]; end % === Flight dynamics ================================ function xdot = flight_dynamics(x) % x = [x;y;xdot;ydot] global m r0 theta0 k g; xdot = [x(3:4); 0; -g/m; 0]; end % === Stance dynamics =============================== function xpdot = stance_dynamics(xp) % xp = [r,theta,rdot,thetadot] global m r0 theta0 k g; xpdot = [xp(3:4); ... k/m*(r0-xp(1)) + xp(1)*xp(4)^2 - g*cos(xp(2)); ... (g*xp(1)*sin(xp(2)) - 2*xp(1)*xp(3)*xp(4))/(xp(1)^2); ... 0]; end % ==== The draw function ======================================= function draw(x,t) global m r0 theta0 k g; figure(25); set(gcf,'DoubleBuffer','on'); cla; hold on; if (x(5)>0) hip = x(1:2); toe = x(1:2) + r0*[sin(theta0); -cos(theta0)]; else hip = [-x(1)*sin(x(2)); x(1)*cos(x(2))]; toe = [0;0]; end if (nargin>1) title(['t = ', num2str(t)]); end line([hip(1);toe(1)], [hip(2); toe(2)],'Color',[0 0 0],'LineWidth',2); t = 0:0.1:2*pi; line(hip(1)+0.15*sin(t),hip(2)+0.15*cos(t),'Color',[0 0 0]); fill(hip(1)+0.15*sin(t),hip(2)+0.15*cos(t),[ 0.502 1.000 1.000 ]); line([-4,4],[0,0]); axis equal; drawnow; end