You are on page 1of 42

# PROJECT II

Presented by:
1.
Lokesh Kumar
2.
Majid Hameed
3.
Saurabh Sharma

## Under the Guidance

of

Prof. S K Saha
MULTI-BODY SYSTEMS
& VIBRATION DESIGN

Objective
Dynamic Modeling of a Slider Crank

Mechanism.

## To carry out Inverse Dynamics.

Perform Free and Forced Simulation.
Verification

Frame Representation
Y1

X2
X4

Y3

Z4

X1
X3

Link parameters
Y1

X2

X4

#2
M2,a2

#1
M1,a1

Y3

1
X1

#3
M3,b4

Z4

X3

## Methods used for modeling

Euler Lagrange Independent Generalised

Coordinate Method.
Euler Lagrange Dependent Set of

## Generalised Coordinates Method.

Using NOC / DeNOC and Lagrange

Multipliers.

## Dependent Set Method

d L
L T
( . )
ai i
dt q

q
i
i

Generalised Equation

T
I q h g a
..

I Matrix

(m1a1 / 3) (m2 a12 ) (m2 a22 / 3) (m2 a1a2 cos 2 ) (m2 a22 / 3) (m2 a1a2 cos 2 / 2) 0
2

0
0

(m2 a22 / 3)
0
0

0 0
0 0
0 m3

H Matrix
.

## [(m2 a1a2 1 sin 2 2 (m2 a1a2 22 sin 2 / 2)]

.

m2 a1a2 sin 2
0
0

2
1

G Matrix
(m1a1 g cos 1 / 2)
0
0
0

Constraint Matrix

Jacobian J Relations

## Matlab Program for Euler Lagrange Dependent set of Coordinates

% Initial Values
a1=1;
a2=3;
m1=1;
m2=1;
m3=1;
th0=0;
ths=pi/2;
T=2;
i=1; g=9.81;
for t= 0:0.01:2
% Initializing
th1(i) = th0 + (((ths-th0)/T)*(t-(((T/(2*pi))*sin(2*pi*t/T)))));
dth1(i) = ((ths-th0)/T)*(1-((T/(2*pi))*cos(2*pi*t/T))*(2*pi/T));
ddth1(i) = ((ths-th0)/T^2)*sin(2*pi*t/T)*2*pi;
% Relations
th2(i)=(2*pi)- th1(i) - asin(2*a1*sin(th1(i))/a2) ;
dth2(i)=-(2*a1*cos(th1(i))*dth1(i))/(a2*cos(th1(i)+th2(i))) - dth1(i);

## ddth2(i)= -ddth1(i)- [2*a1*[(cos(th2(i)+ th1(i))*((cos(th1(i))*ddth1(i))(sin(th1(i))*dth1(i)*dth1(i)))) + (cos(th1(i))*sin(th2(i)+th1(i))*dth1(i)*(dth2(i)+dth1(i)))]/

(cos(th2(i)+th1(i))*cos(th2(i)+th1(i))*a2)];
th3(i)=(2.5*pi)-(th1(i)+th2(i));
dth3(i)=-(dth1(i)+dth2(i));
ddth3(i)=-(ddth1(i)+ddth2(i));
ddb4(i)=(a1*cos(th1(i))*dth1(i))+(a1*sin(th1(i))*ddth1(i))-(a1*cos(th1(i))*tan(th1(i)
+th2(i))*ddth1(i))+(sin(th1(i))*tan(th1(i)+th2(i))*(dth1(i)*dth1(i)))-(cos(th1(i))*(sec(th1(i)
+th2(i))*sec(th1(i)+th2(i))*dth1(i)*(dth1(i)+dth2(i))));
% ddtheta vectors
ddq=[ddth1(i);ddth2(i);ddth3(i);ddb4(i)];
% Inertia Matrix
i11 = (m1*a1*a1/3)+(m2*a1*a1)+(m2*a2*a2/3)+(m2*a1*a2*cos(th2(i))); i12 =
(m2*a2*a2/3)+(m2*a1*a2*cos(th2(i))/2); i13=0; i14=0;
i21 = (m2*a2*a2/3)+(m2*a1*a2*cos(th2(i))/2); i22 = m2*a2*a2/3; i23=0; i24=0;
i31=0; i32=0; i33=0; i34=0;
i41=0; i42=0; i43=0; i44=m3;
I=[i11 i12 i13 i14;i21 i22 i23 i24;i31 i32 i33 i34;i41 i42 i43 i44];
% H matrix
h1=-(m2*a1*a2*dth1(i)*dth2(i)*sin(th2(i)))-(m2*a1*a2*dth2(i)*dth2(i)*sin(th2(i))/2);

