PART 2

Multibody Kinematics and Dynamics

Chapter 1
1.1 INTRODUCTION
Determine appropriate movement of the wipers

View range
Tandem or opposite
Wipe angle
Location of pivots

Timing of wipers
Wiping velocity
The force acting on the wipers
1

Kinematics
Kinematics
Deal with the way things move

Kinematic analysis
Determine
Position, displacement, rotation, speed, velocity,
acceleration
Provide
Geometry dimensions of the mechanism
Operation range

Dynamic analysis
Power capacity, stability, member load

Planar mechanism motion in 2D space

2

1.4 MECHANISM TERMINOLOGY

Mechanism

Design synthesis is the process of developing mechanism to satisfy a set of

performance requirements for the machine.
Analysis ensures that the mechanism will exhibit motion to accomplish the

requirements

Linkage
Frame
Links rigid body
Joint
Primary joint (full joint)

Revolute joint (pin or hinge joint)

pure rotation
Sliding joint (piston or prism joint)
linear sliding

Mechanism Joint

Higher-order joint (half joint)

Simple link

A rigid body contains only two

joints
Crank
Rocker

Complex link

Allow rotation and sliding

Cam joint
Gear connection

A rigid body contains more than

two joints
Rocker arm
Bellcrank

Point of interest
Actuator

A power source link

1.5 Kinematic Diagram

Simple Link

Simple Link
(with point of
interest)

Complex Link

Pin Joint
Revolute Joint
5

Kinematic Diagram

Slider Joint
Translation Joint

Cam Joint

Gear Joint

1.7 MOBILITY

Constrained mechanism : one degree of freedom

Locked mechanism : zero degree of freedom
Unconstrained mechanism : more than one degree of freedom

1.7 Actuators and Drivers

Electric motors (AC/DC)

Engines
Servomotors
Air or hydraulic motors
Hydraulic or pneumatic cylinders
Screw actuators
Manual

10

1.9 SPECLAL CASES OF THE MOBILITY EQUATION

Coincident Joints

One degree of freedom actually if pivoted links are the same size

11

12

1.10 THE FOUR-BAR MECHANISM

13

Crank and Rocker

14

1.10.1 Design of Crank and Rocker

s : short link
l : long link
p , q : intermediate link

15

1.11 SLIDER-CRANK MECHANISM

16

1.12 SPECIAL PURPOSE MECHANISMS

1.12.1 Straight-Line Mechanisms

1.12.2 Parallelogram Mechanisms

17

1.12.3 Quick-Return Mechanisms

1.12.4 Scotch Yoke Mechanism

18

Chapter 4 Displacement Analysis

4.4 DISPLACEMENT ANALYSIS

Locate the positions of all

links as driver link is
displaced
Configuration

Positions of all the links

One degree of freedom

Moving one link will

precisely position all
other links

19

20

4.1 Vector Analysis of Displacement

Y1

r4
r2
r3

r3
r5
X1

r1
r4

(1) r 1 r 2 r 3 r 4 0
r1s1 r2c 2 r3c3 5.3
r c r s r s 3.2 0

1 1 2 2 3 3
r1 3,1 30,r2 4.9,r3 3.3

2 equations for 2 unknows 2 ,3

(2) r 3 r 4 r 5 0
r3c3 r4 c 4 x1
r s r s 0
3 3 4 4 0.8
r1 10.1

2 equations for 2 unknows 4 and x1

21

Y
r1
X

r3
r2

r 1 r 2 r 3 0
0.5c1 1.75c 2 1
0.5s 1.75s y 0

2
for1 2 ,solve for1and ymax
for1 2 ,solve for1and ymin
22

4.7 Limiting Positions and Stroke

23

Chapter 6 Velocity Analysis

6.2 LINEAR AND ANGULAR VELOCITY

6.2.2 Linear Velocity of a General Point

24

r1

X
r2

r4
r3

r 1 r 2 r 3 r 4 0

2 r 2 3r 3 4 r 4 0
eqs forunknowns3 and 4

25

6.6 GRAPHICAL VELOCITY ANALYSIS:RELATIVE VELOCITY METHOD

6.6.1 Points on Links Limited to Pure Rotation or Rectilinear Translation

6.6.2 General Points on a Floating Link

26

X
r1

r4

r3

r2

r 1 r 2 r 3 0
0

1r 1 2 r 2 0
5

eqs for1and 2
r r 1 r 4
r 1r 1 2 r 4
27

Y
r1
r2
X
r3

r 1 r 2 r 3 0
solve forand

1r 1 2 r 2 r2 0
vc 2
1 5rad min,r 2

vs 2
eqs forunknowns2 and v

28

r2
r1
Y
r3
X

r 1 r 2 r 3 0
1
r2 6, 2 340 r 3
3
eqs forunknownsr1and 1

1r 1 2 r 2 r 2 0
8c
r 2 2
8s 2
eqs forunknowns1and 2
29

Y
r2

r1

r3
r4

r 1 r 2 r 3 r 4 0

1r 1 2 r 2 3r 3 0
given 1 find 2 and 3

30

Chapter 7 Acceleration Analysis

7.2 LINEAR ACCELERATION
7.2.1 Linear Acceleration of Rectilinear Points

