46

Computer Implementation: Specialized Approach

• In this example, frames are assumed to be at joints. This is a deviation from our approach that we

will not repeat.

• It is done to produce a more compact code and show how the Newton-Raphson algorithm.

Example: Kinematic Analysis of a four-bar mechanism.

X1

Y1X3

θ2

O

Y3

Y2θ3A X4

Y4B

Cθ1

θ4

X2

System coordinates are:

q = �θ2θ3θ4�

47

The machine has one degree of freedom (𝜃𝜃2). Therefore the kinematic constraint equation is,

�𝑅𝑅1𝑥𝑥𝑅𝑅1𝑦𝑦𝜃𝜃2

� = �𝑙𝑙2 cos𝜃𝜃2 + 𝑙𝑙3 cos 𝜃𝜃3 + 𝑙𝑙4 cos𝜃𝜃4 − 𝑙𝑙1 cos 𝜃𝜃1𝑙𝑙2 sin𝜃𝜃2 + 𝑙𝑙3 sin𝜃𝜃3 + 𝑙𝑙4 sin𝜃𝜃4 − 𝑙𝑙1 sin𝜃𝜃1

𝜃𝜃2 − 𝜔𝜔2𝑡𝑡 − 𝜃𝜃𝑜𝑜2� = �

000�

The above equation is based on the assumption that the crank runs with a uniform velocity of 𝜔𝜔2 starting

at angle, 𝜃𝜃2𝑜𝑜.

The Jacobian matrix of the constraint equations,

𝐶𝐶𝑞𝑞 = �−𝑙𝑙2 sin𝜃𝜃2 −𝑙𝑙3 sin𝜃𝜃3 −𝑙𝑙4 sin𝜃𝜃4𝑙𝑙2 cos𝜃𝜃2 𝑙𝑙3 cos 𝜃𝜃3 𝑙𝑙4 cos𝜃𝜃4

1 0 0�

48

Velocity analysis

�̇�𝑞 = −𝐶𝐶𝑞𝑞−1𝐶𝐶𝑡𝑡

where,

𝐶𝐶𝑞𝑞 = �−𝑙𝑙2 sin𝜃𝜃2 −𝑙𝑙3 sin𝜃𝜃3 −𝑙𝑙4 sin𝜃𝜃4𝑙𝑙2 cos𝜃𝜃2 𝑙𝑙3 cos 𝜃𝜃3 𝑙𝑙4 cos𝜃𝜃4

1 0 0�

Ct = �00

−𝜔𝜔2�

Acceleration Analysis �̈�𝑞 = 𝐶𝐶𝑞𝑞−1𝑄𝑄𝑑𝑑

The vector Qd is

𝑄𝑄𝑑𝑑 = −�𝐶𝐶𝑞𝑞�̇�𝑞�𝑞𝑞�̇�𝑞 − 2𝐶𝐶𝑞𝑞𝑡𝑡�̇�𝑞 − 𝐶𝐶𝑡𝑡𝑡𝑡

Remember that Ct is,

𝐶𝐶𝑡𝑡 = �00

−𝜔𝜔2�

This matrix does not have any term that is in terms of time explicitly. Therefore,

𝐶𝐶𝑞𝑞𝑡𝑡 = 0

𝐶𝐶𝑡𝑡𝑡𝑡 = 0

𝐶𝐶𝑞𝑞�̇�𝑞 = �−𝑙𝑙2 sin𝜃𝜃2 −𝑙𝑙3 sin𝜃𝜃3 −𝑙𝑙4 sin𝜃𝜃4𝑙𝑙2 cos 𝜃𝜃2 𝑙𝑙3 cos𝜃𝜃3 𝑙𝑙4 cos 𝜃𝜃4

1 0 0� �θ̇2θ̇3θ̇4� = �

−θ̇2𝑙𝑙2 sin𝜃𝜃2 −θ̇3𝑙𝑙3 sin𝜃𝜃3 −θ̇4𝑙𝑙4 sin𝜃𝜃4θ̇2𝑙𝑙2 cos 𝜃𝜃2 +θ̇3𝑙𝑙3 cos 𝜃𝜃3 +θ̇4𝑙𝑙4 cos𝜃𝜃4

θ̇2�

The vector Qd is reduced to,

𝑄𝑄𝑑𝑑 = −�𝐶𝐶𝑞𝑞�̇�𝑞�𝑞𝑞�̇�𝑞 = −�−θ̇2𝑙𝑙2 cos 𝜃𝜃2 −θ̇3𝑙𝑙3 cos 𝜃𝜃3 −θ̇4𝑙𝑙4 cos𝜃𝜃4−θ̇2𝑙𝑙2 sin𝜃𝜃2 −θ̇3𝑙𝑙3 sin𝜃𝜃3 −θ̇4𝑙𝑙4 sin𝜃𝜃4

0 0 0� �θ̇2θ̇3θ̇4�

49

Matlab Program for Kinematic Analysis of a 4-Bar Mechanism

close all clear all % Analysis of a Four-bar Mechanism Using Newton Raphson's Method r1=0.35; %fixed link r2=0.2; %crank r3=0.35; %coupler r4=0.25; %output %The mechanism is a crank-rocker since 0.35+0.2 < 0.35+0.25 theta1=0; %fixed link angle %Initial Guess theta20=57.27; theta30=10; theta40=245; %input parameters t(1)=0; dt=0.02; %seconds omega2=5; % input velocity (rad/s) imax=64; %number of time steps converge=0; epsilon1=0.01; %dtheta accuracy epsilon2=0.01; %C accuracy i=1; while i<=imax t(i)=(i)*dt; theta2(i)=theta20+omega2*t(i)*(180/pi); %adjust initial conditions based on the step # if i==1 theta2(i)=theta20; theta3(i)=theta30; theta4(i)=theta40; else theta2(i)=theta2(i-1); theta3(i)=theta3(i-1); theta4(i)=theta4(i-1); end; %Newton Raphson for Position Analysis while converge==0 C=[r2*cosd(theta2(i))+r3*cosd(theta3(i))+r4*cosd(theta4(i))-r1*cosd(theta1);...

50

r2*sind(theta2(i))+r3*sind(theta3(i))+r4*sind(theta4(i))-r1*sind(theta1);... theta2(i)-omega2*t(i)*(180/pi)-theta20]; Cq=[-r2*sind(theta2(i)),-r3*sind(theta3(i)),-r4*sind(theta4(i));... r2*cosd(theta2(i)),r3*cosd(theta3(i)),r4*cosd(theta4(i));... 1,0,0]; dtheta=-inv(Cq)*C; %update thetas theta2(i)=theta2(i)+dtheta(1); theta3(i)=theta3(i)+dtheta(2); theta4(i)=theta4(i)+dtheta(3); %Termination Criteria if sum(abs(dtheta))<epsilon1 | sum(abs(C))<epsilon2 converge=1; i=i+1; end; end; converge=0; end; for i=1:imax %velocity Analysis Ct=[0;0;-omega2]; Cq=[-r2*sind(theta2(i)),-r3*sind(theta3(i)),-r4*sind(theta4(i));... r2*cosd(theta2(i)),r3*cosd(theta3(i)),r4*cosd(theta4(i));... 1,0,0]; vtheta=-inv(Cq)*Ct; theta2d(i)=vtheta(1,1); theta3d(i)=vtheta(2,1); theta4d(i)=vtheta(3,1); %Acceleration Analysis

