MATH 4095

Robotics and Automation Project

Veysel GAZI

A Project About The Double

Pendulum or In Other Words
“Imagination”

Bedir HALCI 150716002

Everything started with two masses concentrated on top of two strings and then, everything
slightly become indistinguishable from each other…
Introduction & Such Things
In the beginning, there was no project. Holy beauty of theoretical beings were floating on
the surface of the Moon or in other words, the Lecture.

The ‘Double Pendulum’ (at its nature) is pretty simple. It can be defined as two point
masses concentrated on top of two individual strings where one of them is pinpointed onto the
Origin and the other is pinpointed onto the former.

The Problem
The problem tells us a ‘RR’ planar robot which is needed to be analysed to achieve
dynamics of the robot to be able to control it from a certain point of view.

To Solve
The procedure to overcome the problem is simple to explain in 10 steps: Framing,
Kinematics, Inverse Kinematics, Jacobian, Angular Velocity Propagation, Linear Acceleration
Propagation, Joint Force Propagation, Joint Torque Propagation, Equation of Motion and,
Control.

Framing. The frames can be selected with respect to the Denavit-Hartenberg procedures
which gives 𝑋0 & 𝑌0 that are coinciding with the 𝑋 & 𝑌 on the world frame, 𝑋1 & 𝑌1 which
makes angle of 𝜃1 around 𝑍1 on the base joint, 𝑋2 & 𝑌2 which makes angle of 𝜃2 around 𝑍2 on
the second joint that is at a distance of 𝐿1 and, 𝑋3 & 𝑌3 which coincides with the former at a
distance of 𝐿2 as the last one.

Relevant equations can be found in attachment.

The Code
Overall system is realised in two .m files (EE4095_BH.m and ode4095_bh.m) where the
former one is some kind of runner code that serves as a user interface and responsible of the
plots. But the latter one, as the name suggests, is the function where dynamics are realised
(according to the trajectory that is needed to be tracked) to solve the differential equation to
achieve the system behaviour.

Relevant codes can be found in the attachment.

Conclusion
It seems that, this project was a proper and funny one to work on.
Attachment 1 – Equations & Derivations
Denavit-Hartenberg Parameters

i α_i-1 a_i-1 d_i θ_i

1 0 0 0 θ_1
2 0 L_1 0 θ_2
3 0 L_2 0 0

Kinematics

