Main subroutine (madc.m):
%====================================
% Matlab implementation of Method of
% Appended Driving Constraints (MADC)
%
% Runs faster under Matlab 6.0 than 7.0 or 5.3
%
% Modified: 31/01/05 by J. McPhee.
% Modified: 28/01/02 by J. McPhee.
% Modified: 15/02/00 by J. McPhee.
% Created: 12/02/97 by J. McPhee.
%=====================================
clear
global L1 L2 t
%
% Slider-crank example in joint coordinates.
%
% Define constant link lengths for slider-crank example,
% time parameters, and initial guess for displacements,
% and pre-allocate the matrices containing results...
L1 = 1; % length of crank
L2 = 2; % length of connecting rod
tstart = 0;
tend = 1.0;
npoints= 100;
q0 = [0 0 L1+L2]; % crank angle, con_rod angle, slider pos.
q = zeros(3,1);
qd = zeros(3,1);
qdd= zeros(3,1);
tp = zeros(npoints+1,1);
qp = zeros(npoints+1,3);
qdp = zeros(npoints+1,3);
qddp= zeros(npoints+1,3);
time0 = clock;
%
% Begin time loop and solve equations for position,
% velocity, and acceleration analyses at each step...
t= tstart;
dt = (tend-tstart)/npoints;
for i=1:npoints+1,
%
% Solve nonlinear positions equations in poseqns.m
% using q0 (from previous step) as initial guess.
% Note that the Jacobian could be included in poseqns.m
% to speed up convergence of the Gauss-Newton iteration...
%
options = optimset('Display','off', ... % turn off Display
'LargeScale','off', ...
'NonlEqnAlgorithm','gn', ... % use Gauss-Newton iteration
'Jacobian','on'); % Jacobian matrix in poseqns
q= fsolve('poseqns',q0,options);
q0= q;
%
% Now solve linear equations for velocities and
% accelerations, using appropriate function files...
% (Note the use of the transpose (') operator)
%
qd= (jacobian(q)\rhs_vel(q)')';
qdd= (jacobian(q)\rhs_acc(q,qd)')';
%
%
% Write results to arrays for subsequent plotting ...
%
qp(i,:)= q;
qdp(i,:)= qd;
qddp(i,:)= qdd;
tp(i)= t;
t= t+dt;
end
% Elapsed cpu time...
cpu = etime(clock,time0);
disp(['Elapsed CPU time is ',num2str(cpu),' seconds.']);
%
% Plot results...
%
figure(1)
plot(tp(:),qp(:,2)*180/pi) % convert to degrees
%axis([0 1 -40 40]) % specify
the axis limits
axis tight
title('\theta_2 versus Time')
xlabel('Time (s)');
ylabel('\theta_2 (deg)');
% print -dps plot1;
figure(2)
plot(tp(:),qp(:,3))
%axis([0 1 1 3])
axis tight
title('x_3 versus Time')
xlabel('Time (s)');
ylabel('x_3 (m)');
% print -dps plot2;
figure(3)
subplot(2,1,1); % combine 2
plots in one figure
plot(tp(:),qdp(:,2))
title('\theta_2dot versus Time')
xlabel('Time (s)');
ylabel('\theta_2dot (rad/s)');
axis tight
subplot(2,1,2);
plot(tp(:),qdp(:,3))
title('x_3dot versus Time')
xlabel('Time (s)');
ylabel('x_3dot (m/s)');
axis tight
% print -dps plot3;
figure(4)
subplot(2,1,1);
plot(tp(:),qddp(:,2))
title('\theta_2ddot versus Time')
xlabel('Time (s)');
ylabel('\theta_2ddot (rad/s/s)');
axis tight
subplot(2,1,2);
plot(tp(:),qddp(:,3))
title('x_3ddot versus Time')
xlabel('Time (s)');
ylabel('x_3ddot (m/s/s)');
axis tight
% print -dps plot4;
Constraint and driver equations, plus Jacobian (poseqns.m):
function [eqns,Jac] =poseqns(q);
global L1 L2 t
%
% Constraint and driver equations for slider-crank...
%
eqns(1) = q(3) - L1*cos(q(1)) - L2*cos(q(2)) ;
eqns(2) = L1*sin(q(1)) - L2*sin(q(2)) ;
eqns(3) = q(1) - 2*pi*t ;
%
% If the Jacobian matrix is available (jacobian.m), use
% it to speed up convergence of fsolve...
%
Jac = jacobian(q);
function j = jacobian(q)
%
% Jacobian of constraint equations in poseqns.m
%
global L1 L2
%
% Slider-crank example.
%
j(1,1)= L1*sin(q(1));
j(2,1)= L1*cos(q(1));
j(3,1)= 1;
j(1,2)= L2*sin(q(2));
j(2,2)= -L2*cos(q(2));
j(3,2)= 0;
j(1,3)= 1;
j(2,3)= 0;
j(3,3)= 0;
Right-hand side of velocity equations (rhs_vel.m):
function rhs = rhs_vel(q)
%
% Right-hand side of velocity equations...
%
global L1 L2
%
% Slider-crank:
%
rhs(1) = 0;
rhs(2) = 0;
rhs(3) = 2*pi;
Right-hand side of acceleration equations (rhs_acc.m):
function rhs = rhs_acc(q,qd)
%
% Right-hand side of acceleration equations...
%
global L1 L2
%
% Slider-crank example.
%
rhs(1)= -L1*qd(1)*qd(1)*cos(q(1))-L2*qd(2)*qd(2)*cos(q(2));
rhs(2)= L1*qd(1)*qd(1)*sin(q(1))-L2*qd(2)*qd(2)*sin(q(2));
rhs(3)= 0;