Qd=-[-theta2d(i)*r2*cosd(theta2(i)),-theta3d(i)*r3*cosd(theta3(i)),-theta4d(i)*r4*cosd(theta4(i));...

-theta2d(i)*r2*sind(theta2(i)),-theta3d(i)*r3*sind(theta3(i)),-theta4d(i)*r4*sind(theta4(i));... 0,0,0]*[theta2d(i);theta3d(i);theta4d(i)]; atheta=inv(Cq)*Qd; theta2dd(i)=atheta(1,1); theta3dd(i)=atheta(2,1); theta4dd(i)=atheta(3,1); end; % Calculate joints xa=r2*cosd(theta2); ya=r2*sind(theta2); xb=r2*cosd(theta2)+r3*cosd(theta3); yb=r2*sind(theta2)+r3*sind(theta3);

51

plot(xa,ya,'r','LineWidth',2); hold on; plot(xb,yb,'g','LineWidth',2); legend ('Crank','Output Link') axis([-0.5 0.5 -0.5 0.5]) axis equal grid on %plot the links figure for i=1:imax %Generate the links at every instant x(1,i)=0; y(1,i)=0; x(2,i)=xa(i); y(2,i)=ya(i); x(3,i)=xb(i); y(3,i)=yb(i); x(4,i)=r1*cos(theta1); y(4,i)=r1*sin(theta1); plot(x(1:4,i),y(1:4,i)) hold on end; axis([-0.5 0.5 -0.5 0.5]) title ('motion of the links') axis equal grid on %plot angular velocities figure plot(t,theta2d,'-',t,theta3d,'--',t,theta4d,'-.','LineWidth',2) legend('omega_2','omega_3','omega_4','Location','SouthWest') title ('Angular velocities of the three links') grid on %plot angular accelerations figure plot(t,theta2dd,'-',t,theta3dd,'--',t,theta4dd,'-.','LineWidth',2) legend('alpha_2','alpha_3','alpha_4','Location','NorthWest') title ('Angular accelerations of the three links') grid on

52

-0.4 -0.2 0 0.2 0.4 0.6-0.5

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

0.5

Crank

Output Link

-0.4 -0.2 0 0.2 0.4 0.6-0.5

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

0.5motion of the links

53

0 0.2 0.4 0.6 0.8 1 1.2 1.4-10

-5

0

5Angular velocities of the three links

omega2

omega3

omega4

0 0.2 0.4 0.6 0.8 1 1.2 1.4-100

-50

0

50

100

150

200

250Angular accelerations of the three links

alpha2

alpha3

alpha4

54

Effect of Accuracy ε1 and ε2 ε1 = ε2 = 0.1

ε1 = ε2 = 0.01

ε1 = ε2 = 0.001

The results show a gradual improvement in the quality of the output curve.

-0.4 -0.2 0 0.2 0.4 0.6-0.5

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

0.5

Crank

Output Link

-0.4 -0.2 0 0.2 0.4 0.6-0.5

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

0.5

Crank

Output Link

-0.4 -0.2 0 0.2 0.4 0.6-0.5

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

0.5

Crank

Output Link

55

Homework • Develop the equations of motion for an offset crank-slider mechanism (Problem 3.4). • Write a program analyzing the motion of this mechanism using the approach presented in this

section. • Plot motion (displacement, velocity and acceleration of the end of the manipulator) versus time. • Compare results with those obtained using a commercial software.

56

Computer Implementation: Standardizing the Kinematics

• It is of interest to automate the generation of the constraint equations when creating a general-

purpose computer program for kinematic analysis.

Choice of the Coordinates: • A more standardized way to analyze kinematics by placing frame at the center of masses. This will

make the analysis more suited for dynamics.

X1

Y1

X3

θ2

O

Y3

X2Y2 θ3AX4

Y4

B

Cθ1

θ4

The ground constraints are,

�𝑅𝑅1𝑥𝑥𝑅𝑅1𝑦𝑦𝜃𝜃1

� = �000�

The revolute joint constraints are:

𝑅𝑅1 + 𝐴𝐴1 𝑢𝑢�1𝑂𝑂 − 𝑅𝑅2 − 𝐴𝐴2 𝑢𝑢�2𝑂𝑂 = 0

𝑅𝑅2 + 𝐴𝐴2 𝑢𝑢�2𝐴𝐴 − 𝑅𝑅3 − 𝐴𝐴3 𝑢𝑢�3𝐴𝐴 = 0

𝑅𝑅3 + 𝐴𝐴3 𝑢𝑢�3𝐵𝐵 − 𝑅𝑅4 − 𝐴𝐴4 𝑢𝑢�4𝐵𝐵 = 0

𝑅𝑅4 + 𝐴𝐴4 𝑢𝑢�4𝐶𝐶 − 𝑅𝑅1 − 𝐴𝐴41 𝑢𝑢�1𝐶𝐶 = 0

𝑢𝑢�𝑖𝑖𝑂𝑂 𝑢𝑢�𝑖𝑖𝐴𝐴 𝑢𝑢�𝑖𝑖𝐵𝐵 𝑢𝑢�𝑖𝑖𝐶𝐶 are the local position vectors of O A B C in the frame of link i. In this case, 𝑢𝑢�1𝑂𝑂 = 0

Ai is the transformation matrix from link i to the global coordinate system

Ri is the global position of the origin of frame i

57

Standard Constraint Library 1. Standard Constraint Library: Revolute Joints

• The kinematic constraint equation for two bodies connected through a revolute joint at point P is,

𝐶𝐶(𝑞𝑞, 𝑡𝑡) = 𝑅𝑅𝑖𝑖 + 𝐴𝐴𝑖𝑖 𝑢𝑢�𝑖𝑖𝑃𝑃 − 𝑅𝑅𝑗𝑗 − 𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃 = 0

Remember that C is 2x1.

In this case q is 6x1 matrix:

𝑞𝑞 = [𝑅𝑅𝑖𝑖 𝜃𝜃𝑖𝑖 𝑅𝑅𝑗𝑗 𝜃𝜃𝑗𝑗]𝑇𝑇 = �𝑅𝑅𝑖𝑖𝑥𝑥 𝑅𝑅𝑖𝑖𝑦𝑦 𝜃𝜃𝑖𝑖 𝑅𝑅𝑗𝑗𝑥𝑥 𝑅𝑅𝑗𝑗𝑦𝑦 𝜃𝜃𝑗𝑗�𝑇𝑇 • Similarly, the Jacobian matrix for this joint

𝐶𝐶𝑞𝑞 =𝜕𝜕𝐶𝐶𝜕𝜕𝑞𝑞

=

⎣⎢⎢⎢⎡𝜕𝜕𝐶𝐶1𝜕𝜕𝑞𝑞𝜕𝜕𝐶𝐶2𝜕𝜕𝑞𝑞 ⎦

