Scribd
Upload
38 views

Modelling Assignment5

This Matlab code simulates the motion of two connected rigid bodies over time. It defines constants, initial conditions, and differential equations to model the system. The code numerically integrates the equations of motion to find the position and orientation of each body at discrete time steps. It then plots the bodies at each time step to animate the simulation.

Uploaded by

jaswanthkumarmanem2003
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
38 views

Modelling Assignment5

This Matlab code simulates the motion of two connected rigid bodies over time. It defines constants, initial conditions, and differential equations to model the system. The code numerically integrates the equations of motion to find the position and orientation of each body at discrete time steps. It then plots the bodies at each time step to animate the simulation.

Uploaded by

jaswanthkumarmanem2003
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 8

Matlab code used for solving:

clc
clear all
global m1 m2 J1 J2 a b g xc yc
m1=1;
J1=1;
m2 = 2;
J2 = 2;
a=0.2;
b=0.2;
g=10;
xc=1;
yc=1;
init=[1.2 0.8 pi/2 1.4 (3-sqrt(2))/5 pi/4 0 0 0 0 0 0];
tspan=0:0.1:10;
[t,z]=ode15s(@pinsq,tspan,init);
x1cg=z(:,1);
y1cg=z(:,2);
theta1=z(:,3);
x2cg=z(:,4);
y2cg=z(:,5);
theta2=z(:,6);
for i=1:1:length(t)
r1cg=[x1cg(i) y1cg(i)]';
r2cg = [x2cg(i) y2cg(i)]';
R1=[cos(theta1(i)) -sin(theta1(i)); sin(theta1(i)) cos(theta1(i))];
R2=[cos(theta2(i)) -sin(theta2(i)); sin(theta2(i)) cos(theta2(i))];
r1=r1cg+R1*[-a -b]';
r2=r1cg+R1*[-a b]';
r3=r1cg+R1*[a b]';
r4=r1cg+R1*[a -b]';
r5=r2cg+R2*[a b]';
r6=r2cg+R2*[-a b]';
r7=r2cg+R2*[-a -b]';
r8=r2cg+R2*[a -b]';
plot([r1(1) r2(1) r3(1) r4(1) r1(1) r5(1) r6(1) r7(1) r8(1) r5(1)],[r1(2)
r2(2) r3(2) r4(2) r1(2) r5(2) r6(2) r7(2) r8(2) r5(2) ],'o-');
axis equal
xlim([-0 2])
ylim([-0.5 1.5])
pause(0.1)

hold off
end
pinsq function:

function zdot=pinsq(t,z)
global m1 m2 J1 J2 a b g xc yc
alpha = 10;
beta = 10;
M=diag([m1 m1 J1 m2 m2 J2]);
F=[0 -m1*g 0 0 -m2*g 0]';
x1cg=z(1);
y1cg=z(2);
theta1=z(3);
x2cg=z(4);
y2cg=z(5);
theta2=z(6);
x1dcg=z(7);
y1dcg=z(8);
theta1d=z(9);
x2dcg=z(10);
y2dcg=z(11);
theta2d=z(12);
U=[1 0 -a*sin(theta1)-b*cos(theta1) 0 0 0; 0 1 a*cos(theta1)-b*sin(theta1) 0 0
0;
1 0 a*sin(theta1)+b*cos(theta1) -1 0 a*sin(theta2)+b*cos(theta2);
0 1 -a*cos(theta1)+b*sin(theta1) 0 -1 -a*cos(theta2)+b*sin(theta2)];
v=[(theta1d^2)*(a*cos(theta1)-b*sin(theta1))-alpha*(x1dcg -
theta1d*a*sin(theta1) - theta1d*b*cos(theta1)) - beta*(x1cg+
a*cos(theta1)-b*sin(theta1)-1);
(theta1d^2)*(a*sin(theta1)+b*cos(theta1))-alpha*(y1dcg +
theta1d*a*cos(theta1) - theta1d*b*sin(theta1)) - beta*(y1cg+
a*sin(theta1)+b*cos(theta1)-1);
-(theta1d^2)*(a*cos(theta1)-b*sin(theta1)) -
(theta2d^2)*(a*cos(theta2)-b*sin(theta2)) -
alpha*(x1dcg+a*theta1d*sin(theta1)+b*theta1d*cos(theta1)-x2dcg+a*theta2d*sin(th
eta2)+b*theta2d*cos(theta2))-beta*(x1cg-a*cos(theta1)+b*sin(theta1)-x2cg-a*cos(
theta2)+b*sin(theta2));
-(theta1d^2)*(a*sin(theta1)+b*cos(theta1)) -
(theta2d^2)*(a*sin(theta2)+b*cos(theta2))] -
alpha*(y1dcg-a*theta1d*cos(theta1)+b*theta1d*sin(theta1)-y2dcg-a*theta2d*cos(th
eta2)+b*theta2d*sin(theta2))-beta*(y1cg-a*sin(theta1)-b*cos(theta1)-y2cg-a*sin(
theta2)-b*cos(theta2));
acc=M\F+(M^(-0.5))*pinv(U*(M^(-0.5)))*(v-U*(M\F));
zdot=[z(7) z(8) z(9) z(10) z(11) z(12) acc']';

I have uploaded the video of the simulation in the classroom. The footage
of the simulation starts a little bit late.

You might also like