𝑐1 −𝑠1 0 0 𝑐2 −𝑠2 0 𝐿1 1 0 0 𝐿2
0 𝑠1 𝑐1 0 0] 1𝑇 = [𝑠2 𝑐2 0 0 ] 2𝑇 = [0 1 0 0]
1𝑇 = [ 0 0 1 0 2 0 0 1 0 3 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1
𝑐12 −𝑠12 0 𝐿1 𝑐1 𝑐12 −𝑠12 0 𝐿1 𝑐1 + 𝐿2 𝑐12 𝑐𝜙 −𝑠𝜙 0 𝑥
𝑠12 𝑐12 0 𝐿1 𝑠1 1 𝑠 𝑐12 0 𝐿1 𝑠1 + 𝐿2 𝑠12 2 𝑠𝜙 𝑐𝜙 0 𝑦
0
2𝑇 = [ ] 2𝑇 = [ 12 ] 3𝑇𝑔 = [ ]
0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1

Inverse Kinematics

𝑐𝜙 = 𝑐12 , 𝑠𝜙 = 𝑠12 , 𝑥 = 𝐿1 𝑐1 + 𝐿2 𝑐12 , 𝑦 = 𝐿1 𝑠1 + 𝐿2 𝑠12

𝑥 2 + 𝑦 2 − 𝐿21 − 𝐿22
𝑥 2 + 𝑦 2 = 𝐿21 + 𝐿22 + 2𝐿1 𝐿2 𝑐2 , 𝑐2 = , 𝑠2 = ±√1 − 𝑐22
2𝐿1 𝐿2

𝑥 = 𝑘1 𝑐1 − 𝑘2 𝑠1 , 𝑦 = 𝑘1 𝑠1 + 𝑘2 𝑐1 , 𝑘1 = 𝐿1 + 𝐿2 𝑐2 = 𝑟𝑐𝛾, 𝑘2 = 𝐿2 𝑠2 = 𝑟𝑠𝛾

𝑟 = √𝑘12 + 𝑘22 , 𝛾 = 𝑎𝑡𝑎𝑛2(𝑘2 , 𝑘1 )

𝜃2 = 𝑎𝑡𝑎𝑛2(𝑠2 , 𝑐2 ), 𝜃1 = 𝑎𝑡𝑎𝑛2(𝑦, 𝑥) − 𝑎𝑡𝑎𝑛2(𝑘2 , 𝑘1 ), 𝜙 = 𝜃1 + 𝜃2

Jacobian

𝐿1 𝑐1 + 𝐿2 𝑐12 −𝜃̇1 (𝐿1 𝑠1 + 𝐿2 𝑠12 ) − 𝜃̇2 𝐿2 𝑠12

𝑃𝐸𝐸 = [𝐿1 𝑠1 + 𝐿2 𝑠12 ] , 𝑉𝐸𝐸 = [ 𝜃̇1 (𝐿1 𝑐1 + 𝐿2 𝑐12 ) + 𝜃̇2 𝐿2 𝑐12 ]
0 0

0 0 −(𝐿1 𝑠1 + 𝐿2 𝑠12 ) −𝐿2 𝑠12 −(𝐿1 𝑠1 + 𝐿2 𝑠12 ) −𝐿2 𝑠12

𝐽𝜔 = [0 0] , 𝐽𝑣 = [ 𝐿1 𝑐1 + 𝐿2 𝑐12 𝐿2 𝑐12 ] , 𝐽=[ ]
𝐿1 𝑐1 + 𝐿2 𝑐12 𝐿2 𝑐12
1 1 0 0
Velocity Propagation

1 𝑇 2 𝑇
𝜔1 = [0 0 𝜃̇1 ] , 𝜔2 = [0 0 (𝜃̇1 + 𝜃̇2 ) ]

2 2 𝑇
1 2
𝑣̇ 1 = [𝑔𝑠1 𝑔𝑐1 0]𝑇 , 𝑣̇ 2 = [(𝑔𝑠12 − 𝐿1 𝜃̇1 𝑐2 + 𝐿1 𝜃̈1 𝑐2 ) (𝑔𝑐12 + 𝐿1 𝜃̇1 𝑠2 + 𝐿1 𝜃̈1 𝑐2 ) 0 ]

2 𝑇
1
𝑣̇ 𝑐1 = [(𝑔𝑠1 − 𝐿1 𝜃̇1 ) (𝑔𝑐1 + 𝐿1 𝜃̈1 ) 0]

2 2 2 2
𝑣̇ 𝑐2 = [(𝑔𝑠12 − 𝐿1 𝜃̇1 𝑐2 + 𝐿1 𝜃̈1 𝑠2 − 𝐿2 (𝜃̇1 + 𝜃2̇ ) ) (𝑔𝑐12 + 𝐿1 𝜃̇1 𝑠2 + 𝐿1 𝜃̈1 𝑐2
𝑇
+ 𝐿2 (𝜃̈1 + 𝜃̈2 )) 0 ]

Force Propagation
1
𝐹1 = 𝑚1 1𝑣̇ 𝑐1 , 2
𝐹2 = 𝑚2 2𝑣̇ 𝑐2 , 1
𝑁1 = 0, 2
𝑁2 = 0

3 3 2
𝑓3 = 0, 𝑛3 = 0, 𝑓2 = 𝑚2 2𝑣̇ 𝑐2 , 2
𝑛2 = 𝑚2 𝐿2 2𝑣̇ 𝑐2𝑦 [0 0 1]𝑇

1
𝑓1 = 𝑚2 12𝑅 2𝑣̇ 𝑐2 + 𝑚1 1𝑣̇ 𝑐1 ,
1
𝑛1 = 2𝑛2 + (𝑚1 𝐿1 1𝑣̇ 𝑐1𝑦 + 𝑚2 𝐿2 (𝑠2 2𝑣̇ 𝑐2𝑥 + 𝑐2 2𝑣̇ 𝑐2𝑦 )[0 0 1]𝑇

𝑓1 = 0, 𝑓2 = 0

2
𝜏2 = 𝑚2 𝐿2 (𝑔𝑐12 + 𝐿1 𝜃̈1 𝑐2 + 𝐿1 𝜃̇1 𝑠2 + 𝐿2 (𝜃̈1 + 𝜃̈2 ))

𝜏1 = 𝜏2 + 𝑚1 𝐿1 (𝑔𝑐1 + 𝐿1 𝜃̈1 )
2
+ 𝑚2 𝐿2 (𝑠2 (𝑔𝑠12 + 𝐿1 𝜃̈1 𝑠2 − 𝐿2 (𝜃̇1 + 𝜃̇2 ) )

+ 𝑐2 (𝑔𝑐12 + 𝐿1 𝜃̈1 𝑐2 + 𝐿2 (𝜃̈1 + 𝜃̈2 )))

System Dynamicxs
2
𝜏1 𝜃̈1 ̇ ̇ 𝜃̇1
Γ = [𝜏 ] = 𝑀(𝜃) [ ] + 𝐵(𝜃)[𝜃1 𝜃2 ] + 𝐶(𝜃) [ 2 ] + 𝐺(𝜃)
2 𝜃̈2 𝜃̇2

𝑚2 𝐿2 (𝐿1 𝑐1 + 𝐿2 ) + 𝑚1 𝐿21 + 𝑚2 𝐿2 (𝐿1 + 𝐿2 𝑐2 ) 𝑚2 𝐿22 (1 + 𝑐2 )

𝑀(𝜃) = [ ]
𝑚2 𝐿2 (𝐿1 𝑐2 + 𝐿2 ) 𝑚2 𝐿22

2 𝑚2 𝐿1 𝐿2 𝑠2 − 𝑚2 𝐿22 𝑠2 −𝑚2 𝐿22 𝑠2

𝐵(𝜃) = [−2𝑚2 𝐿2 𝑠2 ] , 𝐶(𝜃) = [ ]
0 𝑚2 𝐿1 𝐿2 𝑠2 0
 𝑚2 𝐿2 𝑔𝑐12 + 𝑚1 𝐿1 𝑔𝑐1 + 𝑚2 𝐿2 𝑔𝑐1
𝐺(𝜃) = [ ]
𝑚2 𝐿2 𝑔𝑐12

Control 1 – Cubic Polynom

3
𝑇
𝜃(𝑡) = ∑ 𝑎𝑖 𝑡 𝑖 , 𝐴𝑎 = [𝜃(0) 𝜃̇(0) 𝜃(𝑡𝑓 ) 𝜃̇(𝑡𝑓 )]
𝑖=0

1 0 0 0
0 1 0 0
𝐴= 1 𝑡𝑓 𝑡𝑓2 𝑡𝑓3 , 𝑎 = [𝑎0 𝑎1 𝑎2 𝑎3 ]𝑇
[0 1 2𝑡𝑓 3𝑡𝑓2 ]

Control 2 – PD Control

𝑋̇ = 𝐽Θ̇, Θ̇ = 𝐽−1 𝑋̇, 𝑋̈ = 𝐽Θ

̇ ̇ + 𝐽Θ̈, Θ̈ = 𝐽−1 𝑋̈ − 𝐽−1 𝐽𝐽̇ −1 𝑋̇

𝜃̇1 𝐿1 𝑐1 + (𝜃̇1 + 𝜃̇2 )𝐿2 𝑐12 (𝜃̇1 + 𝜃̇2 )𝐿2 𝑐12

𝑗̇ = − [ ]
𝜃̇1 𝐿1 𝑠1 + (𝜃̇1 + 𝜃̇2 )𝐿2 𝑠12 (𝜃̇1 + 𝜃̇2 )𝐿2 𝑠12

𝑥 + 𝑟𝑠𝑖𝑛(𝜔𝑡) cos(𝜔𝑡) sin(𝜔𝑡)

𝑋=[ 𝑐 ], 𝑋̇ = 𝑟𝜔 [ ], 𝑋̈ = −𝑟𝜔2 [ ]
𝑦𝑐 + 𝑟𝑐𝑜𝑠(𝜔𝑡) − sin(𝜔𝑡) cos(𝜔𝑡)

𝜏 ′ = Θ̈𝑑 + 𝑘𝑣 𝐸̇ + 𝑘𝑝 𝐸, 𝐸̇ = Θ̇𝑑 − Θ̇, 𝐸 = Θ𝑑 − Θ

Attachment 2 – EE4095_BH.m
close all
clear
clc

%str='haydariyeli';

kp=500*[1 0;0 1];

kv=50*[1 0;0 1];

c=0;
xc=0;
yc=1;
r=0.5;
w=2*pi/0.2;

xf=0;
yf=1;

m1=1;
m2=1;
L1=1;
L2=1;
g=9.8;

th10=0*pi/180;
th20=0*pi/180;
thd10=0;
thd20=0;

mode=2;

tf=2;
dt=0.01;
ts=0:dt:tf(length(tf));
u0=[th10 th20 thd10 thd20];
[t,U]=ode45(@(t,U)
ode4095_bh(U,m1,m2,L1,L2,g,xf,yf,tf,t,c,xc,yc,r,w,kp,kv,th10,th2
0,thd10,thd20,mode),ts,u0);

u1=U(:,1);
u2=U(:,2);
ud1=U(:,3);
ud2=U(:,4);

e1x=L1*cos(u1);
e1y=L1*sin(u1);
e2x=e1x+L2*cos(u1+u2);
e2y=e1y+L2*sin(u1+u2);

figure(1)
plot3(t,u1,ud1)
hold on
plot3(t,u2,ud2)
rotate3d on
grid on
xlabel('t')
ylabel('theta')
zlabel('thetad')

figure(2)
plot(u1,u2);
grid on
xlabel('theta1')
ylabel('theta2')

figure(3)
plot(ud1,ud2);
grid on
xlabel('thetad1')
ylabel('thetad2')

pause;

%V = VideoWriter(str);
%open(V);

figure(4)
for i=1:length(t)
scatter(e1x(i),e1y(i),'b','o')
hold on
scatter(e2x(i),e2y(i),'r','o')
scatter(0,0,'k','o')
plot([0 e1x(i)],[0 e1y(i)],'k')
plot([e1x(i) e2x(i)],[e1y(i) e2y(i)],'k')
if ~c
scatter(xf,yf,80,'p','d')
else

scatter(xc*ones(size(t))+r*sin(w*t),yc*ones(size(t))+r*cos(w*t),
80,'p','d');
end
grid on
 xlim([-L1-L2 L1+L2]);
ylim([-L1-L2 L1+L2]);
xlabel('x')
ylabel('y')
pause(0.001)
%frame = getframe(gcf);
%writeVideo(V,frame);
hold off
end

%close(V);
Attachment 3 – ode4095_bh.m
function d =
ode4095_bh(U,m1,m2,L1,L2,g,xf,yf,tf,t,c,xc,yc,r,w,kp,kv,th10,th2
0,thd10,thd20,mode)
u1=U(1);
u2=U(2);
ud1=U(3);
ud2=U(4);

Mq=[m1*L1*L1 + m2*L2*(L1+L2)*(1+cos(u2)), m2*L2*L2*(1+cos(u2));

m2*L2*(L1*cos(u2)+L2), m2*L2*L2];
Bq=[-2*m2*L2*L2*sin(u2);
0];
Cq=[m2*L2*sin(u2)*(L1-L2), -m2*L2*L2*sin(u2);
m2*L1*L2*sin(u2), 0];
Gq=g*[m1*L1*cos(u1)+m2*L2*(cos(u1)+cos(u1+u2));
m2*L2*cos(u1+u2)];
mq=inv(Mq);

if ~c
uf21=atan2(+sqrt(1-((xf^2+yf^2-L1^2-
L2^2)/(2*L1*L2))^2),((xf^2+yf^2-L1^2-L2^2)/(2*L1*L2)));
uf22=atan2(-sqrt(1-((xf^2+yf^2-L1^2-
L2^2)/(2*L1*L2))^2),((xf^2+yf^2-L1^2-L2^2)/(2*L1*L2)));
k11=L1+L2*cos(uf21);
k12=L1+L2*cos(uf22);
k21=L2*sin(uf21);
k22=L2*sin(uf22);
uf11=atan2(yf,xf)-atan2(k21,k11);
uf12=atan2(yf,xf)-atan2(k22,k12);
if abs(uf11-u1)<abs(uf12-u1)
uf1=uf11;
uf2=uf21;
else
uf1=uf12;
uf2=uf22;
end
if mode==1
kp=0;
kv=0;
A=[1 0 0 0;
0 1 0 0;
1 tf tf^2 tf^3;
0 1 2*tf 3*tf^2];
u=inv(A);
v1=[th10;thd10;uf1;0];
 a1=u*v1;
a10=a1(1);
a11=a1(2);
a12=a1(3);
a13=a1(4);
u1d=a10+a11*t+a12*t^2+a13*t^3;
u1_d=a11+2*a12*t+3*a13*t^2;
u1_dd=2*a12+6*a13*t;
v2=[th20;thd20;uf2;0];
a2=u*v2;
a20=a2(1);
a21=a2(2);
a22=a2(3);
a23=a2(4);
u2d=a20+a21*t+a22*t^2+a23*t^3;
u2_d=a21+2*a22*t+3*a23*t^2;
u2_dd=2*a22+6*a23*t;
else
u1d=uf1;
u1_d=0;
u1_dd=0;
u2d=uf2;
u2_d=0;
u2_dd=0;
end
else
x=xc+r*sin(w*t);
y=yc+r*cos(w*t);
uf21=atan2(+sqrt(1-((x^2+y^2-L1^2-
L2^2)/(2*L1*L2))^2),(((x^2+y^2-L1^2-L2^2)/(2*L1*L2))));
uf22=atan2(-sqrt(1-((x^2+y^2-L1^2-
L2^2)/(2*L1*L2))^2),(((x^2+y^2-L1^2-L2^2)/(2*L1*L2))));
k11=L1+L2*cos(uf21);
k12=L1+L2*cos(uf22);
k21=L2*sin(uf21);
k22=L2*sin(uf22);
uf11=atan2(y,x)-atan2(k21,k11);
uf12=atan2(y,x)-atan2(k22,k12);
XD=w*r*[cos(w*t);-sin(w*t)];
XDD=-w^2*r*[sin(w*t);cos(w*t)];
if abs(uf11-u1)<abs(uf12-u1)
uf1=uf11;
uf2=uf21;
else
uf1=uf12;
uf2=uf22;
end
 J=[-(L1*sin(uf1)+L2*sin(uf1+uf2)), -L2*sin(uf1+uf2);
L1*cos(uf1)+L2*cos(uf1+uf2), L2*cos(uf1+uf2)];
THD=inv(J)*XD;
ufd1=THD(1);
ufd2=THD(2);
JD=-[ufd1*L1*cos(uf1)+(ufd1+ufd2)*L2*cos(uf1+uf2),
(ufd1+ufd2)*L2*cos(uf1+uf2);
ufd1*L1*sin(uf1)+(ufd1+ufd2)*L2*sin(uf1+uf2),
(ufd1+ufd2)*L2*sin(uf1+uf2)];
THDD=inv(J)*(XDD-JD*THD);
ufdd1=THDD(1);
ufdd2=THDD(2);
u1d=uf1;
u1_d=ufd1;
u1_dd=ufdd1;
u2d=uf2;
u2_d=ufd2;
u2_dd=ufdd2;
end
Md=Mq;
Bd=Bq;
Cd=Cq;
Gd=Gq;
E=[u1d-u1;u2d-u2];
ED=[u1_d-ud1;u2_d-ud2];
TT=[u1_dd;u2_dd]+kv*ED+kp*E;
T=Md*TT+Bd*[ud1*ud2]+Cd*[ud1*ud1;ud2*ud2]+Gd;
%T=[0;0];
udd=mq*(T-Bq*[ud1*ud2]-Cq*[ud1*ud1;ud2*ud2]-Gq);
udd1=udd(1);
udd2=udd(2);

d=[ud1;ud2;udd1;udd2];
end