function double_pendulum(plotCM) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % Simulation of the Equations of Motion for a Planar Double % Pendulum (with zero initial velocity conditions) % % By: Alyssa Novelia (4/10/2014) % Revised: Daniel Kawano (12/18/2017) % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % Nested function layout: % start dragging mass 1 % drag mass 1 % stop dragging mass 1 % stop executing drag command % start dragging mass 2 % drag mass 2 % stop dragging mass 2 % stop executing drag command % get initial conditions % start animating motion % integrate EOM according to initial conditions % animate motion! % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % plotCM is a variable with a value of 0 or 1 that specifies whether to % plot the animation side-by-side with the motion of the representative % particle in the configuration space; 0 = no plot, 1 = include plot. % If no input argument is specified, then plotCM = 1 by default. if nargin == 0 plotCM = 1; end % m1 = upper hanging mass % m2 = lower hanging mass % L1 = rod connecting the origin to m1 % L2 = rod connecting m1 to m2 % Main parameters: lambda = 1; % rod length ratio, L2/L1 mu = 1; % mass ratio, m2/m1 % Animation is plotted faster when using fewer timesteps -- higher % animation speed, lower plotting frequency: animation_speed = 8; % Figure objects' properties: L1_length = 1; % length of L1 in MATLAB figure units L2_length = L1_length*lambda; max_xy = L1_length + L2_length + 0.5; % for XLim and YLim % Draw and set parameters of main figure: main_fig = figure; aH = axes('XLim', [-max_xy max_xy], 'YLim', [-max_xy max_xy]); axis equal; set(main_fig, 'Color', 'White', 'Name', 'Double pendulum', 'Position', [375 198 622 388]); title('Click and drag the upper hanging mass'); hold on; % Draw objects: origin = line('xdata', 0, 'ydata', 0, 'Marker', 'o', 'MarkerFaceColor', 'k', 'Color', 'k', 'LineWidth', 1); L1 = line('xdata', [0 0], 'ydata', [0 -L1_length], 'LineWidth', 2, 'Color', 'k'); L2 = line('xdata', [0 0], 'ydata', [-L1_length -(L1_length + L2_length)], 'LineWidth', 2, 'Color', 'k'); mass2 = line('xdata', 0, 'ydata', -(L1_length + L2_length), 'Marker', 'o', 'MarkerFaceColor', 'k', 'Color', 'k', 'LineWidth', 3); mass1 = line('xdata', 0, 'ydata', -L1_length, 'Marker', 'o', 'MarkerFaceColor', 'k', 'Color', 'k', 'LineWidth', 3); % Start mass1 interaction: set(mass1, 'ButtonDownFcn', @StartDragFcn1); % enable clicking mass1 % Call StartDragFcn1 when mass1 is clicked: set(main_fig, 'WindowButtonUpFcn', @StopDragFcn1); % execute this line when mouse button is released function StartDragFcn1(varargin) % Call dragging function when mass1 is dragged: set(main_fig, 'WindowButtonMotionFcn', @DraggingFcn1); end function DraggingFcn1(varargin) % Dragging mass1: set(mass1, 'Color', 'r'); % change mass1 color when object is dragged cursor_pt = get(aH, 'CurrentPoint'); % get real-time cursor position theta1_t0 = atan(cursor_pt(1,2)/cursor_pt(1,1)); % theta defined by MATLAB, not by coordinate system defined in notes (atan(Y/X)) if cursor_pt(1,1) < 0 theta1_t0 = pi + theta1_t0; % differentiate 2nd quadrant from 4th, 3rd from 1st end % Current masses' x and y coordinates: mass1_pt = L1_length*[cos(theta1_t0) sin(theta1_t0)]; mass2_pt = mass1_pt + L2_length*[0 -1]; % mass2 relative to mass 1 does not update % Update all objects' positions when dragged: set(mass1, 'xdata', mass1_pt(1,1), 'ydata', mass1_pt(1,2)); set(L1, 'xdata', [0 mass1_pt(1,1)], 'ydata', [0 mass1_pt(1,2)]); set(mass2, 'xdata', mass2_pt(1,1), 'ydata', mass2_pt(1,2)); set(L2, 'xdata', [mass1_pt(1,1) mass2_pt(1,1)], 'ydata', [mass1_pt(1,2) mass2_pt(1,2)]); end function StopDragFcn1(varargin) mass1_t0 = [get(mass1, 'xdata') get(mass1, 'ydata')]; % get mass1 position when released, use as initial condition set(mass1, 'Color', 'k'); set(main_fig, 'WindowButtonMotionFcn', ''); % disable dragging function when mouse button is released (IMPORTANT) title('Click and drag the lower hanging mass'); % update instruction, drag mass2 next % Start mass 2 interaction: set(mass2, 'ButtonDownFcn', @StartDragFcn2); % enable clicking mass2 % Call StartDragFcn1 when mass2 is clicked: set(main_fig, 'WindowButtonUpFcn', @StopDragFcn2); % execute this line when mouse button is released function StartDragFcn2(varargin) % Call dragging function when mass2 is dragged: set(main_fig, 'WindowButtonMotionFcn', @DraggingFcn2); end function DraggingFcn2(varargin) % Dragging mass 2: set(mass2, 'Color', 'r'); % change mass2 color when object is dragged cursor_pt = get(aH, 'CurrentPoint'); % get real-time cursor position cursor_rel_pt = cursor_pt(1,1:2) - mass1_t0; % position of cursor relative to m1 theta2_t0 = atan(cursor_rel_pt(1,2)/cursor_rel_pt(1,1)); % theta defined by MATLAB, not by coordinate system defined in notes if cursor_rel_pt(1,1) < 0 theta2_t0 = pi + theta2_t0; % differentiate 2nd quadrant from 4th, 3rd from 1st end % Calculate only mass2's x and y coordinates: mass2_pt = mass1_t0 + L2_length*[cos(theta2_t0) sin(theta2_t0)]; % Update mass2's and L2's positions when mass2 is dragged: set(mass2, 'xdata', mass2_pt(1,1), 'ydata', mass2_pt(1,2)); set(L2, 'xdata', [mass1_t0(1,1) mass2_pt(1,1)], 'ydata', [mass1_t0(1,2) mass2_pt(1,2)]); end function StopDragFcn2(varargin) mass2_t0 = [get(mass2, 'xdata') get(mass2, 'ydata')]; % get mass2 position when released, use as initial condition set(mass2, 'Color', 'k'); set(main_fig, 'WindowButtonMotionFcn', ''); % disable dragging function when mouse button is released (IMPORTANT) % Since our EOM is a function of theta1 and theta2, calculate % those values from positions of the masses. % Calculate initial theta1: theta1_t0_MAT = atan(mass1_t0(1,2)/mass1_t0(1,1)); if mass1_t0(1,1) < 0 theta1_t0_MAT = pi + theta1_t0_MAT; % differentiate 2nd quadrant from 4th, 3rd from 1st end % Calculate initial theta2. Note how theta2 is defined (angle of % L2 from the horizontal (MATLAB figure), or vertical (EOM)): mass2rel_t0 = mass2_t0 - mass1_t0; theta2_t0_MAT = atan(mass2rel_t0(1,2)/mass2rel_t0(1,1)); if mass2rel_t0(1,1) < 0 theta2_t0_MAT = pi + theta2_t0_MAT; % differentiate 2nd quadrant from 4th, 3rd from 1st end % Rotate MATLAB basis 90 deg CW to get EOM basis: theta1_t0_EOM = theta1_t0_MAT + pi/2; theta2_t0_EOM = theta2_t0_MAT + pi/2; % Create new subplot (for pendulum): if plotCM left_fig = subplot(1,2,1); else clf; left_fig = subplot(1,1,1); end title('Planar double pendulum'); axis equal; set(left_fig, 'XLim', [-max_xy max_xy], 'YLim', [-max_xy max_xy]); % Duplicate objects on left figure: Lorigin = line('xdata', 0, 'ydata', 0, 'Marker', 'o', 'MarkerFaceColor', 'k', 'Color', 'k', 'LineWidth', 1); Lmass2 = line('xdata', mass2_t0(1,1), 'ydata', mass2_t0(1,2), 'Marker', 'o', 'MarkerFaceColor', 'k', 'Color', 'k', 'LineWidth', 3); Lmass1 = line('xdata', mass1_t0(1,1), 'ydata', mass1_t0(1,2), 'Marker', 'o', 'MarkerFaceColor', 'k', 'Color', 'k', 'LineWidth', 3); LL1 = line('xdata', [0 mass1_t0(1,1)], 'ydata', [0 mass1_t0(1,2)], 'LineWidth', 2, 'Color', 'k'); LL2 = line('xdata', [mass1_t0(1,1) mass2_t0(1,1)], 'ydata', [mass1_t0(1,2) mass2_t0(1,2)], 'LineWidth', 2, 'Color', 'k'); Lpathline1 = line('xdata', 0, 'ydata', 0, 'LineWidth', 1, 'Color', 'r', 'LineStyle', '-'); Lpathline2 = line('xdata', 0, 'ydata', 0, 'LineWidth', 1, 'Color', 'b', 'LineStyle', '-'); if plotCM == 1 % Create new subplot (for configuration manifold): right_fig = subplot(1,2,2); title({'Representative particle''s trajectory'; 'on the configuration manifold'}); axis equal; set(right_fig, 'XLim', [-2 2], 'YLim', [-2 2], 'ZLim', [-2 2], ... 'CameraPosition', [-2 -2 2]); CM_path = line('xdata', 0, 'ydata', 0, 'zdata', 0, 'LineWidth', 2, 'Color', 'b'); CM_pointer = line('xdata', 0, 'ydata', 0, 'zdata', 0, 'LineWidth', 3, 'MarkerFaceColor', 'b', 'Marker', '.'); hold on; % Define torus radius: R2 = 0.5; R1 = 1.5; % Credits to % http://www.cise.ufl.edu/research/sparse/MATLAB/6thEdition/torus.m % for the torus code: [x y z] = torus(R2, 30, R1); torus_obj = surf(x,y,z, 'EdgeColor', [1 1 1]); alpha(0.1); end % Run plotting function: plotdoublependulum(theta1_t0_EOM, theta2_t0_EOM); function plotdoublependulum(theta1, theta2) % Set ode45 tolerance: tol = 1e-8; options = odeset('abstol', tol, 'reltol', tol); % Run ode45. % Y = [theta1, theta2, theta1', theta2']: [T Y] = ode45(@doublependulumEOM, [0 30], [theta1 theta2 0 0], options); % LEFT SUBPLOT % Bound the angles from -pi to pi: theta1_EOM = normalize_theta(Y(:,1)); theta2_EOM = normalize_theta(Y(:,2)); % Calculate the masses' positions (in MATLAB figure) over % time from the angles defined by EOM. % % Rotate e_r_EOM 90 deg CCW: % % [ 0 1]* [cos(theta_EOM)] = [sin(theta_EOM) ] % [-1 0] [sin(theta_EOM)] [-cos(theta_EOM)] mass1_MAT = L1_length*[sin(theta1_EOM) -cos(theta1_EOM)]; mass2_MAT = mass1_MAT + L2_length*[sin(theta2_EOM) -cos(theta2_EOM)]; % RIGHT SUBPLOT % Config space position: r = R1(cos(x)E1 + sin(x)E2) + % R2(cos(y)E2 + sin(y)E3): if plotCM rx = R1*cos(theta1_EOM) + R2*cos(theta2_EOM).*cos(theta1_EOM); ry = R1*sin(theta1_EOM) + R2*cos(theta2_EOM).*sin(theta1_EOM); rz = R2*sin(theta2_EOM); end % Draw the animation sequence: for iii = 1:animation_speed:size(Y,1) % Left figure (real space): set(Lpathline1, 'xdata', mass1_MAT(1:iii), 'ydata', mass1_MAT(1:iii,2)); set(LL1, 'xdata', [0 mass1_MAT(iii,1)], 'ydata', [0 mass1_MAT(iii,2)]); set(Lmass1, 'xdata', mass1_MAT(iii,1), 'ydata', mass1_MAT(iii,2)); set(Lpathline2, 'xdata', mass2_MAT(1:iii), 'ydata', mass2_MAT(1:iii,2)); set(LL2, 'xdata', [mass1_MAT(iii,1) mass2_MAT(iii,1)], 'ydata', [mass1_MAT(iii,2) mass2_MAT(iii,2)]); set(Lmass2, 'xdata', mass2_MAT(iii,1), 'ydata', mass2_MAT(iii,2)); % Right figure (configuration space): if plotCM set(CM_path, 'xdata', rx(1:iii), 'ydata', ry(1:iii), 'zdata', rz(1:iii)); set(CM_pointer, 'xdata', rx(iii), 'ydata', ry(iii), 'zdata', rz(iii)); end drawnow; end % Create phase portrait for theta1: pplane_theta = figure; set(pplane_theta, 'Color', 'White', 'Name', 'Phase portaits', 'Position', [375 198 622 388]); xmax = max(max(theta1_EOM, theta2_EOM)); xmin = min(min(theta1_EOM, theta2_EOM)); ymax = max(max(Y(:,3), Y(:,4))); ymin = min(min(Y(:,3), Y(:,4))); pplane_theta1 = subplot(1,2,1); plot(theta1_EOM, Y(:,3), 'r', 'linewidth', 2); set(pplane_theta1, 'Color', 'White'); xlabel('\theta_1'); ylabel('\theta_1^{\prime} ', 'rotation', 0); axis([xmin, xmax, ymin, ymax]); axis equal; grid on; % Create phase portrait for theta2: pplane_theta2 = subplot(1,2,2); plot(theta2_EOM, Y(:,4), '-b', 'linewidth', 2); set(pplane_theta2, 'Color', 'White'); xlabel('\theta_2'); ylabel('\theta_2^{\prime} ', 'rotation', 0); axis([xmin, xmax, ymin, ymax]); axis equal; grid on; % Plot x, y trajectory over time: trajectory = figure; set(trajectory, 'Color', 'White', 'Name', 'Trajectory of the masses', 'Position', [375 198 622 388]); % Mass 1 trajectory: subplot(2,1,1); plot(T, mass1_MAT(:,1), 'k', T, mass1_MAT(:,2), 'b', 'linewidth', 2); xlabel('Time, \tau'); ylabel('Displacement') legend('\itx\rm(\tau)', '\ity\rm(\tau)'); title('Displacement of mass \itm\rm_1'); grid on; % Mass 2 trajectory: subplot(2,1,2) plot(T, mass2_MAT(:,1), 'k', T, mass2_MAT(:,2), 'b', 'linewidth', 2); xlabel('Time, \tau'); ylabel('Displacement') legend('\itx\rm(\tau)', '\ity\rm(\tau)'); title('Displacement of mass \itm\rm_2'); grid on; end function dY = doublependulumEOM(T, Y) % Y = [theta1, theta2, theta1', theta2']; theta1 = Y(1); theta2 = Y(2); dtheta1 = Y(3); dtheta2 = Y(4); dY = zeros(4,1); % Pre-calculate M and f: M = [1+mu, mu*lambda*cos(theta1-theta2); mu*lambda*cos(theta1-theta2), mu*lambda^2]; f = [-(1+mu)*sin(theta1) - mu*lambda*(sin(theta1-theta2))*dtheta2^2; mu*lambda*(-sin(theta2) + (sin(theta1-theta2))*dtheta1^2)]; % Equations of motion for planar double pendulum in state % space: dY = [dtheta1; dtheta2; M\f]; end end end end