⎥⎥⎥⎤

= �

𝜕𝜕𝐶𝐶1𝜕𝜕𝑅𝑅𝑖𝑖

𝜕𝜕𝐶𝐶1𝜕𝜕𝜃𝜃𝑖𝑖

𝜕𝜕𝐶𝐶1𝜕𝜕𝑅𝑅𝑗𝑗

𝜕𝜕𝐶𝐶1𝜕𝜕𝜃𝜃𝑗𝑗

𝜕𝜕𝐶𝐶2𝜕𝜕𝑅𝑅𝑖𝑖

𝜕𝜕𝐶𝐶2𝜕𝜕𝜃𝜃𝑖𝑖

𝜕𝜕𝐶𝐶2𝜕𝜕𝑅𝑅𝑗𝑗

𝜕𝜕𝐶𝐶2𝜕𝜕𝜃𝜃𝑗𝑗

�

Or,

𝐶𝐶𝑞𝑞 = � 𝜕𝜕𝐶𝐶𝜕𝜕𝑅𝑅𝑖𝑖

𝜕𝜕𝐶𝐶𝜕𝜕𝜃𝜃𝑖𝑖

𝜕𝜕𝐶𝐶𝜕𝜕𝑅𝑅𝑗𝑗

𝜕𝜕𝐶𝐶𝜕𝜕𝜃𝜃𝑗𝑗

� = [𝐼𝐼 𝐴𝐴𝑖𝑖𝜃𝜃 𝑢𝑢�𝑖𝑖𝑃𝑃 −𝐼𝐼 −𝐴𝐴𝑗𝑗𝜃𝜃 𝑢𝑢�𝑗𝑗𝑃𝑃]

where,

I is a 2x2 identity matrix

Aiθ is the partial derivative of Ai

𝐴𝐴𝑖𝑖𝜃𝜃 = �−sin𝜃𝜃𝑖𝑖 − cos𝜃𝜃𝑖𝑖cos 𝜃𝜃𝑖𝑖 −sin𝜃𝜃𝑖𝑖

�

Cq is a 2x6 matrix

58

Velocity Analysis

The velocity analysis is based on,

𝐶𝐶𝑞𝑞�̇�𝑞 = −𝐶𝐶𝑡𝑡

However. Ct=0 (Nothing in C for a revolute joint is an explicit function of time)

Acceleration Analysis

The acceleration analysis is based on,

�̈�𝑞 = 𝐶𝐶𝑞𝑞−1Qd = 𝐶𝐶𝑞𝑞−1 �−�𝐶𝐶𝑞𝑞�̇�𝑞�𝑞𝑞�̇�𝑞 − 2𝐶𝐶𝑞𝑞𝑡𝑡�̇�𝑞 − 𝐶𝐶𝑡𝑡𝑡𝑡�

This matrix does not have any term that is in terms of time explicitly. Therefore,

𝐶𝐶𝑞𝑞𝑡𝑡 = 0

𝐶𝐶𝑡𝑡𝑡𝑡 = 0

Similarly, �̇�𝑞 is not function of q, which means that,

�̇�𝑞𝑞𝑞 = 0

Therefore,

�̈�𝑞 = 𝐶𝐶𝑞𝑞−1Qd = 𝐶𝐶𝑞𝑞−1 �−�𝐶𝐶𝑞𝑞�̇�𝑞�𝑞𝑞�̇�𝑞�

Substituting the Jacobian matrix in the equation above,

�𝐶𝐶𝑞𝑞�̇�𝑞� = [𝐼𝐼 𝐴𝐴𝑖𝑖𝜃𝜃 𝑢𝑢�𝑖𝑖𝑃𝑃 −𝐼𝐼 −𝐴𝐴𝑗𝑗𝜃𝜃 𝑢𝑢�𝑗𝑗𝑃𝑃]�

�̇�𝑅𝑖𝑖

�̇�𝜃𝑖𝑖

�̇�𝑅𝑗𝑗

�̇�𝜃𝑗𝑗

� = ��̇�𝑅𝑖𝑖 + �̇�𝜃𝑖𝑖𝐴𝐴𝑖𝑖𝜃𝜃 𝑢𝑢�𝑖𝑖𝑃𝑃−�̇�𝑅𝑗𝑗 − �̇�𝜃𝑗𝑗𝐴𝐴𝑗𝑗𝜃𝜃 𝑢𝑢�𝑗𝑗𝑃𝑃�

�𝐶𝐶𝑞𝑞�̇�𝑞�𝑞𝑞 = �0 −�̇�𝜃𝑖𝑖𝐴𝐴𝑖𝑖 𝑢𝑢�𝑖𝑖𝑃𝑃 0 �̇�𝜃𝑗𝑗𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃�

where, 0 is a 2x2 zero matrix

Qd = −�𝐶𝐶𝑞𝑞�̇�𝑞�𝑞𝑞�̇�𝑞 = −�0 −�̇�𝜃𝑖𝑖𝐴𝐴𝑖𝑖 𝑢𝑢�𝑖𝑖𝑃𝑃 0 �̇�𝜃𝑗𝑗𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃� �

�̇�𝑅𝑖𝑖

�̇�𝜃𝑖𝑖

�̇�𝑅𝑗𝑗

�̇�𝜃𝑗𝑗

� = − �−�̇�𝜃𝑖𝑖2𝐴𝐴𝑖𝑖 𝑢𝑢�𝑖𝑖𝑃𝑃 + �̇�𝜃𝑗𝑗

2𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃�

Qd is a 2x1 matrix

59

Homework Using the standard constraint library, write a program analyzing the motion of a 2R manipulator using standard constraints equations. The system data are:

l2=l3=1m θ2o= θ 3o=0 �̇�𝜃2 = 1 𝑟𝑟𝑟𝑟𝑟𝑟/𝑠𝑠 �̇�𝜃3 = 2𝑡𝑡 𝑟𝑟𝑟𝑟𝑟𝑟/𝑠𝑠 Duration of the simulation is 2 seconds.

Plot motion (displacement, velocity and acceleration of the end of the manipulator) versus time. Compare results with those obtained using a commercial software.

60

2. Standard Constraint Library: Prismatic Joints

θi

Body j

Body i

Pj

Pi

hi Qirij P

• The kinematic constraint equation for two bodies connected through a prismatic joint is,

𝐶𝐶 = �𝐶𝐶1𝐶𝐶2� = �𝜃𝜃

𝑖𝑖 − 𝜃𝜃𝑗𝑗 − 𝑐𝑐ℎ𝑖𝑖𝑇𝑇𝑟𝑟𝑖𝑖𝑗𝑗𝑃𝑃

� = �00�

ℎ𝑖𝑖 = 𝐴𝐴𝑖𝑖ℎ�𝑖𝑖 = 𝐴𝐴𝑖𝑖� 𝑢𝑢�𝑖𝑖𝑃𝑃 − 𝑢𝑢�𝑖𝑖𝑄𝑄�

𝑟𝑟𝑖𝑖𝑗𝑗𝑃𝑃 = 𝑟𝑟𝑖𝑖𝑃𝑃 − 𝑟𝑟𝑗𝑗𝑃𝑃 = �𝑅𝑅𝑖𝑖 + 𝐴𝐴𝑖𝑖 𝑢𝑢�𝑖𝑖𝑃𝑃� − �𝑅𝑅𝑗𝑗 + 𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃�

• Therefore,

𝐶𝐶 = �𝐶𝐶1𝐶𝐶2� = �

𝜃𝜃𝑖𝑖 − 𝜃𝜃𝑗𝑗 − 𝑐𝑐�ℎ𝑖𝑖�𝑇𝑇𝑟𝑟𝑖𝑖𝑗𝑗𝑃𝑃

� = �𝜃𝜃𝑖𝑖 − 𝜃𝜃𝑗𝑗 − 𝑐𝑐

�𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇 ��𝑅𝑅𝑖𝑖 + 𝐴𝐴𝑖𝑖 𝑢𝑢�𝑖𝑖𝑃𝑃� − �𝑅𝑅𝑗𝑗 + 𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃��� = �00�

• The Jacobian matrix is,

𝐶𝐶 = 𝐶𝐶𝑞𝑞 =𝜕𝜕𝐶𝐶𝜕𝜕𝑞𝑞

=

⎣⎢⎢⎢⎡𝜕𝜕𝐶𝐶1𝜕𝜕𝑞𝑞𝜕𝜕𝐶𝐶2𝜕𝜕𝑞𝑞 ⎦

⎥⎥⎥⎤

= �

𝜕𝜕𝐶𝐶1𝜕𝜕𝑅𝑅𝑖𝑖

𝜕𝜕𝐶𝐶1𝜕𝜕𝜃𝜃𝑖𝑖

𝜕𝜕𝐶𝐶1𝜕𝜕𝑅𝑅𝑗𝑗

𝜕𝜕𝐶𝐶1𝜕𝜕𝜃𝜃𝑗𝑗

𝜕𝜕𝐶𝐶2𝜕𝜕𝑅𝑅𝑖𝑖

𝜕𝜕𝐶𝐶2𝜕𝜕𝜃𝜃𝑖𝑖

𝜕𝜕𝐶𝐶2𝜕𝜕𝑅𝑅𝑗𝑗

𝜕𝜕𝐶𝐶2𝜕𝜕𝜃𝜃𝑗𝑗

�

𝐶𝐶𝑞𝑞 = �0 1 0 −1

�𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇 �𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇 �(𝑅𝑅𝑖𝑖 + 𝐴𝐴𝑖𝑖 𝑢𝑢�𝑖𝑖𝑃𝑃) − (𝑅𝑅𝑗𝑗 + 𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃)� + �𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇𝐴𝐴𝑖𝑖𝜃𝜃 𝑢𝑢�𝑖𝑖𝑃𝑃 −�𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇 −�𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇𝐴𝐴𝑗𝑗𝜃𝜃 𝑢𝑢�𝑗𝑗𝑃𝑃

�

Velocity analysis The velocity analysis is based on,

𝐶𝐶𝑞𝑞�̇�𝑞 = −𝐶𝐶𝑡𝑡

However. Ct=0 (Nothing in C for a prismatic joint is usually an explicit function of time)

61

Acceleration Analysis

The acceleration analysis is based on,

�̈�𝑞 = 𝐶𝐶𝑞𝑞−1Qd = 𝐶𝐶𝑞𝑞−1 �−�𝐶𝐶𝑞𝑞�̇�𝑞�𝑞𝑞�̇�𝑞 − 2𝐶𝐶𝑞𝑞𝑡𝑡�̇�𝑞 − 𝐶𝐶𝑡𝑡𝑡𝑡�

This matrix does not have any term that is usually in terms of time explicitly. Therefore,

𝐶𝐶𝑞𝑞𝑡𝑡 = 0

𝐶𝐶𝑡𝑡𝑡𝑡 = 0

Therefore,

�̈�𝑞 = 𝐶𝐶𝑞𝑞−1Qd = 𝐶𝐶𝑞𝑞−1 �−�𝐶𝐶𝑞𝑞�̇�𝑞�𝑞𝑞�̇�𝑞�

Substituting the Jacobian matrix in the equation above,

�𝐶𝐶𝑞𝑞�̇�𝑞� = �0 1 0 −1

�𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇 �𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇 �(𝑅𝑅𝑖𝑖 + 𝐴𝐴𝑖𝑖 𝑢𝑢�𝑖𝑖𝑃𝑃) − (𝑅𝑅𝑗𝑗 + 𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃)� + �𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇𝐴𝐴𝑖𝑖𝜃𝜃 𝑢𝑢�𝑖𝑖𝑃𝑃 −�𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇 −�𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇𝐴𝐴𝑗𝑗𝜃𝜃 𝑢𝑢�𝑗𝑗𝑃𝑃

���̇�𝑅𝑖𝑖�̇�𝜃𝑖𝑖�̇�𝑅𝑗𝑗�̇�𝜃𝑗𝑗�

�𝐶𝐶𝑞𝑞�̇�𝑞� = ��̇�𝜃𝑖𝑖 − �̇�𝜃𝑗𝑗

�𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇�̇�𝑅𝑖𝑖 + �̇�𝜃𝑖𝑖 ��𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇 �(𝑅𝑅𝑖𝑖 + 𝐴𝐴𝑖𝑖 𝑢𝑢�𝑖𝑖𝑃𝑃) − (𝑅𝑅𝑗𝑗 + 𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃)� + �𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇𝐴𝐴𝑖𝑖𝜃𝜃 𝑢𝑢�𝑖𝑖𝑃𝑃�−�𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�

𝑇𝑇�̇�𝑅𝑗𝑗 − �̇�𝜃𝑗𝑗 �−�𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇𝐴𝐴𝑗𝑗𝜃𝜃 𝑢𝑢�𝑗𝑗𝑃𝑃��

�𝐶𝐶𝑞𝑞�̇�𝑞�𝑞𝑞 = � 0 0 0 0 𝐶𝐶21 𝐶𝐶22 𝐶𝐶23 𝐶𝐶24

�

where,

𝐶𝐶21 =𝜕𝜕�𝐶𝐶𝑞𝑞�̇�𝑞�𝜕𝜕𝑅𝑅𝑖𝑖

= �̇�𝜃𝑖𝑖 ��𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇�

𝐶𝐶22 =𝜕𝜕�𝐶𝐶𝑞𝑞�̇�𝑞�𝜕𝜕𝜃𝜃𝑖𝑖

= �𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇�̇�𝑅𝑖𝑖 + �̇�𝜃𝑖𝑖 ��−𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇 �(𝑅𝑅𝑖𝑖 + 𝐴𝐴𝑖𝑖 𝑢𝑢�𝑖𝑖𝑃𝑃) − (𝑅𝑅𝑗𝑗 + 𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃)� + �𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�

𝑇𝑇 �(𝐴𝐴𝑖𝑖𝜃𝜃 𝑢𝑢�𝑖𝑖𝑃𝑃)�+�𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇𝐴𝐴𝑖𝑖𝜃𝜃 𝑢𝑢�𝑖𝑖𝑃𝑃 − �𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�

