%% Candidate Name: Ravi Tiwari

%% Email: tiwaritiwari@kgpian.iitkgp.ac.in

%% Dynamics of Planer RP Manipulator

% Defining variables
global m1 r1 l1 I1 m2 g
m1 = 1;
r1 = 1;
l1 = 2;
I1 = 1;
m2 = 0.5;
g = -9.81;

theta1 = deg2rad(0); % Initial Positions (Angle)

theta1_dot = deg2rad(0); % Initial Angular Velocities
d2 = 0;
d2_dot = 0;

% Initial Conditions
X0 = [theta1,theta1_dot,d2, d2_dot];

% Time Span
tSpan=0:0.1:20;

% Solving Forward Dynamics Equation using ode45

[tSol, xSol] = ode45(@(t,x) FrwdDynaRP(t,x), tSpan, X0);

% Data for plotting results

theta1 = rad2deg(xSol(:,1));
theta1_dot = rad2deg(xSol(:,2));

d2 = xSol(:,3);
d2_dot = xSol(:,4);

plot(tSol,theta1,'r')
xlabel('Time (s)')
ylabel('theta1 (deg)')
title('theta1 vs Time')

hold on

% plot(tSol,theta1_dot,'r')
% xlabel('Time (s)')
% ylabel('theta1_dot (deg/sec)')
% title('theta1_dot vs Time')
%
% hold on
%
% plot(tSol,d2,'r')
% xlabel('Time (s)')
% ylabel('d2 (m)')
% title('d2 vs Time')
%
% hold on
%
% plot(tSol,d2_dot,'r')
% xlabel('Time (s)')
% ylabel('d2_dot (m/s)')
% title('d2_dot vs Time')

%% Writing the function for Forward Dynamics of RP Manipulator in

State Space Format
function fVal = FrwdDynaRP(t,x)

% x = [theta1, theta1_dot, d2, d2_dot]

global m1 r1 l1 I1 m2 g

%initalizing fVal as [theta1_dot, theta1_dotdot, d2_dot, d2_dotdot]

fVal = zeros(4,1);

% Inertia matrix is defined at 2x2 matrix

M = [(m1*r1^2 + I1 + m2*(l1+x(3))^2), 0 ; 0, m2];

% C matrix as 2x1 matrix for centrifugal and coriollis components

C = [2*m2*(l1+x(3))*x(2)*x(4); -m2*(l1+x(3))*x(2)^2];

% G matrix as 2x1 matrix for gravity terms

G = [(m1*r1+m2*(l1+x(3)))*g*cos(x(1)); m2*g*sin(x(1))];

% Torque/Force matrix as 2x1 matrix for input torque/force, here F2

is
% assumed as -10*d2_dot, as equation in question is written as F(t)
=
% -10*d2(t); moreover if it is -10*d2*t then it keeps on increasing.
I hope
% it is right assumption in absence of clear data

T = [10*(cos(t))^2 + 4*sin(t); -10*(x(4))];

% Solving for theta1_dotdot & d2_dotdot using equation no 34 in

lecture
% slide.
XX= inv(M)*(T-C-G);

% Assigning values to xSol as per definition.

fVal(1,1) = x(2);
fVal(2,1) = XX(1);
fVal(3,1) = x(4);
fVal(4,1) = XX(2);

end
 theta1 vs Time
200

180

160

140

120
theta1 (deg)

100

80

60

40

20

0
0 2 4 6 8 10 12 14 16 18 20
Time (s)

theta1 d ot vs Time
200

150

100
theta1 d ot (deg/sec)

50

-50

-100

-150
0 2 4 6 8 10 12 14 16 18 20
Time (s)
 d2 vs Time
12

10

8
d2 (m)

0
0 2 4 6 8 10 12 14 16 18 20
Time (s)

d2d ot vs Time
2

1.5

1
d2 d ot (m/s)

0.5

-0.5
0 2 4 6 8 10 12 14 16 18 20
Time (s)