## h2=(m2*a1*a2*sin(th2(i))*dth1(i)*dth1(i)/2); h3=0; h4=0;

H=[h1;h2;h3;h4];
% G matrix
g1=m1*g*a1*cos(th1(i))/2; g2=0; g3=0; g4=0;
G=[g1;g2;g3;g4];
% Constraint Matrix [Jacobian]
a11=(-a1*sin(th1(i)))-(a2*sin(th1(i)+th2(i))/2); a21=-(a2*sin(th1(i)+th2(i))/2); a31=0;
a41=1;
a12=(a1*cos(th1(i)))+(a2*cos(th1(i)+th2(i))/2); a22=a2*cos(th1(i)+th2(i))/2; a32=0;
a42=0;
a13=1; a23=1; a33=1; a43=0;
A=[a11 a12 a13 ;a21 a22 a23 ;a31 a32 a33 ;a41 a42 a43];
% J Matrix 'Orthogonal Complement of Constraint Matrix'
j11=1;
j21=(-1-(2*a1*cos(th1(i))/(a2*cos(th2(i)))));
j31= 2*a1*cos(th1(i)) / (a2*cos(th1(i)+th2(i)));
j41=((a1*sin(th1(i)))-(a1*cos(th1(i))*tan(th1(i)+th2(i))));
J=[j11 j21 j31 j41];
% Torque matrix
Tor(i) = (J*I*ddq) + (J*H) + (J*G)

i=i+1;
end
t=0:0.01:2
subplot(2,2,1);
plot(t,ddb4,'linewidth',4)
grid on;
%figure(2)
subplot(2,2,2);
plot(t,th1,t,dth1,t,ddth1)
grid on;
subplot(2,2,3);
plot(t,th2,t,dth2,t,ddth2)
subplot(2,2,4);
plot(t,th3,t,dth3,t,ddth3)
figure(4);
plot(t,Tor)
pp1=spline(t,Tor);
save('td1.mat','pp1')

## Euler Lagrange Independent

Coordinate Method

Contd.

## Matlab Program for Independent Euler Lagrange Method

m1=1; m2=1;
m3=1; a1=1;a2=3;
g=9.81; i=1; q1= 0; q2= (pi/2); T=2;
for t= 0:0.01:2
th1(i)= (((q2-q1))/T)*(t-((T/(2*pi))*sin(2*pi*t/T)));
thd1(i)=(((q2-q1)/T)*(1-cos(2*pi*t/T)));
thdd1(i)= ((q2-q1)/T)*(2*pi/T)*(sin(2*pi*t/T));
th2(i)=asin(-2*a1*sin(th1(i))/a2)-th1(i);
thd2(i)=(-1-2*a1*cos(th1(i))/(a2*cos(th1(i)+th2(i))))*thd1(i);
thdd2(i)=(2*a1*(sin(th1(i))*(thd1(i))*(thd1(i))-cos(th1(i))*(thdd1(i)))/a2+sin(th1(i)
+th2(i))*((thd1(i))+thd2(i))*((thd1(i))+thd2(i)))/cos(th1(i)+th2(i))-(thdd1(i));
bd(i)=a1*sin(th1(i))*thd1(i)+a2*sin(th1(i)+th2(i))*(thd1(i)+thd2(i))/2;
bdd(i)=a1*sin(th1(i))*thdd1(i)+a1*cos(th1(i))*thd1(i)*thd1(i)+a2*sin(th1(i)
+th2(i))*((thdd1(i))+thdd2(i))/2+a2*cos(th1(i)+th2(i))*(thd1(i)+thd2(i))*(thd1(i)+thd2(i));
s(i)=m1*a1*a1/3+m2*a1*a1+m2*a1*a2*cos(th2(i))/2-2*m2*a1*a2*cos(th1(i))/(3*cos(th1(i)
+th2(i)))-m2*a1*a1*cos(th1(i))*cos(th2(i))/cos(th1(i)+th2(i));
u(i)=m2*a1*a2*cos(th2(i))/2-2*m2*a1*a2*cos(th1(i))/(3*cos(th1(i)+th2(i)));