𝑇𝑇𝐴𝐴𝑖𝑖 𝑢𝑢�𝑖𝑖𝑃𝑃�

−�𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇�̇�𝑅𝑗𝑗 − �̇�𝜃𝑗𝑗 �−�𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�

𝑇𝑇𝐴𝐴𝑗𝑗𝜃𝜃 𝑢𝑢�𝑗𝑗𝑃𝑃�

𝐶𝐶23 =𝜕𝜕�𝐶𝐶𝑞𝑞�̇�𝑞�𝜕𝜕𝑅𝑅𝑗𝑗

= �̇�𝜃𝑖𝑖 �−�𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇�

𝐶𝐶24 =𝜕𝜕�𝐶𝐶𝑞𝑞�̇�𝑞�𝜕𝜕𝜃𝜃𝑗𝑗

= �̇�𝜃𝑖𝑖�𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇 �−(𝐴𝐴𝑗𝑗𝜃𝜃 𝑢𝑢�𝑗𝑗𝑃𝑃)� − �̇�𝜃𝑗𝑗 ��𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃�

62

�̈�𝑞 = 𝐶𝐶𝑞𝑞−1Qd = 𝐶𝐶𝑞𝑞−1

⎝

⎜⎛−� 0 0 0 0

𝐶𝐶21 𝐶𝐶22 𝐶𝐶23 𝐶𝐶24�

⎩⎪⎨

⎪⎧�̇�𝑅

𝑖𝑖

�̇�𝜃𝑖𝑖

�̇�𝑅𝑗𝑗

�̇�𝜃𝑗𝑗⎭⎪⎬

⎪⎫

⎠

⎟⎞

C22 can be simplified if we remember these characteristics of the transformation matrix that we discussed earlier in this chapter,

𝐴𝐴𝑖𝑖 = �cos𝜃𝜃𝑖𝑖 − sin𝜃𝜃𝑖𝑖sin𝜃𝜃𝑖𝑖 cos 𝜃𝜃𝑖𝑖

�

𝐴𝐴𝑖𝑖𝜃𝜃 = �−sin𝜃𝜃𝑖𝑖 − cos𝜃𝜃𝑖𝑖cos 𝜃𝜃𝑖𝑖 −sin𝜃𝜃𝑖𝑖

�

𝐴𝐴𝑖𝑖𝑇𝑇𝐴𝐴𝑖𝑖 = � cos 𝜃𝜃𝑖𝑖 sin𝜃𝜃𝑖𝑖−sin𝜃𝜃𝑖𝑖 cos𝜃𝜃𝑖𝑖

� �cos 𝜃𝜃𝑖𝑖 − sin𝜃𝜃𝑖𝑖sin𝜃𝜃𝑖𝑖 cos 𝜃𝜃𝑖𝑖

� = 𝐼𝐼

𝐴𝐴𝑖𝑖𝜃𝜃𝑇𝑇𝐴𝐴𝑖𝑖𝜃𝜃 = �−sin𝜃𝜃𝑖𝑖 cos𝜃𝜃𝑖𝑖

−cos𝜃𝜃𝑖𝑖 −sin𝜃𝜃𝑖𝑖� �−sin𝜃𝜃𝑖𝑖 − cos 𝜃𝜃𝑖𝑖

cos 𝜃𝜃𝑖𝑖 −sin𝜃𝜃𝑖𝑖� = 𝐼𝐼

𝐴𝐴𝑖𝑖𝜃𝜃𝑇𝑇𝐴𝐴𝑖𝑖 = �−sin𝜃𝜃𝑖𝑖 cos 𝜃𝜃𝑖𝑖

−cos𝜃𝜃𝑖𝑖 −sin𝜃𝜃𝑖𝑖� �cos 𝜃𝜃𝑖𝑖 − sin𝜃𝜃𝑖𝑖

sin𝜃𝜃𝑖𝑖 cos𝜃𝜃𝑖𝑖� = � 0 1

−1 0�

Therefore,

𝐶𝐶22 = �𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇�̇�𝑅𝑖𝑖 + �̇�𝜃𝑖𝑖 ��−𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇 �(𝑅𝑅𝑖𝑖 + 𝐴𝐴𝑖𝑖 𝑢𝑢�𝑖𝑖𝑃𝑃) − (𝑅𝑅𝑗𝑗 + 𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃)� + �𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�

𝑇𝑇 �(𝐴𝐴𝑖𝑖𝜃𝜃 𝑢𝑢�𝑖𝑖𝑃𝑃)�+�𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇𝐴𝐴𝑖𝑖𝜃𝜃 𝑢𝑢�𝑖𝑖𝑃𝑃 − �𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�

𝑇𝑇𝐴𝐴𝑖𝑖 𝑢𝑢�𝑖𝑖𝑃𝑃�

−�𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇�̇�𝑅𝑗𝑗 − �̇�𝜃𝑗𝑗 �−�𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�

𝑇𝑇𝐴𝐴𝑗𝑗𝜃𝜃 𝑢𝑢�𝑗𝑗𝑃𝑃� becomes,

𝐶𝐶22 = �𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇�̇�𝑅𝑖𝑖 + �̇�𝜃𝑖𝑖 ��−𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇 �𝑅𝑅𝑖𝑖 − (𝑅𝑅𝑗𝑗 + 𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃)� − �ℎ�𝑖𝑖�𝑇𝑇 𝑢𝑢�𝑖𝑖𝑃𝑃 + �ℎ�𝑖𝑖�𝑇𝑇 𝑢𝑢�𝑖𝑖𝑃𝑃+�ℎ�𝑖𝑖�𝑇𝑇 𝑢𝑢�𝑖𝑖𝑃𝑃 − �ℎ�𝑖𝑖�

𝑇𝑇 𝑢𝑢�𝑖𝑖𝑃𝑃�

−�𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇�̇�𝑅𝑗𝑗 − �̇�𝜃𝑗𝑗 �−�𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�

𝑇𝑇𝐴𝐴𝑗𝑗𝜃𝜃 𝑢𝑢�𝑗𝑗𝑃𝑃�

𝐶𝐶22 = �𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇�̇�𝑅𝑖𝑖 + �̇�𝜃𝑖𝑖�−𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇(𝑅𝑅𝑖𝑖 − 𝑅𝑅𝑗𝑗 − 𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃) − �𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�

𝑇𝑇�̇�𝑅𝑗𝑗 − �̇�𝜃𝑗𝑗 �−�𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇𝐴𝐴𝑗𝑗𝜃𝜃 𝑢𝑢�𝑗𝑗𝑃𝑃�

𝐶𝐶22 = �𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇��̇�𝑅𝑖𝑖 − �̇�𝑅𝑗𝑗� + �̇�𝜃𝑖𝑖�−𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇(𝑅𝑅𝑖𝑖 − 𝑅𝑅𝑗𝑗 − 𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃) + �̇�𝜃𝑗𝑗�𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�

𝑇𝑇𝐴𝐴𝑗𝑗𝜃𝜃 𝑢𝑢�𝑗𝑗𝑃𝑃