31

X
r1

r2
r4
r3

r 1 r 2 r 3 r 4 0

1r 1 2 r 2 3r 3 0
given 1 find 2 and 3

1 1r 1 2 r 2 2 2 r 2 3r 3 3 3r 3 0
solve for2 and 3
32

r5

rp

rp r 1 r 2 r 3 r 5
rp 1r 1 2 r 2 3 r 3 r 5
rp 1 1r 1 2 r 2 2 2 r 2 3 r 3 r 5 3 3 r 3 r 5

33

Y
X

r1
r2

r3

r 1 r 2 r 3 0
0

1r 1 2 r 2 0
v

solve for2 and v

1r 1 1 1r 1 2 r 2 2 2 r 2 0
a

solve for2 and a

34

7.12 EQUIVALENT LINKAGES

35

Computation of Multibody Kinematics And Dynamics

1.1 Multibody Mechanical Systems

2D Planar Mechanism

3D Spatial Mechanism

36

Computer-Aided Design (CAD)

Mechanics: Statics and dynamics.
Dynamics : kinematics and kinetics.
Kinematics is the study of motion, i.e., the study of
displacement, velocity, and acceleration, regardless
of the forces that produce the motion.

Kinetics or Dynamics is the study of motion and its

relationship with the forces that produce that motion.
37

1.2 Coordinate Systems

A four-bar mechanism with generalized coordinates.

4 Coordinate

q [1 2 3 ]T
3 Constraints

(r 2 l 2 s 2 d 2 ) 2rl cos 2ls cos 1 2rs cos( 1 ) 0

(r 2 l 2 s 2 d 2 ) 2rl cos 2ds cos 2 0

1 2 3 2 0
degrees of freedom 4 3 = 1
38

Generalized Coordinates
1
q 2
3

2 constraints

r cos 1 d cos 2 s cos 3 l 0

r sin 1 d sin 2 s sin 3 0
dof 3 2 1
39

Cartesian Coordinates
12 coordinates and 11 kinematic constraints

q x1 y1 1 x2 y2 2 x3 y3 3

r
x1 cos 1 0
2
r
y1 sin 1 0
2
r
d
x1 cos 1 x2 cos 2 0
2
2
r
d
y1 sin 1 y2 sin 2 0
2
2
d
s
x2 cos 2 x3 cos 3 0
2
2
d
s
y2 sin 2 y3 sin 3 0
2
2
s
x3 cos 3 l 0
2
s
y3 sin 3 0
2

dof 9 8 1
40

1.3 Computation Kinematics

A mechanism that is formed from a collection of links or bodies kinematically connected to one
another.
An open-loop mechanism may contain links with single joint.
A closed-loop mechanism is formed from a closed chain, wherein each link is connected to at
least two other links of the mechanism.

Figure (a) Open-loop mechanismdouble pendulum

and (b) closed-loop mechanismfour-bar linkage.

Single and Multi-Loop Mechanism

Figure (a) Single-loop mechanism and (b) multi-loop mechanism.

41

High and Low Pair of Kinematic Joint

Figure Example of kinematic pairs: (a) revolute joint, (b) translational joint,
(c) gear set, (d) cam follower, (e) screw joint, and (f) spherical ball joint.
42

Generalized Coordinates
3 generalized coordinates,
2 algebraic constraint equations,

l1 cos 1 l2 cos 2 l3 cos 3 d1 0

l1 sin 1 l2 sin 2 l3 sin 3 d2 0

Figure four-bar mechanism.

43

2.1 Planar Kinematics in Cartesian Coordinates

The column vector qi [ x, y, ]Ti is the vector of coordinates
for body i in a plane.

qi x, y, z, 1 , 2 , 3 i is the vector of coordinates for body i

T

in three - dimensional space.

Inertial system Y
Bodyfixed system

ri P ri Ai siP
Coordinate transformation matrix

cos sin
Ai

sin

cos

44

Constraint Equation

A constraint equation describing a condition on the vector of coordinates of

a system can be expressed as follows: q 0
In some constraint and driving function, the variable time may appear
explicitly: q, t 0
Constraint Jacobian Matrix by differentiating the constraint equations

( q ) 0

q 0,often denoted asq q 0

q


q

also denoted as
q+
q0
q
q
q q (q q )q q 0
q q = (q q )q q
45

Redundant Constraint

Kinematically equivalent.

Figure (a) A double parallel-crank mechanism and (b) its kinematically equivalent.

Kinematics of Mechanism
Mainly composed of revolute joint and translation joint

Figure Quick-return mechanism: (a) schematic presentation and

(b) its equivalent representation without showing the actual outlines.

46

2.2 Revolute Joint

ri siP rj s Pj 0

( r ,2) ri Ai siP rj Aj sjP 0

( r ,2)

xi iP cos i iP sin i x j jP cos j Pj sin j 0

P
P
P
P
i cos i y j j sin j
j cos j
yi i sin i +
0

Figure Revolute joint P connecting bodies i and j .

47

Time Derivative of Revolute Joint Constraint

1 xi iP cos i iP sin i x j jP cos j Pj sin j 0