v(i)=m3*a1*sin(th1(i))-m3*a1*cos(th1(i))*sin(th1(i)+th2(i))/cos(th1(i)+th2(i));
w(i)=-m2*a1*a2*sin(th2(i))*thd2(i)*(thd1(i)/2+thd2(i)/2-a1*cos(th1(i))*thd1(i)/(a2*cos(th1(i)
+th2(i))));
x(i)=(cos(th1(i))*sin(th1(i)+th2(i))*(thd1(i)+thd2(i))-cos(th1(i)+th2(i))*sin(th1(i))*thd1(i))*(m2*a1*a1*cos(th2(i))*thd1(i)-2*m2*a1*a2*(thd1(i)+thd2(i))/3)/(cos(th1(i)+th2(i))*cos(th1(i)
+th2(i)));
y(i)=m3*bd(i)*(a1*cos(th1(i))*thd1(i)-a1*cos(th1(i))*(thd1(i)+thd2(i))/(cos(th1(i)
+th2(i))*cos(th1(i)+th2(i)))+a1*sin(th1(i))*sin(th1(i)+th2(i))*thd1(i)/cos(th1(i)+th2(i)));
z(i)=m2*a1*a2*(1+2*a1*cos(th1(i))/(a2*cos(th1(i)+th2(i))))*sin(th2(i))*(thd1(i)*thd1(i)
+thd1(i)*thd2(i))/2-m1*g*a1*cos(th1(i))/2;
tor(i)=s(i)*thdd1(i)+u(i)*thdd2(i)+v(i)*bdd(i)+w(i)+x(i)+y(i)-z(i);
i=i+1;
end
t= 0:0.01:2
figure(11)
plot(t,tor);
pp=spline(t,tor)
save ('torquedata.mat',' pp')

Pendode Function
function dy=pendodes(t,y)
load torquedata.mat;
tor=ppval(pp,t);
dy=zeros(2,1)
m1=1; m2=1; m3=1; a1=1; a2=3; g=9.81;
th2=-y(1)-asin((2*a1*sin(y(1)))/a2);
thd2=-(1+(2*a1*cos(y(1)))/(a2*cos(y(1)+th2)))*y(2);
bd=a1*sin(y(1))*y(2)+a2*sin(y(1)+th2)*(y(2)+thd2)/2;
i=m1*a1*a1/3+m2*a1*a1+m2*a1*a2*cos(th2)/2-2*m2*a1*a2*cos(y(1))/(3*cos(y(1)+th2))m2*a1*a1*cos(y(1))*cos(th2)/cos(y(1)+th2);
j=(m2*a1*a2*cos(th2)/2-2*m2*a1*a2*cos(y(1))/(3*cos(y(1)+th2)))*(-1-2*a1*cos(y(1))/
(a2*cos(y(1)+th2)))*(a1*sin(y(1))-(a1*cos(y(1))*tan(y(1)+th2)));
k=m3*a1*sin(y(1))-m3*a1*cos(y(1))*sin(y(1)+th2)/cos(y(1)+th2);
p=2*a1/a2*y(2)*(sin(y(1))*y(2)-cos(y(1))*tan(y(1)+th2)*(y(2)+thd2));
q=a1*cos(y(1))*y(2)*y(2)-a1*cos(y(1))*y(2)*(y(2)+thd2)/(cos(y(1)+th2)*cos(y(1)+th2))
+a1*sin(y(1))*y(2)*y(2)*tan(y(1)+th2);

