Notes_04_06
page 1 of 9
D-mechanism
B
units = mm
C
A
30
y1
x1
3
D
45
10
y3
x3
P
105
50
B3
C3
B2
15
2
C4
x2
y2
4
A2
y4
5
P3
x4
30
D4
CONSTANT LOCAL BODY-FIXED LOCATIONS OF SPECIFIC POINTS
{s1}’
A
{
{s2}’
,
}
T
B
C
D
P
{
,
}
{s3}’
{
,
}
{
,
}
{s4}’
T
T
,
}
{
,
}
T
{
T
T
{
,
}
T
{
,
}
{
,
}
T
T
Notes_04_06
page 2 of 9
{q} = { x2 y2 2 x3 y3 3 x4 y4 4 }T
ESTIMATED GLOBAL POSE OF COORDINATE FRAMES
Link
1
Origin {ri}
Angle i



  


 2
2
{ 0, 0 }
T
3
{
0 deg
,
}
T
4
{
,
30 deg
}
T
{
,
}
deg
deg
r2 A  r1 A
r3 B  r2 B
r4 C  r3 C
r4 D  r1 D
  2 _ START
  I 2 

  I 2 
 q  0 2 x 2 

0 2 x 2 
0 
 1x 2
 




  0


  2 t 
 B 2 s 2 ' A
 B 2 s 2 ' B
0 2 x1 
0 2 x1 
1
0 2 x 2 
 I 2 
 I 2 
0 2 x 2 
01x 2 
B
0 2 x1 
 B 3 s 3 ' B
 B 3 s 3 ' C
0 2 x1 
0
0 2 x 2 
0 2 x 2 
 I 2 
 I 2 
01x 2 
0 2 x1  
0 2 x1  
 B 4 s 4 ' C 

 B 4 s 4 ' D 


0
B
15
50
50
C
A
15
C
A
MAX
54.08
30
D
54.08
L4
D
T
Notes_04_06
page 3 of 9
D mechanism - L4 = 30
40
30
yP [mm]
20
10
0
-10
-20
70
80
90
100
110
xP [mm]
120
130
140
130
140
D mechanism - L4 = 19.1
40
30
yP [mm]
20
10
0
-10
-20
70
80
90
100
110
xP [mm]
120
Notes_04_06
page 4 of 9
100
40
30
Omega4 [rad/s]
50
20
yP [mm]
L4=30
L4=25
L4=20
L4=19.1
10
0
0
-50
-10
-20
80
100
xP [mm]
120
-100
140
0
50
100
150
200
Phi2 [deg]
250
300
350
0
50
100
150
200
Phi2 [deg]
250
300
350
4
2
x 10
0
det(JAC) [mm2]
Alpha4 [rad/s 2]
1
0
-1
-2
-500
-1000
-3
-4
0
50
100
150
200
Phi2 [deg]
250
300
350
-1500
Notes_04_06
% dm_main.m - D-mechanism four-bar - ref: Haug, Fig P5.1, page 196
%
main
% HJSIII - 06.03.09
% initialize
dm_ini;
% one crank revolution
tpr = 2 * pi / w2;
dt = tpr / 200;
% starting position
phi2_start = 0 * d2r;
% time for one revolution
% 200 positions
% start at zero
% timer loop
keep = [];
for t = 0 : dt : tpr,
% kinematics
dm_kin;
% save
ang2
ang3
ang4
kinematics
= phi2 / d2r;
= phi3 / d2r;
= phi4 / d2r;
gamma = (phi3-phi4) / d2r;
detJAC = det(JAC);
x3P = r3P(1);
y3P = r3P(2);
x3Pd = r3Pd(1);
y3Pd = r3Pd(2);
keep = [ keep ; ang2 phi4d phi4dd detJAC x3P y3P x3Pd y3Pd ];
% bottom of crank rotation loop
end
% data for plotting
ang2 = keep(:,1);
phi4d = keep(:,2);
phi4dd = keep(:,3);
detJAC = keep(:,4);
x3P = keep(:,5);
y3P = keep(:,6);
x3Pd = keep(:,7);
y3Pd = keep(:,8);
% four figures per page
figure( 1 )
% locus of coupler point
subplot( 2,2,1 )
plot( x3P,y3P,plot_str )
hold on
axis( [70 140 -25 45] )
axis square
xlabel('xP [mm]')
ylabel('yP [mm]')
% follower velocity
subplot( 2,2,2 )
plot( ang2,phi4d,plot_str )
hold on
axis( [ 0 360 -100 100 ] )
xlabel('Phi2 [deg]')
ylabel('Omega4 [rad/s]')
legend( 'L4=30', 'L4=25', 'L4=20', 'L4=19.1' )
page 5 of 9
Notes_04_06
% follower acceleration
subplot( 2,2,3 )
plot( ang2,phi4dd,plot_str )
hold on
axis( [ 0 360 -40000 20000 ] )
xlabel('Phi2 [deg]')
ylabel('Alpha4 [rad/s^2]')
% Jacobian
subplot( 2,2,4 )
plot( ang2,detJAC,plot_str )
hold on
axis( [ 0 360 -1500 0 ] )
xlabel('Phi2 [deg]')
ylabel('det(JAC) [mm^2]')
% coupler point velocity - quiver plot
npos = length( x3P );
iskip = (1:12:npos);
figure( 2 )
quiver( x3P(iskip),y3P(iskip), x3Pd(iskip),y3Pd(iskip) )
axis( [70 140 -25 45] )
axis square
hold on
plot( x3P(iskip),y3P(iskip),'o' )
xlabel('xP [mm]')
ylabel('yP [mm]')
title('D mechanism - L4 = 30')
title('D mechanism - L4 = 19.1')
page 6 of 9
Notes_04_06
% dm_ini.m - D-mechanism four-bar - ref: Haug, Fig P5.1, page 196
%
initialize constants and assembly guesses
% HJSIII - 06.03.09
% general constants
d2r = pi / 180;
R = [ 0 -1; 1 0 ];
% mechanism constants
len2 = 15;
len3 = 50;
len4 = 30;
% possible lengths for parameter study of link 4
len4 = 19.1;
plot_str = 'r';
len4 = 20;
plot_str = 'b';
len4 = 25;
plot_str = 'g';
len4 = 30;
plot_str = 'k';
s2pA = [ 0 0 ]';
s2pB = [ len2 0 ]';
s3pB = [ 0 0 ]';
s3pC = [ len3 0 ]';
s3pP = [ 105 -5.00 ]';
s4pC = [ 0 0 ]';
s4pD = [ len4 0 ]';
r1A = [ 0 30 ]';
r1D = [ 45 0 ]';
% initial guesses - angles measured by protractor
phi2 = 60*d2r;
phi3 = -25*d2r;
phi4 = -110*d2r;
q(1,1) = 0;
q(2,1) = 30;
q(3,1) = phi2;
q(4,1) = len2*cos(phi2)+q(1);
q(5,1) = len2*sin(phi2)+q(2);
q(6,1) = phi3;
q(7,1) = len3*cos(phi3)+q(4);
q(8,1) = len3*sin(phi3)+q(5);
q(9,1) = phi4;
% driver for crank - phi2 = phi2_start + w2*t
phi2_start = 30 * d2r;
w2 = +1000 * 2 * pi / 60;
% 1000 rpm CCW, convert to rad/sec
t = 0;
page 7 of 9
Notes_04_06
% dm_phi.m - D-mechanism four-bar - ref: Haug, Fig P5.1, page 196
%
evaluate constraints and Jacobian for crank driving constraint
% HJSIII - 06.03.09
% global location of local frames and rotation matrices
% Eq 2.4.4, page 33 - Eq 2.6.5, page 42
r2 = q(1:2);
r3 = q(4:5);
r4 = q(7:8);
phi2 = q(3);
phi3 = q(6);
phi4 = q(9);
A2 = [ cos(phi2) -sin(phi2); sin(phi2) cos(phi2) ];
A3 = [ cos(phi3) -sin(phi3); sin(phi3) cos(phi3) ];
A4 = [ cos(phi4) -sin(phi4); sin(phi4) cos(phi4) ];
B2 = R * A2;
B3 = R * A3;
B4 = R * A4;
% global
r2A = r2
r2B = r2
r3B = r3
r3C = r3
r4C = r4
r4D = r4
r3P = r3
locations - Eq 2.4.8, page 33
+ A2*s2pA;
+ A2*s2pB;
+ A3*s3pB;
+ A3*s3pC;
+ A4*s4pC;
+ A4*s4pD;
+ A3*s3pP;
% revolute
PHI(1:2,1)
PHI(3:4,1)
PHI(5:6,1)
PHI(7:8,1)
constraints - A,B,C,D - Eq 3.3.10, page 65
= r2A – r1A;
= r3B – r2B;
= r4C – r3C;
= r4D - r1D;
% crank driving constraint
PHI(9,1) = phi2 - phi2_start - w2*t;
% Jacobian by rows - Eq 3.3.12, page 66 for revolutes
JAC = zeros(9,9);
JAC(1:2,1:3) = [ eye(2)
B2*s2pA ];
JAC(3:4,1:3) = [ -eye(2)
JAC(3:4,4:6) = [ eye(2)
-B2*s2pB ];
B3*s3pB ];
JAC(5:6,4:6) = [ -eye(2)
JAC(5:6,7:9) = [ eye(2)
-B3*s3pC ];
B4*s4pC ];
JAC(7:8,7:9) = [
eye(2)
B4*s4pD ];
% driving constraint in Jacobian - Eq 3.1.9, page 52
JAC(9,3) = 1;
% current results
current_crank = phi2 / d2r;
page 8 of 9
Notes_04_06
% dm_kin.m - D-mechanism four-bar - ref: Haug, Fig P5.1, page 196
%
positon, velocity, and acceleration at desired_crank
% HJSIII - 06.03.09
% Newton-Raphson position solution - Eq 3.6.7 and 3.6.8, page 100
assy_tol = 0.00001;
dm_phi;
while max(abs(PHI)) > assy_tol,
q = q - inv(JAC) * PHI;
dm_phi;
end
% velocity - Eq 3.1.9, page 52 - also page 66 for revolutes
velrhs(9,1) = w2;
qd = inv(JAC) * velrhs;
% global velocities - Eq 2.6.4, page 41
r2d = qd(1:2);
r3d = qd(4:5);
r4d = qd(7:8);
phi2d = qd(3);
phi3d = qd(6);
phi4d = qd(9);
r3Pd = r3d + phi3d * B3 * s3pP;
% acceleration - Eq 3.1.10, page 53 - also page 66 for revolutes
accrhs(1:2,1) = A2*s2pA*phi2d*phi2d;
accrhs(3:4,1) = A3*s3pB*phi3d*phi3d - A2*s2pB*phi2d*phi2d;
accrhs(5:6,1) = A4*s4pC*phi4d*phi4d - A3*s3pC*phi3d*phi3d;
accrhs(7:8,1) = A4*s4pD*phi4d*phi4d;
accrhs(9,1) = 0;
qdd = inv(JAC) * accrhs;
% global accelerations
r2dd = qdd(1:2);
r3dd = qdd(4:5);
r4dd = qdd(7:8);
phi2dd = qdd(3);
phi3dd = qdd(6);
phi4dd = qdd(9);
r3Pdd = r3dd + phi3dd*B3*s3pP - phi3d*phi3d*A3*s3pP;
page 9 of 9