2 yi iP sin i iP cos i y j jP sin j Pj cos j 0

xi - (xiP sin fi + hiP cos fi )fi - x j + (x jP sin f j + h Pj cos f j )f j = 0

yi + (xiP cos fi - hiP sin fi )fi - y j - (x jP cos f j - h Pj sin f j )f j = 0

48

Time Derivative of Revolute Constraint

1 xi iP cos i iP sin i x j jP cos j Pj sin j 0
2 yi iP sin i iP cos i y j jP sin j Pj cos j 0

xi - (xiP sin fi + hiP cos fi )fi - x j + (x jP sin f j + h Pj cos f j )f j = 0

yi + (xiP cos fi - hiP sin fi )fi - y j - (x jP cos f j - h Pj sin f j )f j = 0

xi (iP sin i iP cos i )i (iP cos i iP sin i ) i2 x j
( jP sin j Pj cos j ) j ( jP cos j Pj sin j ) j2 0

yi (iP cos i iP sin i )i (iP sin i iP cos i ) i2 y j

( jP cos j Pj sin j ) j ( jP sin j Pj cos j ) j2 0
or

sip i2 s jp j2
49

2.3 Translational Joint

Figure Different representations of a translational joint.

Translational Joint
niT d 0
P
P

x
i
j
P
R
P
R
xi xi yi yi P
0
P
yi y j
( xiP x Pj )( yiP yiQ ) ( yiP y Pj )( xiP xQj ) 0

0
0
i j (i j )

Figure A translational joint between bodies i and j .

50

Time Derivative of Translational Joint Constraint

( xiP xiQ )( y Pj yiP ) ( yiP yiQ )( x Pj xiP ) 0

0
0
i j (i j )