Homework: Derive the standard constraint equations (position, velocity, and acceleration) of a wheel (j) rolling on the ground (i). Assume that no slippage occurs.

63

Kinematic Modeling and Analysis of Mechanical Systems

2

3

4

θ2

A θ3

BO X4

Y4

X1

Y1

X2

Y2

X 3

Y 3

h4

The crank-slider mechanism can be represented as a sequence of RRRP joints.

• l2=0.15

• l3=0.35

• No offset

• The frames of links 2 and 3 are placed in the middle of these two links

The local position vectors can be defined as follows:

𝑢𝑢�1𝑂𝑂 = �0.00.0� 𝑢𝑢�2𝑂𝑂 = �−0.075

0.0 �

𝑢𝑢�2𝐴𝐴 = �0.0750.0 � 𝑢𝑢�3𝐴𝐴 = �−0.175

0.0 �

𝑢𝑢�3𝐵𝐵 = �0.1750.0 � 𝑢𝑢�4𝐵𝐵 = �0.0

0.0�

The absolute coordinates of the machines are:

𝑞𝑞 = � 𝑅𝑅1𝑥𝑥 𝑅𝑅1𝑦𝑦 𝜃𝜃1 𝑅𝑅2𝑥𝑥 𝑅𝑅2𝑦𝑦 𝜃𝜃2 𝑅𝑅3𝑥𝑥 𝑅𝑅3𝑦𝑦 𝜃𝜃3 𝑅𝑅4𝑥𝑥 𝑅𝑅4𝑦𝑦 𝜃𝜃4�𝑇𝑇

The input of the machine is,

𝜃𝜃2 = 𝜔𝜔2𝑡𝑡 + 𝜃𝜃𝑜𝑜2

64

Kinematic Constraints:

Since Frame 1 is fixed,

𝑅𝑅1𝑥𝑥 = 𝑅𝑅1𝑦𝑦 = 𝜃𝜃1 = 0

The first three joints are revolute whose constraints equations are:

[𝐼𝐼 𝐴𝐴𝑖𝑖𝜃𝜃 𝑢𝑢�𝑖𝑖𝑃𝑃 −𝐼𝐼 −𝐴𝐴𝑗𝑗𝜃𝜃 𝑢𝑢�𝑗𝑗𝑃𝑃] = 0

The table below show frames i and j for joints O, A, and B,

Joint Frame i Frame j

O 2 1

A 3 2

B 4 3

The fourth joint is prismatic with the following constraint equations:

�𝜃𝜃𝑖𝑖 − 𝜃𝜃𝑗𝑗 − 𝑐𝑐ℎ𝑖𝑖𝑇𝑇𝑟𝑟𝑖𝑖𝑗𝑗𝑃𝑃

� = �00�

This is a special case (why?). Frames i and j will be 1 and 4 respectively.

c = 0 (why?)

ℎ�4 is unit vector in the local y direction of frame 4.

The above equations become:

�𝜃𝜃4 − 𝜃𝜃1

�𝐴𝐴4 �01��𝑇𝑇��𝑅𝑅1 + 𝐴𝐴1 �10���

� = �00�

�𝜃𝜃4 − 𝜃𝜃1

[sin𝜃𝜃4 cos 𝜃𝜃4]��𝑅𝑅1 + �cos𝜃𝜃1sin𝜃𝜃1

���� = �00�

65

Constraint Matrix Assembly:

Based on the above discussion, the constraint equations are,

𝐶𝐶(𝑞𝑞, 𝑡𝑡) =

⎩⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎧ 𝑅𝑅1𝑥𝑥

𝑅𝑅1𝑦𝑦 𝜃𝜃1

𝑅𝑅1 + 𝐴𝐴1 𝑢𝑢�1𝑂𝑂 − 𝑅𝑅2 − 𝐴𝐴2 𝑢𝑢�2𝑂𝑂𝑅𝑅2 + 𝐴𝐴2 𝑢𝑢�2𝐴𝐴 − 𝑅𝑅3 − 𝐴𝐴3 𝑢𝑢�3𝐴𝐴𝑅𝑅3 + 𝐴𝐴3 𝑢𝑢�3𝐵𝐵 − 𝑅𝑅4 − 𝐴𝐴4 𝑢𝑢�4𝐵𝐵

𝜃𝜃4 − 𝜃𝜃1

[sin𝜃𝜃4 cos 𝜃𝜃4]��𝑅𝑅1 + �cos𝜃𝜃1sin𝜃𝜃1

���

𝜃𝜃2−𝜔𝜔2𝑡𝑡 − 𝜃𝜃𝑜𝑜2 ⎭⎪⎪⎪⎪⎬

⎪⎪⎪⎪⎫

= 0

Jacobian Matrix Assembly:

The Jacobian matrix is,

𝐶𝐶𝑞𝑞

=

⎣⎢⎢⎢⎢⎢⎢⎢⎡

𝐼𝐼(3𝑥𝑥3) 0(3𝑥𝑥9)𝐼𝐼 𝐴𝐴1𝜃𝜃 𝑢𝑢�1𝑂𝑂 −𝐼𝐼 −𝐴𝐴2𝜃𝜃 𝑢𝑢�2𝑂𝑂 0(2𝑥𝑥6)

0 0 𝐼𝐼 𝐴𝐴2𝜃𝜃 𝑢𝑢�2𝐴𝐴 −𝐼𝐼 −𝐴𝐴3𝜃𝜃 𝑢𝑢�3𝐴𝐴 0(2𝑥𝑥3)0 0 0 0 𝐼𝐼 𝐴𝐴3𝜃𝜃 𝑢𝑢�3𝐵𝐵 −𝐼𝐼 −𝐴𝐴4𝜃𝜃 𝑢𝑢�4𝐵𝐵0 0 −1 0 0 0 0 0 0 0 0 1

[sin𝜃𝜃4 cos𝜃𝜃4] (− sin𝜃𝜃4 sin𝜃𝜃1 + cos 𝜃𝜃4 cos 𝜃𝜃1) 0 0 0 0 0 [cos 𝜃𝜃4 −sin𝜃𝜃4]��𝑅𝑅1 + �cos𝜃𝜃1sin𝜃𝜃1

���

0 0 0 0 0 1 0 0 0 0 0 0 ⎦⎥⎥⎥⎥⎥⎥⎥⎤

Use Newton-Raphson to analyze the displacement of the machine

66

Velocity analysis

𝐶𝐶𝑞𝑞�̇�𝑞 = −𝐶𝐶𝑡𝑡

In this case Ct

𝐶𝐶𝑡𝑡 = [0 0 0 0 0 0 0 0 0 0 0 −𝜔𝜔2]𝑇𝑇

Acceleration Analysis The first three joints are revolute whose Qd equations are:

Qd = −�

0

−�̇�𝜃𝑖𝑖2𝐴𝐴𝑖𝑖 𝑢𝑢�𝑖𝑖𝑃𝑃0

�̇�𝜃𝑗𝑗2𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃

�

The fourth joint is prismatic with the following Qd equations