w=-m2*a1*a2*sin(th2)*thd2*(y(2)/2+thd2/2-a1*cos(y(1))*y(2)/(a2*cos(y(1)+th2)));
x=(cos(y(1))*sin(y(1)+th2)*(y(2)+thd2)-cos(y(1)+th2)*sin(y(1))*y(2))*(m2*a1*a1*cos(th2)*y(2)-2*m2*a1*a2*(y(2)+thd2)/3)/(cos(y(1)+th2)*cos(y(1)+th2));
u=m3*bd*(a1*cos(y(1))*y(2)-a1*cos(y(1))*(y(2)+thd2)/(cos(y(1)+th2)*cos(y(1)+th2))
+a1*sin(y(1))*sin(y(1)+th2)*y(2)/cos(y(1)+th2));
z=m2*a1*a2*(1+2*a1*cos(y(1))/(a2*cos(y(1)+th2)))*sin(th2)*(y(2)*y(2)+y(2)*thd2)/2m1*g*a1*cos(y(1))/2;
thdd1=(tor-(p+q+w+x+u-z))/(i+j+k);
dy(1)=y(2);
dy(2)=thdd1;

## Matlab Program for Solution using Lagrange Multipliers and NOC

clc;clf;clear;
a1=1;
a=1;
a2=3;
b=3;
g=9.81;
m1=1;
m2=1;
m3=1;
i=1;
a0=4;
q1= 0;
q2=(.5*pi);
T=2;
for t1= 0:0.01:2
th1(i)= (((q2-q1))/T)*(t1-((T/(2*pi))*sin(2*pi*t1/T)));
thd1(i)=(((q2-q1)/T)*(1-cos(2*pi*t1/T)));
thdd1(i)= ((q2-q1)/T)*(2*pi/T)*(sin(2*pi*t1/T));
th2(i)=-th1(i)-asin((2*a1*sin(th1(i)))/a2);
thd2(i)=-(1+(2*a1*cos(th1(i)))/(a2*cos(th1(i)+th2(i))))*thd1(i);

thdd2(i)=sec(th1(i)+th2(i))*[2*a1/a2]*(sin(th1(i))*thd1(i)*thd1(i)-cos(th1(i))*thdd1(i))
+sec(th1(i)+th2(i))*(sin(th1(i)+th2(i))*(th1(i)+th2(i))*(th1(i)+th2(i)))-thdd1(i);
b4(i)=a0-a1*cos(th1(i))-0.5*a2*cos(th1(i)+th2(i))
bd4(i)=(a1*sin(th1(i))-a1*cos(th1(i))*sin(th1(i)+th2(i))/cos(th1(i)+th2(i)))*thd1(i);
bdd4(i)=(a1*sin(th1(i))-(a1*cos(th1(i))*sin(th1(i)+th2(i)))/cos(th1(i)+th2(i)))*thdd1(i)+
(a1*cos(th1(i))*thd1(i)*thd1(i))-(a1*cos(th1(i))*thd1(i)*(thd1(i)+thd2(i)))/(cos((th1(i)
+th2(i)))*cos((th1(i)+th2(i))))+a1*thd1(i)*sin(th1(i))*tan(th1(i)+th2(i))*thd1(i);
i11(i)=(m1*a*a/3)+m2*(a*a+a*b*cos(th2(i))+b*b/3);
i12(i)=m2*((b*b/3)+0.5*a*b*cos(th2(i)));
i21(i)=m2*((b*b/3)+0.5*a*b*cos(th2(i)));
i22(i)=m2*b*b/3;
h1(i)=-m2*a*b*sin(th2(i))*thd1(i)*thd2(i)-m2*(a/2)*b*sin(th2(i))*thd2(i)*thd2(i);
h2(i)=m2*(a/2)*b*sin(th2(i))*thd1(i)*thd1(i);
g1(i)=m1*g*a/2*cos(th1(i))+m2*g*(a*cos(th1(i))+b/2*cos(th1(i)+th2(i)));
g2(i)=m2*g*b/2*cos(th1(i)+th2(i));