g = -2[(xiP - xiQ )(xi - x j ) + ( yiP - yiQ )( yi - y j )f -[(xiP - xiQ )( yi - y j ) - ( yiP - yiQ )(xi - x j )fi2

51

Driving Link

Figure (a) The motion of the slider is controlled in the x direction

and (b) the motion of point P is controlled in the y direction.

xi d (t ) 0

yiP d (t ) 0
52

A Matlab Program for

Kinematics of a four-bar linkage
8 Constraint equations:
x1 40 cos 1 0

OA 80mm
AB 260mm

y1 40 sin 1 0

BC 180mm

x1 40 cos 1 x2 130 cos 2 0

OC 180mm

1
O

x2 130 cos 2 x3 90 cos 3 0

y2 130 sin 2 y3 90 sin 3 0

x3 90 cos 3 180 0

y1 40 sin 1 y2 130 sin 2 0

y3 90 sin 3 0

driving link
1 2 t 2 0

To solve the 9 equations for 9 unknown qT x1 , y1 ,1 , x2 , y2 ,2 , x3 , y3 ,3

53

Jacobian matrix and velocity equations

1 1 0 40sin
2 0 1 40 cos

3 1 0 40sin

4 0 1 40 cos
J 5 0 0
0

6 0 0
0
7 0 0
0

8 0 0
0
9 0 0
1

0 0
0
0 0
0
1 0 130sin 2
0 1 130 cos 2
1 0 130sin 2
0 1 130 cos 2
0 0
0
0 0
0
0 0
0

0 0
0

0 0
0

0 0
0

0 0
0

1 0 90sin 3

0 1 90 cos 3
1 0 90sin 3

0 1 90 cos 3

0 0
0

0
0

0
0

0
0

0

0
2

To solve Jq for the velocity q

54

Acceleration equations
2

40 cos

2
40
sin

2
2
50 cos 130 cos 2 2

2 130 sin 2
50
sin

2
2

2
130 cos 2 2 90 cos 3 3 2

2 90 sin 2
130
sin

2
2
3
3

90 cos 3 3

2
90
sin

3
3

To solve Jq for the accelerati on q

55

The procedure of m-file

Initialize and start at t 0
Call nonlinear solver wit h initial position to solve qt
Determine Jacobian matrix and with qt to solve q t
Determine with q t to solve qt

Assume qt t with intended motion

and set it as the new initial position of next iteration
Plot the time response of qt , qt and qt

Determine the positions of the bars and animate them

56

The code of m.file (1)

1. % Set up the time interval and the initial positions of the nine coordinates
2. T_Int=0:0.01:2;
3. X0=[0 50 pi/2
125.86 132.55 0.2531
215.86 82.55 4.3026];
4. global T
5. Xinit=X0;
6.
7. % Do the loop for each time interval
8. for Iter=1:length(T_Int);
9.
T=T_Int(Iter);
10.
% Determine the displacement at the current time
11.
[Xtemp,fval] = fsolve(@constrEq4bar,Xinit);
12.
13.
% Determine the velocity at the current time
14.
phi1=Xtemp(3); phi2=Xtemp(6); phi3=Xtemp(9);
15.
JacoMatrix=Jaco4bar(phi1,phi2,phi3);
16.
Beta=[0 0 0
0 0 0
0 0 2*pi]';
17.
Vtemp=JacoMatrix\Beta;
18.
19.
% Determine the acceleration at the current time
20.
dphi1=Vtemp(3); dphi2=Vtemp(6); dphi3=Vtemp(9);
21.
Gamma=Gamma4bar(phi1,phi2,phi3,dphi1,dphi2,dphi3);
22.
Atemp=JacoMatrix\Gamma;
23.
24.
% Record the results of each iteration
25.
X(:,Iter)=Xtemp; V(:,Iter)=Vtemp; A(:,Iter)=Atemp;
26.
27.
% Determine the new initial position to solve the equation of the next
28.
% iteration and assume that the kinematic motion is with inertia
29.
if Iter==1
30.
Xinit=X(:,Iter);
31.
else
32.
Xinit=X(:,Iter)+(X(:,Iter)-X(:,Iter-1));
33.
end
34.
35.end

57

The code of m.file (2)

36.% T vs displacement plot for the nine coordinates
37.figure
38.for i=1:9;
39.
subplot(9,1,i)
40.
plot (T_Int,X(i,:))
41.
set(gca,'xtick',[], 'FontSize', 5)
42.end
43.% Reset the bottom subplot to have xticks
44.set(gca,'xtickMode', 'auto')
45.
46.% T vs velocity plot for the nine coordinates
47.figure
48.for i=1:9;
49.
subplot(9,1,i)
50.
plot (T_Int,V(i,:))
51.
set(gca,'xtick',[], 'FontSize', 5)
52.end
53.set(gca,'xtickMode', 'auto')
54.
55.% T vs acceleration plot for the nine coordinates
56.figure
57.for i=1:9;
58.
subplot(9,1,i)
59.
plot (T_Int,A(i,:))
60.
AxeSup=max(A(i,:));
61.
AxeInf=min(A(i,:));
62.
if AxeSup-AxeInf<0.01
63.
axis([-inf,inf,(AxeSup+AxeSup)/2-0.1 (AxeSup+AxeSup)/2+0.1]);
64.
end
65.
set(gca,'xtick',[], 'FontSize', 5)
66.end
67.set(gca,'xtickMode', 'auto')

58

The code of m.file (3)

68.% Determine the positions of the four revolute joints at each iteration
69.Ox=zeros(1,length(T_Int));
70.Oy=zeros(1,length(T_Int));
71.Ax=80*cos(X(3,:));
72.Ay=80*sin(X(3,:));
73.Bx=Ax+260*cos(X(6,:));
74.By=Ay+260*sin(X(6,:));
75.Cx=180*ones(1,length(T_Int));
76.Cy=zeros(1,length(T_Int));
77.
78.% Animation
79.figure
80.for t=1:length(T_Int);
81.
bar1x=[Ox(t) Ax(t)];
82.
bar1y=[Oy(t) Ay(t)];
83.
bar2x=[Ax(t) Bx(t)];
84.
bar2y=[Ay(t) By(t)];
85.
bar3x=[Bx(t) Cx(t)];
86.
bar3y=[By(t) Cy(t)];
87.
88.
plot (bar1x,bar1y,bar2x,bar2y,bar3x,bar3y);
89.
axis([-120,400,-120,200]);
90.
axis normal
91.
92.
M(:,t)=getframe;
93.end

59

Initialization
1. % Set up the time interval and the initial positions of the nine coordinates
2. T_Int=0:0.01:2;
3. X0=[0 50 pi/2
125.86 132.55 0.2531
215.86 82.55 4.3026];
4. global T
5. Xinit=X0;

1.

The sentence is notation that is behind symbol %.

2.

Simulation time is set from 0 to 2 with t = 0.01.

3.

Set the appropriate initial positions of the 9 coordinates which are used to solve nonlinear solver.

4.

Declare a global variable T which is used to represent the current time t and determine the
driving constraint for angular velocity.

60

Determine the displacement

10. [Xtemp,fval] = fsolve(@constrEq4bar,Xinit);

10. Call the nonlinear solver fsolve in which the constraint equations and initial values are necessary. The
initial values is mentioned in above script. The constraint equations is written as a function (which can
be treated a kind of subroutine in Matlab) as following and named as constrEq4bar. The fsolve finds a
root of a system of nonlinear equations and adopts the trust-region dogleg algorithm by default.
a. function F=constrEq4bar(X)
b.
c. global T
d.
e. x1=X(1); y1=X(2); phi1=X(3);
f. x2=X(4); y2=X(5); phi2=X(6);
g. x3=X(7); y3=X(8); phi3=X(9);
h.
i. F=[ -x1+40*cos(phi1);
j.
-y1+40*sin(phi1);
k.
x1+40*cos(phi1)-x2+130*cos(phi2);
l.
y1+40*sin(phi1)-y2+130*sin(phi2);
m.
x2+130*cos(phi2)-x3+90*cos(phi3);
n.
y2+130*sin(phi2)-y3+90*sin(phi3);
o.
x3+90*cos(phi3)-180;
p.
y3+90*sin(phi3);
q.
phi1-2*pi*T-pi/2];

The equation of driving constraint

is depended on current time T

61

Determine the velocity

14.
15.
16.
17.

phi1=Xtemp(3); phi2=Xtemp(6); phi3=Xtemp(9);

JacoMatrix=Jaco4bar(phi1,phi2,phi3);
Beta=[0 0 0
0 0 0
0 0 2*pi]';
Vtemp=JacoMatrix\Beta;

15. Call the function Jaco4bar to obtain the Jacobian Matrix depended on current values of
displacement.
16. Declare the right-side of the velocity equations.
17. Solve linear equation by left matrix division \ roughly the same as J-1. The algorithm adopts
several methods such as LAPACK, CHOLMOD, and LU. Please find the detail in Matlab Help.
a. function JacoMatrix=Jaco4bar(phi1,phi2,phi3)
b.
c. JacoMatrix=[
-1 0 -40*sin(phi1)
0 0 0
d.
0 -1 40*cos(phi1)
0 0 0
e.
1 0 -40*sin(phi1)
-1 0 -130*sin(phi2)
f.
0 1 40*cos(phi1)
0 -1 130*cos(phi2)
g.
0 0 0
1 0 -130*sin(phi2)
h.
0 0 0
0 1 130*cos(phi2)
i.
0 0 0
0 0 0
j.
0 0 0
0 0 0
k.
0 0 1
0 0 0

0 0 0;
0 0 0;
0 0 0;
0 0 0;
-1 0 -90*sin(phi3);
0 -1 90*cos(phi3);
1 0 -90*sin(phi3);
0 1 90*cos(phi3);
0 0 0];
62

Determine the acceleration

20.
21.
22.

dphi1=Vtemp(3); dphi2=Vtemp(6); dphi3=Vtemp(9);

Gamma=Gamma4bar(phi1,phi2,phi3,dphi1,dphi2,dphi3);
Atemp=JacoMatrix\Gamma;

21. Call the function Gamma4bar to obtain the right-side of the velocity equations depended on
current values of velocity.
22. Solve linear equation to obtain the current acceleration.

a. function Gamma=Gamma4bar(phi1,phi2,phi3,dphi1,dphi2,dphi3)
b.
c. Gamma=[ 40*cos(phi1)*dphi1^2;
d.
40*sin(phi1)*dphi1^2;
e.
40*cos(phi1)*dphi1^2+130*cos(phi2)*dphi2^2;
f.
40*sin(phi1)*dphi1^2+130*sin(phi2)*dphi2^2;
g.
130*cos(phi2)*dphi2^2+90*cos(phi3)*dphi3^2;
h.
130*sin(phi2)*dphi2^2+90*sin(phi3)*dphi3^2;
i.
90*cos(phi3)*dphi3^2;
j.
90*sin(phi3)*dphi3^2;
k.
0];
63

Determine next initial positions

29.
30.
31.
32.
33.

if Iter==1
Xinit=X(:,Iter);
else
Xinit=X(:,Iter)+(X(:,Iter)-X(:,Iter-1));
end

29.~33. Predict the next initial positions with assumption of inertia except the first time of the loop.

64

Plot time response

37.figure
38.for i=1:9;
39.
subplot(9,1,i)
40.
plot (T_Int,X(i,:))
41.
set(gca,'xtick',[], 'FontSize', 5)
42.end
43.% Reset the bottom subplot to have xticks
44.set(gca,'xtickMode', 'auto')
45.
46.% T vs velocity plot for the nine coordinates
47.figure
48.for i=1:9;
37.

37. Create a blank figure .

39. Locate the position of subplot in the figure.
40. Plot the nine subplots for the time responses of nine coordinates.
41. Eliminate x-label for time-axis and set the font size of y-label.

44. Resume x-label at bottom because the nine subplots share the same time-axis.
47.~ It is similar to above.
65

Animation
69.Ox=zeros(1,length(T_Int));
70.Oy=zeros(1,length(T_Int));
71.Ax=80*cos(X(3,:));
72.Ay=80*sin(X(3,:));
73.Bx=Ax+260*cos(X(6,:));
74.
80.for
81.
82.
83.
84.
85.
86.
87.
88.
89.
90.
91.
92.
93.end

t=1:length(T_Int);
bar1x=[Ox(t) Ax(t)];
bar1y=[Oy(t) Ay(t)];
bar2x=[Ax(t) Bx(t)];
bar2y=[Ay(t) By(t)];
bar3x=[Bx(t) Cx(t)];
bar3y=[By(t) Cy(t)];

plot (bar1x,bar1y,bar2x,bar2y,bar3x,bar3y);
axis([-120,400,-120,200]);
axis normal
M(:,t)=getframe;

69. Determine the displacement of revolute joint.

80. Repeat to plot the locations by continue time elapsing.
81. Determine the horizontal location of OA .
88. Plot OA, AB, BC, and OC.
89. Set an appropriate range of axis.

66

Time response of displacement

x1

x2
y

x3
y

t
67

Time response of velocity

x1

x 2
y

x 3
y

t
68

Time response of acceleration

x1

x2
y

x3
y

t
69

Example of a slider-crank mechanism

y

11 Constraint equations:
x1 0

y1 0

1 0
x4 x3 200 cos 3 0

G
C

y4 y3 200 sin 3 0
x3 300 cos 3 x2 100 cos 2 0

1
x

y3 300 sin 3 y2 100 sin 2 0

AG 200mm

x2 100 cos 2 x1 0

GB 300mm

y2 100 cos 2 y1 0

BO 200mm

100 cos 4 y1 y4 100 sin 4 100 sin 4 x1 x4 100 cos 4 0

4 1 0
driving link
2 5.76 1.2t 0
To solve the 12 equations for 12 unknown qT x1 , y1 , 1, x2 , y2 , 2 , x3 , y3 , 3 , x4 , y4 , 4

70

Jacobian matrix
1 , 2 , 3 , 6 , 10 , 14 , 18 , 22 , 26 , 34 , 36 1

19 300 cos 3

4 , 8 , 12 , 16 , 20 , 24 , 35 1

28 100 sin 4

5 200 sin 3

29 100 cos 4

9 200 cos 3

31 28

13 , 23 100 sin 2

32 29
33 100cos4 x4 x1 sin 4 y4 y1

15 300 sin 3
17 , 27 100 cos 2

1
2
3
4
5
6
J
7
8
9
10
11
12

1
0
0
0
0
0
0
20
0
28
0
0

0
2
0
0
0
0
0
0
24
29
0
0

0
0
3
0
0
0
0
0
0
0
34
0

0
0
0
0
0
12
0
22
0
0
0
0

0
0
0
0
0
0
16
0
26
0
0
0

0
0
0
0
0
13
17
23
27
0
0
36

0
0
0
4
0
14
0
0
0
0
0
0

0
0
0
0
8
0
18
0
0
0
0
0

0
0
0
5
9
15
19
0
0
0
0
0

0
0
0
6
0
0
0
0
0
31
0
0

0
0
0
0
10
0
0
0
0
32
0
0

0
0

0
0
0

0
0

0
0

33
35

0
0

0
0

0
0

0

0
0

0
0

1.2

71

The right-side
of the acceleration equations
0

2
200
cos

3 3

2
200 sin

3 3

2
2

300 cos 33 100 cos 22

2
2

300 sin 33 100 sin 22

100 cos 2

2 2

100 sin 22

10

2
where 10 200 cos4 x1 x4 4 200 sin 4 y1 y 4 4 4 100 sin 4 x1 x4 100 cos4 y1 y4

72

Time response of displacement

x2
y

x3
y

x4
y

73

Time response of velocity

x 2
y

x 3
y

x 4
y

74

Time response of acceleration

x2
y

x3
y

x4
y

75

Kinematic Modeling
Ground

x1 0.0, y1 0.0, 1 0.0

Revolute joint

4A 0.0,

4A 0.0, 3A 200.0, 3A 0.0

3B 300.0, 3B 0.0,

2B 100.0, 2B 0.0

20 100.0, 20 0.0,

10 0.0,

10 0.0

translational joint

4A 0.0,

4A 0.0, 1B 100.0, 1B 0.0,

4C 100.0, 4C 0.0, 10 0.0,

10 0.0

driving link

2 5.76 1.2t 0.0

Figure Kinematic modeling of a slider-crank mechanism.

76

Translation Joint

y1

x1 100c1 x4
d
y
y

100
s

1
1
4
100c4 y1 100s1 y4 x1 100c1 x4 100s4

100s4
x1

y1 100c4

100 100c4 c1 100s4 s1

1

y1 100s1 y4 100s4 x1 100c1 x4 100c4 0

x c s1 100 x1 100c1
B 1 1
0 y 100s
y
s

1
1 1
1
1
c s4 0 100s4
n 4
100 100c
s

4
4
4
x 100c1 x4
nT d 100 s4 100c4 1

y1 100s1 y4

77

Kinematic Modeling
Ground

1 x1 0.0
2 y1 0.0
3 1 0.0

revolute joints

4 x4 x3 200 cos 3 0
5 y4 y3 200sin 3 0
6 x3 300 cos 3 x2 100 cos 2 0
7 y3 300sin 3 y2 100sin 2 0
8 x2 100 cos 2 x1 0
9 y2 100sin 2 y1 0

translational joint
10 (100cos 4 )( y1 100sin 1 y4 )
( x1 100cos 1 x4 )(100sin 4 ) 0
11 4 1 0

driving constraint
12 2 5.76 1.2t 0

Figure Kinematic modeling of a slider-crank mechanism.

78

Jacobian Matrix

Figure The Jacobian matrix.

79

Contd

30

80

3.2 Solution Technique

kinematic constraints (q) 0
driving link
( d ) (q, t ) 0
velocity equations

acceleration

q = 0orq q 0
q
( d )

q+
= 0orq ( d ) q t ( d ) 0
q
t
q
0
( d ) q =
(d )

q
t

q + q = 0orq q (q q ) q q 0
q
q
q ( d ) q (q ( d ) q) q q 2qt ( d ) q tt ( d ) 0

81

Solution Technique
At any given instant t
(1) Solve q (q)q = 0
q d (q, t )q = (the right hand side)

n equations for n unknowns q(t )

(2) Solve
q (q)q = 0

q d (q, t )q = (the right hand side)

n equations for n unknowns q(t )

(3) Solve
q (q )q =

q d (q )q = (the right hand side)

n equations for n unknowns q(t )

82

4.1 Planar Rigid Body Dynamics

mi xi f xi
mi yi f yi

ii ni

m 0 0 x f x
0 m 0 y f

y
0 0 n i

M i qi = gi
83

Constraint Force
n1

(q, t ) 0,

q 0orq q 0, q
q

mn

There exists Lagrange Multiplier ,

m1

such that T q is that constraint force.

The equations of motion can be written as
Mq g g ( c )
g ( c ) qT
Mq qT g
84

Illustration of Constraint Force

Pure rolling of a disk down a slope
mg

x
y

mgs f mx
mgc N my
mgc N 0
f R I c
4 eqs. for 5 unknowns x, y, , f , N
x R 0
constraint equation
y R 0
2
2g
1
x gs , y 0,
s ,N mgc , f mgs
3
3R
3
85

Generalized coordinate x,
Constraint eq. x R 0 y R 0
x R 0

mgs
q q

mgc

0 0

0
1 0 x
m 0
mgs
0 m

0
0 1 y

mgc

1
2
0
0
mR

R
0

0
2

1 0

R
0 0
1 0
0
0 1

0
0 0 2

2
2g
1
3 eqs. for 3 unknowns x gs , y 0,
s , mgs
3
3R
3
1

mgs

1 0
T

The constraint force

0 1 mgc
q
1

R 0
mgRs
3
31
is the friction force for pure rolling.

86

4.2 Constraint Force in Revolute Joint

1
0
m 0 0 x
fx
1
0 m 0 y

0
1 fy

P
P
0 0 i ( yi yi ) ( xi xi )
n i

mi xi = f ( x)i - l1

mi yi = f ( y)i - l2

mifi = ni - ( yiP - yi )l1 + (xiP - xi )l2

87

Constraint Force in Translational Joint

For a translational joint between i and j,

the equation of motion for body i can be written as

mi xi f xi ( yiP yiQ )1

mi yi f yi ( xiP xiQ )1

ii ni [( x Pj xi )( xiP xiQ ) ( y Pj yi )( yiP yiQ )]1 2

88

4.3 Formulation of Multi-body Dynamic Systems

Mq qT g

n m linear algebraic equations in n m unknowns

for q and .

89

A Matlab Program for

Dynamics of a four-bar linkage
OA 0.08m

M OA 0.08kg

I OA 4.27 105 kg m 2

AB 0.26m

M AB 0.26kg

I AB 1.46 103 kg m 2

BC 0.18m

M BC 0.18kg

I BO 4.86 104 kg m 2

OC 0.18m

g 9.8 m s2
B

A
T 0.1N m
C
O

90

Mass matrix and external force vector

M OA 0.08kg

I OA 4.27 105 kg m 2

g 9.8 m s2

M AB 0.26kg

I AB 1.46 103 kg m 2

M BC 0.18kg

I BO 4.86 104 kg m 2

T 0.1N m

0
0
0.08
0
0.08
0

0
4.27 10 5
0
0
0
0

M 0
0
0
0
0
0

0
0
0

0
0
0
0
0
0

0
0
0
0
0
0
0
0
0
0.26
0
0
0
0.26
0
0
0 1.46 10 3
0
0
0
0
0
0
0
0
0

0
0
0

0
0
0

0
0
0

0
0
0

0
0
0

0
0
0

0.18 0
0

0
0.18
0

4
0
0
4.86 10

0.08 9.8

0
.
1

g 0.26 9.8

0
.
18

9
.
8

91

Jacobian matrix and

OA 0.08m
AB 0.26m
BC 0.18m
OC 0.18m

J 9 9

1 0 0.04 sin
0 1 0.04 cos

0 0.04 sin
1
0
1
0.04 cos

0
0
0
0
0
0

0
0
0

0
0
0
0
0
1

0
0
0
0
0
0
1 0 0.13 sin 2
0 1 0.13 cos 2
1
0 0.13 sin 2
0
1
0.13 cos 2
0
0
0
0
0
0
0
0
0

0
0
0

0
0
0

0
0
0

0
0
0

1 0 0.09 sin 3
0 1 0.09 cos 3
1
0 0.09 sin 3

0
1
0.09 cos 3

0
0
0

0.04 cos

2
0.04 sin

2
2

0.05 cos 0.13 cos 2 2

2
2
0.05 sin 0.13 sin 2 2
0.13 cos 2 2 2 0.09 cos 3 32

2 0.09 sin 2
0
.
13
sin

2
2
3
3

0.09 cos 3 3

2
0.09 sin 3 3

92

Computation
M
The objective is to solve the differenti al equation 99
J 98

J T98 q g
for qt .

O88

M J T98 q g
In numerial computing, the first step is to solve

J
O

98
88
for q091 and 081 with initial position q0 and velocity q 0.

q0 t q t
Integrate
q t , and use the q t and q t

q
0

to repeat the above step for q t and t .

Repeat t t until terminal condition .

93

A convenient way with Matlab solver

Solve initial value problems for ordinary differential equations with
ode45(commended), ode23, ode113
The equations are described in the form of z=f(t,z)

x1
y
1

q
q
Let z 3 , z
x1 q
q
y1

3
94

The syntax for calling solver in Matlab

A vector of initial
conditions

Solution array
[T,Z] = ode45(@Func4Bar,[0:0.005:2],Z0);

column vector of
time points

A vector specifying the interval

of integration
A function that evaluates the right side of the
differential equations

function dz=Func4Bar(t,z)
global L1 L2 L3 L4 torque gravity
phi1=z(3); phi2=z(6); phi3=z(9);
dphi1=z(12); dphi2=z(15); dphi3=z(18);
M=diag([L1 L1 L1^3/12 L2 L2 L2^3/12 L3 L3 L3^3/12]);
J=[ -1 0 -0.5*L1*sin(phi1)
0 -1 0.5*L1*cos(phi1)
1 0 -0.5*L1*sin(phi1)
0 1 0.5*L1*cos(phi1)
0 0 0
0 0 0
0 0 0
0 0 0

0 0 0
0 0 0
-1 0 -0.5*L2*sin(phi2)
0 -1 0.5*L2*cos(phi2)
1 0 -0.5*L2*sin(phi2)
0 1 0.5*L2*cos(phi2)
0 0 0
0 0 0

0 0 0;
0 0 0;
0 0 0;
0 0 0;
-1 0 -0.5*L3*sin(phi3);
0 -1 0.5*L3*cos(phi3);
1 0 -0.5*L3*sin(phi3);
0 1 0.5*L3*cos(phi3)];

95

The syntax for calling solver in Matlab

J=[ -1 0 -0.5*L1*sin(phi1)
0 -1 0.5*L1*cos(phi1)
1 0 -0.5*L1*sin(phi1)
0 1 0.5*L1*cos(phi1)
0 0 0
0 0 0
0 0 0
0 0 0

0 0 0
0 0 0
-1 0 -0.5*L2*sin(phi2)
0 -1 0.5*L2*cos(phi2)
1 0 -0.5*L2*sin(phi2)
0 1 0.5*L2*cos(phi2)
0 0 0
0 0 0

0 0 0;
0 0 0;
0 0 0;
0 0 0;
-1 0 -0.5*L3*sin(phi3);
0 -1 0.5*L3*cos(phi3);
1 0 -0.5*L3*sin(phi3);
0 1 0.5*L3*cos(phi3)];

gamma=[ 0.5*L1*cos(phi1)*dphi1^2;
0.5*L1*sin(phi1)*dphi1^2;
0.5*L1*cos(phi1)*dphi1^2+0.5*L2*cos(phi2)*dphi2^2;
0.5*L1*sin(phi1)*dphi1^2+0.5*L2*sin(phi2)*dphi2^2;
0.5*L2*cos(phi2)*dphi2^2+0.5*L3*cos(phi3)*dphi3^2;
0.5*L2*sin(phi2)*dphi2^2+0.5*L3*sin(phi3)*dphi3^2;
0.5*L3*cos(phi3)*dphi3^2;
0.5*L3*sin(phi3)*dphi3^2];
g=[0 gravity*L1 torque

0 gravity*L2 0

0 gravity*L3 0]';

Matrix=[M J';
J zeros(size(J,1),size(J,1))];
d2q=Matrix\[g;gamma];
dz=[z(10:18,:); d2q(1:9,:)];

96

Time response of displacement

x1

x2
y

x3
y

t
97

Time response of velocity

x1

x 2
y

x 3
y

t
98

Time response of acceleration

103
x1

x2
y

x3
y

t
99

Time response of
1

103

3
4

5
6

8
t
100

A slider-crank mechanism
B

T 0.1N m
G
A

C
O

AC 0.1m

M AC 0.1kg

I slider 6.67 104 kg m 2

AG 0.2 m

M AB 0.5kg

I AB 1.04 102 kg m 2

GB 0.3m

M BO 0.2 kg

I BO 6.67 104 kg m 2

BO 0.2 m

g 9.8 m s2

101

Time response of displacement

x2
y

x3
y

x4
y

102

Time response of velocity

x 2
y

x 3
y

x 4
y

103

Time response of acceleration

103
x2
y

x3
y

x4
y

104

Time response of
1

103

3
4

5
6
7
8

10

11
t
105

5.1 Euler Angles

c c s c s c s s c c s s
A = s c c c s s s c c c c s

s s
s c
c

A DCB

c
D s
0

s
c
0

0
1 0
0 C 0 c
0 s
1

0
c s 0
s B s c 0
0
c
0 1
106

Time Derivatives of Euler Angles

w(x ) = y sin q sin f + q cos f

w(h ) = y sin q cos f - q sin f
w(z ) = y cosq + f
sin sin

sin cos

cos

cos 0
sin 0
0
1

107

Bryant Angles
A DCB

0
1 0
c2
D 0 c1 s1 C 0
0 s1 c1
s2

c2 c3

A = c1s3 s1s2 c3
s1s3 c1s2 c3

0 s2
c3
1 0 B s3
0
0 c2

s3 0
c3 0
0
1

c2 s3
c1c3 s1s2 s3
s1c3 c1s2 s3

s2
s1c2
c1c2
108

Time Derivative of Bryant Angles

cos cos
3
1

cos 2 sin 3

sin 2

sin 3
cos 3
0

0 1

0 2
1 3

109