Qd = − � 0 0 0 0 𝐶𝐶21 𝐶𝐶22 𝐶𝐶23 𝐶𝐶24

�

⎩⎪⎨

⎪⎧�̇�𝑅

𝑖𝑖

�̇�𝜃𝑖𝑖

�̇�𝑅𝑗𝑗

�̇�𝜃𝑗𝑗⎭⎪⎬

⎪⎫

Frames i and j will be 1 and 4 respectively. The above equations become:

Qd = −� 0 0 0 0 𝐶𝐶21 𝐶𝐶22 𝐶𝐶23 𝐶𝐶24

�

⎩⎪⎨

⎪⎧�̇�𝑅

4

�̇�𝜃4

�̇�𝑅1

�̇�𝜃1⎭⎪⎬

⎪⎫

𝐶𝐶21 = �̇�𝜃𝑖𝑖 ��𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇�

𝐶𝐶22 = �𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇��̇�𝑅𝑖𝑖 − �̇�𝑅𝑗𝑗� + �̇�𝜃𝑖𝑖�−𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�𝑇𝑇�𝑅𝑅𝑖𝑖 − 𝑅𝑅𝑗𝑗 − 𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃� + �̇�𝜃𝑗𝑗�𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�

𝑇𝑇𝐴𝐴𝑗𝑗𝜃𝜃 𝑢𝑢�𝑗𝑗𝑃𝑃

𝐶𝐶23 = �̇�𝜃𝑖𝑖 �−�𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇�

𝐶𝐶24 = �̇�𝜃𝑖𝑖 �𝐴𝐴𝑖𝑖𝜃𝜃ℎ�𝑖𝑖�𝑇𝑇�−�𝐴𝐴𝑗𝑗𝜃𝜃 𝑢𝑢�𝑗𝑗𝑃𝑃�� − �̇�𝜃𝑗𝑗 ��𝐴𝐴𝑖𝑖ℎ�𝑖𝑖�

𝑇𝑇𝐴𝐴𝑗𝑗 𝑢𝑢�𝑗𝑗𝑃𝑃�

Substituting the matrices we used earlier

𝐶𝐶21 = �̇�𝜃4 ���−sin𝜃𝜃4 − cos 𝜃𝜃4cos 𝜃𝜃4 − sin𝜃𝜃4

� �0.01.0��

𝑇𝑇� = �̇�𝜃4[− cos 𝜃𝜃4 − sin𝜃𝜃4]

𝐶𝐶22 = 0

𝐶𝐶23 = �̇�𝜃1 �− ��−sin𝜃𝜃4 − cos𝜃𝜃4cos𝜃𝜃4 −sin𝜃𝜃4

� �0.01.0��

𝑇𝑇� = 0

𝐶𝐶24 = 0

67

Matlab Program for Kinematic Analysis of a Crank-Slider Mechanism close all clear all % Analysis of a Crank-Slider Mechanism Using Newton Raphson's Method r2=0.15; %crank r3=0.35; %coupler theta1=0; %fixed link angle %Initial Guess theta20=30; theta30=348; R2x0=(r2/2)*cosd(theta20); R2y0=(r2/2)*sind(theta20); R3x0=(r2)*cosd(theta20)+(r3/2)*cosd(theta30); R3y0=(r2)*sind(theta20)+(r3/2)*sind(theta30); R4x0=(r2)*cosd(theta20)+(r3)*cosd(theta30); R4y0=0; theta40=0; %input parameters t(1)=0; dt=0.001; %seconds omega2=150; % input velocity (rad/s) imax=44; %number of time steps converge=0; epsilon1=0.001; %dtheta accuracy epsilon2=0.001; %C accuracy %Define the local position vectors u1O=[0;0]; u2O=[-0.075;0]; u2A=[0.075;0]; u3A=[-0.175;0]; u3B=[0.175;0]; u4B=[0;0]; i=1; while i<=imax t(i)=(i)*dt; theta2(i)=theta20+omega2*t(i)*(180/pi); %adjust initial conditions based on the step # if i==1 R2x(i)=R2x0; R2y(i)=R2y0;

68

theta2(i)=theta20; R3x(i)=R3x0; R3y(i)=R3y0; theta3(i)=theta30; R4x(i)=R4x0; R4y(i)=R4y0; theta4(i)=theta40; else R2x(i)=R2x(i-1); R2y(i)=R2y(i-1); theta2(i)=theta2(i-1); R3x(i)=R3x(i-1); R3y(i)=R3y(i-1); theta3(i)=theta3(i-1); R4x(i)=R4x(i-1); R4y(i)=R4y(i-1); theta4(i)=theta4(i-1); end; %Newton Raphson for Position Analysis while converge==0 %Transformation Matrix A2=[cosd(theta2(i)),-sind(theta2(i));sind(theta2(i)),cosd(theta2(i))]; A3=[cosd(theta3(i)),-sind(theta3(i));sind(theta3(i)),cosd(theta3(i))]; %Prepare Constraint Matrix %We are eliminating the first three which correspond to the fixed frame C12=[R2x(i);R2y(i)]+A2*u2O; C34=[R2x(i);R2y(i)]+A2*u2A-[R3x(i);R3y(i)]-A3*u3A; C56=[R3x(i);R3y(i)]+A3*u3B-[R4x(i);0]; C(1:2,1)=C12(1:2,1); C(3:4,1)=C34(1:2,1); C(5:6,1)=C56(1:2,1); C(7,1)=theta4(i); C(8,1)=R4y(i); C(9,1)=theta2(i)-omega2*t(i)*(180/pi)-theta20; %Jacobian Matrix %We are eliminating the first three which correspond to the fixed frame Cq=zeros(9,9); Cq(1,1)=1; Cq(1,3)=(r2/2)*sind(theta2(i)); Cq(2,2)=1; Cq(2,3)=-(r2/2)*cosd(theta2(i)); Cq(3,1)=1; Cq(3,3)=-(r2/2)*sind(theta2(i)); Cq(3,4)=-1; Cq(3,6)=-(r3/2)*sind(theta3(i)); Cq(4,2)=1; Cq(4,3)=(r2/2)*cosd(theta2(i)); Cq(4,5)=-1; Cq(4,6)=(r3/2)*cosd(theta3(i)); Cq(5,4)=1; Cq(5,6)=-(r3/2)*sind(theta3(i)); Cq(5,7)=-1;

69