s11(i)=(i11(i)*thdd1(i)+i12(i)*thdd2(i)+h1(i)+g1(i));
s12(i)=(a1*sin(th1(i))+a2*.5*sin(th1(i)+th2(i)))* m3*bdd4(i);
s13(i)=-((a1*cos(th1(i))+a2*.5*cos(th1(i)+th2(i)))*2*(i21(i)*thdd1(i)+i22(i)*thdd2(i)+h2(i)
+g2(i)+a2*.5*sin(th1(i)+th2(i))*m3*bdd4(i))/(a2*cos(th1(i)+th2(i))));
s1(i)=s11(i)+s12(i)+s13(i)
i=i+1;
end
t1=0:0.01:2
figure(1)
plot(t1,s1)
figure(2)
plot(t1,th1,'-',t1,thd1,':',t1,thdd1,'.')
legend('1st link pos','1st link velocity','1st link accelaratio')
figure(3)
plot(t1,th2,'-',t1,thd2,':',t1,thdd2,'.')
legend('2nd link pos','2nd link velocity','2nd link accelaratio')
figure(4)
plot(t1,bd4,':',t1,bdd4,'.')
legend('slider position','slider velocity','slider accelaration')
pp1=spline(t1,s1);
save('torquedata1.mat','pp1')

Pendode Function
function dy=pendode(t,y)
dy=zeros(2,1)
m1=1;m2=1;m3=1;a1=1;a2=3;a3=1;g=9.81;
th1=y(1);
thd1=y(2);
T1=0;
th2=-th1-asin((2*a1*sin(th1))/a2);
thd2=-(1+(2*a1*cos(th1))/(a2*cos(th1+th2)))*thd1;
thdd1=3*cos(th1+th2)^2/a1^2/(cos(th1+th2)^2*m1+3*cos(th1+th2)^2*m2
-6*m2*cos(th1+th2)*cos(th2)*cos(th1)+4*cos(th1)^2*m2
+6*cos(th1)*sin(th1+th2)*m3*cos(th1+th2)*sin(th1)
-3*cos(th1)^2*sin(th1+th2)^2*m3
-3*cos(th1+th2)^2*m3*sin(th1)^2)*(T1+m2*a1*a2*sin(th2)*thd1*thd2
+1/2*m2*a1*a2*sin(th2)*thd2^2-1/2*m1*g*a1*cos(th1)-m2*g*(a1*cos(th1)
+1/2*a2*cos(th1+th2)))-3/a2*cos(th1+th2)*(2*a1*cos(th1)
+a2*cos(th1+th2))/a1^2/(cos(th1+th2)^2*m1+3*cos(th1+th2)^2*m2
-6*m2*cos(th1+th2)*cos(th2)*cos(th1)
+4*cos(th1)^2*m2+6*cos(th1)*sin(th1+th2)*m3*cos(th1+th2)*sin(th1)
-3*cos(th1)^2*sin(th1+th2)^2*m3
-3*cos(th1+th2)^2*m3*sin(th1)^2)*(-1/2*m2*a1*a2*sin(th2)*thd1^2
-1/2*m2*g*a2*cos(th1+th2))+1/a1*(-3*m2*cos(th1+th2)*cos(th2)
+4*m2*cos(th1)+3*sin(th1+th2)*m3*cos(th1+th2)*sin(th1)

-3*sin(th1+th2)^2*m3*cos(th1))/(cos(th1+th2)^2*m1+3*cos(th1+th2)^2*m2
-6*m2*cos(th1+th2)*cos(th2)*cos(th1)+4*cos(th1)^2*m2
+6*cos(th1)*sin(th1+th2)*m3*cos(th1+th2)*sin(th1)-3*cos(th1)^2*sin(th1+th2)^2*m3
3*cos(th1+th2)^2*m3*sin(th1)^2)*(a1*sin(th1)*thd1+1/2*a2*sin(th1+th2)*(thd1+thd2)*thd
1
+1/2*a2*sin(th1+th2)*(thd1+thd2)*thd2)-3*cos(th1+th2)*m3/a1*(-cos(th1+th2)*sin(th1)
+sin(th1+th2)*cos(th1))/(cos(th1+th2)^2*m1+3*cos(th1+th2)^2*m2
-6*m2*cos(th1+th2)*cos(th2)*cos(th1)+4*cos(th1)^2*m2
+6*cos(th1)*sin(th1+th2)*m3*cos(th1+th2)*sin(th1)-3*cos(th1)^2*sin(th1+th2)^2*m3
3*cos(th1+th2)^2*m3*sin(th1)^2)*((a1*cos(th1)*thd1+1/2*a2*cos(th1+th2)*(thd1+thd2))*t
hd1
+1/2*a2*cos(th1+th2)*(thd1+thd2)*thd2);
dy(1)=y(2);
dy(2)=thdd1;

clc;clf;clear;
syms b s a m1 m2 m3 a1 a2 b4 th1 th2 g T c d thd1 thd2
S=[(m1*a1*a1/3)+m2*(a1*a1+a1*a2*cos(th2)+a2*a2/3)
2*((a2*a2/3)+0.5*a1*a2*cos(th2))
0 (a1*sin(th1)+a2*.5*sin(th1+th2)),
-(a1*cos(th1)+a2*.5*cos(th1+th2));
m2*((a2*a2/3)+0.5*a1*a2*cos(th2)),
m2*a2*a2/3,
0,
-a2*.5*sin(th1+th2),
-2*0.5*cos(th1+th2));
0, 0 , m3 , -1, 0;
(a1*cos(th1)+a2*0.5*cos(th1+th2)), a2*0.5*cos(th1+th2), 0 , 0, 0;
(-a1*sin(th1)-a2*0.5*sin(th1+th2)), -(a2*0.5*sin(th1+th2)), -1, 0 , 0];
c=[T-(-m2*a1*a2*sin(th2)*thd1*thd2-m2*(a1/2)*a2*sin(th2)*thd2*thd2)(m1*g*a1/2*cos(th1)+m2*g*(a1*cos(th1)+a2/2*cos(th1+th2)));
-(m2*(a1/2)*a2*sin(th2)*thd1*thd1)-m2*g*a2/2*cos(th1+th2);
0;
a1*sin(th1)*thd1+a2*0.5*sin(th1+th2)*(thd1+thd2)*thd1)+
(a2*0.5*sin(th1+th2)*(thd1+thd)*thd2
((a1*cos(th1)*thd1+a2*0.5*cos(th1+th2)*(thd1+thd2))*thd1)+
(a2*0.5*cos(th1+th2)*(thd1+thd2)*thd2)];
d=inv(S)*c

RESULTS
INVERSE DYNAMICS
FREE SIMULATION
FORWARD DYNAMICS

TORQUE OBTAINED

FREE SIMULATION

Forward Dynamics
10
link1 pos
link2 vel
8

-2

0.2

0.4

0.6

0.8

1.2

1.4

1.6

1.8

Verification
6
link1 pos
link2 vel

-2

-4

-6

0.2

0.4

0.6

0.8

1.2

## M2=0; m3=0; a2=0;

1.4

1.6

1.8

Verification

link1 posi
link1 vel

4
3
2
1
0
-1
-2
-3
-4

0.5

1.5

2.5

3.5

4.5

ROBO ANALYSER
Comparison of Results obtained
from Matlab and Robo-Analyser
for RP manipulator System.