Cq(6,5)=1; Cq(6,6)=(r3/2)*cosd(theta3(i)); Cq(6,8)=-1; Cq(7,9)=1; Cq(8,8)=1; Cq(9,3)=1; dq=-inv(Cq)*C; %update thetas R2x(i)=R2x(i)+dq(1); R2y(i)=R2y(i)+dq(2); theta2(i)=theta2(i)+dq(3); R3x(i)=R3x(i)+dq(4); R3y(i)=R3y(i)+dq(5); theta3(i)=theta3(i)+dq(6); R4x(i)=R4x(i)+dq(7); R4y(i)=R4y(i)+dq(8); theta4(i)=theta4(i)+dq(9); %Termination Criteria if sum(abs(dq))<epsilon1 | sum(abs(C))<epsilon2 converge=1; i=i+1; end; end; converge=0; end; for i=1:imax %velocity Analysis Ct=[0;0;0;0;0;0;0;0;-omega2]; Cq=zeros(9,9); Cq(1,1)=1; Cq(1,3)=(r2/2)*sind(theta2(i)); Cq(2,2)=1; Cq(2,3)=-(r2/2)*cosd(theta2(i)); Cq(3,1)=1; Cq(3,3)=-(r2/2)*sind(theta2(i)); Cq(3,4)=-1; Cq(3,6)=-(r3/2)*sind(theta3(i)); Cq(4,2)=1; Cq(4,3)=(r2/2)*cosd(theta2(i)); Cq(4,5)=-1; Cq(4,6)=(r3/2)*cosd(theta3(i)); Cq(5,4)=1; Cq(5,6)=-(r3/2)*sind(theta3(i)); Cq(5,7)=-1; Cq(6,5)=1; Cq(6,6)=(r3/2)*cosd(theta3(i)); Cq(6,8)=-1; Cq(7,9)=1; Cq(8,8)=1; Cq(9,3)=1;

70

vq=-inv(Cq)*Ct; R2xd(i)=vq(1,1); R2yd(i)=vq(2,1); theta2d(i)=vq(3,1); R3xd(i)=vq(4,1); R3yd(i)=vq(5,1); theta3d(i)=vq(6,1); R4xd(i)=vq(7,1); R4yd(i)=vq(8,1); theta4d(i)=vq(9,1); %Acceleration Analysis %Transformation Matrices A2=[cosd(theta2(i)),-sind(theta2(i));sind(theta2(i)),cosd(theta2(i))]; A3=[cosd(theta3(i)),-sind(theta3(i));sind(theta3(i)),cosd(theta3(i))]; A4=[cosd(theta4(i)),-sind(theta4(i));sind(theta4(i)),cosd(theta4(i))]; A4t=[-sind(theta4(i)),-cosd(theta4(i));cosd(theta4(i)),-sind(theta4(i))]; %Prepare joint Qd matrices for the revolute joints Qd12=theta2d(i)^2*A2*u2O; Qd34=theta2d(i)^2*A2*u2A-theta3d(i)^2*A3*u3A; Qd56=theta3d(i)^2*A3*u3B; Qd(1:2,1)=Qd12(1:2,1); Qd(3:4,1)=Qd34(1:2,1); Qd(5:6,1)=Qd56(1:2,1); %Prepare joint Qd matrices for the prismatic joint C21=transpose(theta4d(i)*A4t*[0;1]); C22=0; C23=[0,0]; C24=0; Qd78=-[zeros(1,6);[C21(1),C21(2),C22,C23(1),C23(2),C24]]*... [R4xd(i);R4yd(i);theta4d(i);0;0;0]; %Last row corresponds to the degree of freedom Qd(9,1)=0; aq=inv(Cq)*Qd; R2xdd(i)=aq(1,1); R2ydd(i)=aq(2,1); theta2dd(i)=aq(3,1); R3xdd(i)=aq(4,1); R3ydd(i)=aq(5,1); theta3dd(i)=aq(6,1); R4xdd(i)=aq(7,1); R4ydd(i)=aq(8,1); theta4add(i)=aq(9,1); end; % Calculate joints xa=r2*cosd(theta2); ya=r2*sind(theta2); xb=r2*cosd(theta2)+r3*cosd(theta3); yb=r2*sind(theta2)+r3*sind(theta3);

71

plot(xa,ya,'r','LineWidth',2); hold on; plot(xb,yb,'g','LineWidth',2); legend ('Crank','Output Link') axis([-0.3 0.3 -0.3 0.3]) axis equal grid on %plot the links figure for i=1:imax %Generate the links at every instant x(1,i)=0; y(1,i)=0; x(2,i)=xa(i); y(2,i)=ya(i); x(3,i)=xb(i); y(3,i)=yb(i); plot(x(1:3,i),y(1:3,i)) hold on end; axis([-0.3 0.3 -0.3 0.3]) title ('motion of the links') axis equal grid on %plot angular velocities figure plot(t,theta2d,'-',t,theta3d,'--','LineWidth',2) legend('omega_2','omega_3','Location','SouthWest') title ('Angular velocities of the two links') xlabel('Time (s)') ylabel('rad/s') grid on %plot velocity of the Block figure plot(t,R4xd,'-','LineWidth',2) legend('Velocity of the Block','Location','NorthWest') xlabel('Time (s)') ylabel('m/s') grid on %plot angular accelerations figure plot(t,theta2dd,'-',t,theta3dd,'--','LineWidth',2) legend('alpha_2','alpha_3','Location','SouthWest') title ('Angular accelerations of the two links') xlabel('Time (s)') ylabel('rad/s^2') grid on %plot acceleration of the Block figure plot(t,R4xdd,'-','LineWidth',2)

72

legend('Acceleration of the Block','Location','SouthWest') xlabel('Time (s)') ylabel('m/s^2') grid on

73

-0.2 -0.1 0 0.1 0.2 0.3 0.4 0.5-0.3

-0.2

-0.1

0

0.1

0.2

Crank

Output Link

-0.2 -0.1 0 0.1 0.2 0.3 0.4 0.5-0.3

-0.2

-0.1

0

0.1

0.2

motion of the links

74

Time (s)

0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04 0.045

rad/

s

-100

-50

0

50

100

150Angular velocities of the two links

omega2

omega3

Time (s)

0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04 0.045

m/s

-25

-20

-15

-10

-5

0

5

10

15

20

25

Velocity of the Block

75

Time (s)

0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04 0.045

rad/

s2

10 4

-1.5

-1

-0.5

0

0.5

1

1.5Angular accelerations of the two links

alpha2

alpha3

Time (s)

0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04 0.045

m/s

2

-5000

-4000

-3000

-2000

-1000

0

1000

2000

3000

Acceleration of the Block

76

Effect of Accuracy ε1 and ε2 ε1 = ε2 = 1

ε1 = ε2 = 0.01

ε1 = ε2 = 0.0001

-0.2 -0.1 0 0.1 0.2 0.3 0.4 0.5-0.3

-0.2

-0.1

0

0.1

0.2

Crank

Output Link

-0.2 -0.1 0 0.1 0.2 0.3 0.4 0.5-0.3

-0.2

-0.1

0

0.1

0.2

Crank

Output Link

-0.2 -0.1 0 0.1 0.2 0.3 0.4 0.5-0.3

-0.2

-0.1

0

0.1

0.2

Crank

Output Link

77

Homework Problem 3.21

Additionally derive relevant velocity and acceleration matrices. Simulate the motion for these data: 1. Body 2 is 1 meter long 2. Body 2 rotate at constant angular velocity of 1 rpm 3. τ is a constant velocity of 0.5 m/s (outward). 4. Body 2 starts horizontally 5. Body 3 starts at the middle of Body 2 (0.5 m away from the hinge) 6. Simulate the motion for 1 second Bonus: Implement condition in your code to eliminate Body 3 if it is outside Body 2.