In [20]:

    

from sympy import *
from sympy.physics.vector import dynamicsymbols

x1, y1, x2, y2, x3, y3, g, m1, l1, m2, l2, m3, l3 = symbols('x1 y1 x2 y2 x3 y3 g m1 l1 m2 l2 m3 l3')
theta1, theta2, theta3 = dynamicsymbols('theta1 theta2 theta3')
t = Symbol('t')


    

In [21]:

    

x1 = l1*sin(theta1)
y1 = l1*cos(theta1)
x2 = x1 + l2*sin(theta1 + theta2)
y2 = y1 + l2*cos(theta1 + theta2)
x3 = x2 + l3*sin(theta1 + theta2 + theta3)
y3 = x2 + l3*cos(theta1 + theta2 + theta3)


    

In [22]:

    

T = m1*diff(x1, t)**2/2 + m1*diff(y1, t)**2/2 + m2*diff(x1, t)**2/2 + m2*diff(y1, t)**2/2 + m3*diff(x3, t)**2/2 + m3*diff(y3, t)**2/2
print(T)


    

    




l1**2*m1*sin(theta1(t))**2*Derivative(theta1(t), t)**2/2 + l1**2*m1*cos(theta1(t))**2*Derivative(theta1(t), t)**2/2 + l1**2*m2*sin(theta1(t))**2*Derivative(theta1(t), t)**2/2 + l1**2*m2*cos(theta1(t))**2*Derivative(theta1(t), t)**2/2 + m3*(l1*cos(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*cos(theta1(t) + theta2(t)) - l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*sin(theta1(t) + theta2(t) + theta3(t)))**2/2 + m3*(l1*cos(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*cos(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*cos(theta1(t) + theta2(t) + theta3(t)))**2/2

In [23]:

    

V = m1*g*y1 + m2*g*y2 + m3*g*y3
print(V)


    

    




g*l1*m1*cos(theta1(t)) + g*m2*(l1*cos(theta1(t)) + l2*cos(theta1(t) + theta2(t))) + g*m3*(l1*sin(theta1(t)) + l2*sin(theta1(t) + theta2(t)) + l3*cos(theta1(t) + theta2(t) + theta3(t)))

In [27]:

    

L = T - V

dLdtheta1dot = diff(diff(L, diff(theta1, t)), t)
dLdtheta2dot = diff(diff(L, diff(theta2, t)), t)
dLdtheta3dot = diff(diff(L, diff(theta3, t)), t)

dLdtheta1 = diff(L, theta1)
dLdtheta2 = diff(L, theta2)
dLdtheta3 = diff(L, theta3)

f1 = dLdtheta1dot - dLdtheta1
f2 = dLdtheta2dot - dLdtheta2
f3 = dLdtheta3dot - dLdtheta3


    

In [26]:

    

print(simplify(dLdtheta1dot))
print("")
print(simplify(dLdtheta2dot))
print("")
print(simplify(dLdtheta2dot))
print("")
print(simplify(dLdtheta1))
print("")
print(simplify(dLdtheta2))
print("")
print(simplify(dLdtheta3))
print("")


    

    




l1**2*m1*sin(theta1(t))**2*Derivative(theta1(t), t, t) + l1**2*m1*cos(theta1(t))**2*Derivative(theta1(t), t, t) + l1**2*m2*sin(theta1(t))**2*Derivative(theta1(t), t, t) + l1**2*m2*cos(theta1(t))**2*Derivative(theta1(t), t, t) - m3*(l1*cos(theta1(t)) + l2*cos(theta1(t) + theta2(t)) - l3*sin(theta1(t) + theta2(t) + theta3(t)))*(l1*sin(theta1(t))*Derivative(theta1(t), t)**2 - l1*cos(theta1(t))*Derivative(theta1(t), t, t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))**2*sin(theta1(t) + theta2(t)) - l2*(Derivative(theta1(t), t, t) + Derivative(theta2(t), t, t))*cos(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))**2*cos(theta1(t) + theta2(t) + theta3(t)) + l3*(Derivative(theta1(t), t, t) + Derivative(theta2(t), t, t) + Derivative(theta3(t), t, t))*sin(theta1(t) + theta2(t) + theta3(t))) - m3*(l1*cos(theta1(t)) + l2*cos(theta1(t) + theta2(t)) + l3*cos(theta1(t) + theta2(t) + theta3(t)))*(l1*sin(theta1(t))*Derivative(theta1(t), t)**2 - l1*cos(theta1(t))*Derivative(theta1(t), t, t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))**2*sin(theta1(t) + theta2(t)) - l2*(Derivative(theta1(t), t, t) + Derivative(theta2(t), t, t))*cos(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))**2*sin(theta1(t) + theta2(t) + theta3(t)) - l3*(Derivative(theta1(t), t, t) + Derivative(theta2(t), t, t) + Derivative(theta3(t), t, t))*cos(theta1(t) + theta2(t) + theta3(t))) - m3*(l1*sin(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*sin(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*sin(theta1(t) + theta2(t) + theta3(t)))*(l1*cos(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*cos(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*cos(theta1(t) + theta2(t) + theta3(t))) - m3*(l1*sin(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*sin(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*cos(theta1(t) + theta2(t) + theta3(t)))*(l1*cos(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*cos(theta1(t) + theta2(t)) - l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*sin(theta1(t) + theta2(t) + theta3(t)))

-m3*((l2*cos(theta1(t) + theta2(t)) - l3*sin(theta1(t) + theta2(t) + theta3(t)))*(l1*sin(theta1(t))*Derivative(theta1(t), t)**2 - l1*cos(theta1(t))*Derivative(theta1(t), t, t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))**2*sin(theta1(t) + theta2(t)) - l2*(Derivative(theta1(t), t, t) + Derivative(theta2(t), t, t))*cos(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))**2*cos(theta1(t) + theta2(t) + theta3(t)) + l3*(Derivative(theta1(t), t, t) + Derivative(theta2(t), t, t) + Derivative(theta3(t), t, t))*sin(theta1(t) + theta2(t) + theta3(t))) + (l2*cos(theta1(t) + theta2(t)) + l3*cos(theta1(t) + theta2(t) + theta3(t)))*(l1*sin(theta1(t))*Derivative(theta1(t), t)**2 - l1*cos(theta1(t))*Derivative(theta1(t), t, t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))**2*sin(theta1(t) + theta2(t)) - l2*(Derivative(theta1(t), t, t) + Derivative(theta2(t), t, t))*cos(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))**2*sin(theta1(t) + theta2(t) + theta3(t)) - l3*(Derivative(theta1(t), t, t) + Derivative(theta2(t), t, t) + Derivative(theta3(t), t, t))*cos(theta1(t) + theta2(t) + theta3(t))) + (l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*sin(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*sin(theta1(t) + theta2(t) + theta3(t)))*(l1*cos(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*cos(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*cos(theta1(t) + theta2(t) + theta3(t))) + (l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*sin(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*cos(theta1(t) + theta2(t) + theta3(t)))*(l1*cos(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*cos(theta1(t) + theta2(t)) - l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*sin(theta1(t) + theta2(t) + theta3(t))))

-m3*((l2*cos(theta1(t) + theta2(t)) - l3*sin(theta1(t) + theta2(t) + theta3(t)))*(l1*sin(theta1(t))*Derivative(theta1(t), t)**2 - l1*cos(theta1(t))*Derivative(theta1(t), t, t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))**2*sin(theta1(t) + theta2(t)) - l2*(Derivative(theta1(t), t, t) + Derivative(theta2(t), t, t))*cos(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))**2*cos(theta1(t) + theta2(t) + theta3(t)) + l3*(Derivative(theta1(t), t, t) + Derivative(theta2(t), t, t) + Derivative(theta3(t), t, t))*sin(theta1(t) + theta2(t) + theta3(t))) + (l2*cos(theta1(t) + theta2(t)) + l3*cos(theta1(t) + theta2(t) + theta3(t)))*(l1*sin(theta1(t))*Derivative(theta1(t), t)**2 - l1*cos(theta1(t))*Derivative(theta1(t), t, t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))**2*sin(theta1(t) + theta2(t)) - l2*(Derivative(theta1(t), t, t) + Derivative(theta2(t), t, t))*cos(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))**2*sin(theta1(t) + theta2(t) + theta3(t)) - l3*(Derivative(theta1(t), t, t) + Derivative(theta2(t), t, t) + Derivative(theta3(t), t, t))*cos(theta1(t) + theta2(t) + theta3(t))) + (l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*sin(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*sin(theta1(t) + theta2(t) + theta3(t)))*(l1*cos(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*cos(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*cos(theta1(t) + theta2(t) + theta3(t))) + (l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*sin(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*cos(theta1(t) + theta2(t) + theta3(t)))*(l1*cos(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*cos(theta1(t) + theta2(t)) - l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*sin(theta1(t) + theta2(t) + theta3(t))))

g*l1*m1*sin(theta1(t)) + g*m2*(l1*sin(theta1(t)) + l2*sin(theta1(t) + theta2(t))) - g*m3*(l1*cos(theta1(t)) + l2*cos(theta1(t) + theta2(t)) - l3*sin(theta1(t) + theta2(t) + theta3(t))) - m3*(l1*sin(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*sin(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*sin(theta1(t) + theta2(t) + theta3(t)))*(l1*cos(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*cos(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*cos(theta1(t) + theta2(t) + theta3(t))) - m3*(l1*sin(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*sin(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*cos(theta1(t) + theta2(t) + theta3(t)))*(l1*cos(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*cos(theta1(t) + theta2(t)) - l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*sin(theta1(t) + theta2(t) + theta3(t)))

g*l2*m2*sin(theta1(t) + theta2(t)) - g*m3*(l2*cos(theta1(t) + theta2(t)) - l3*sin(theta1(t) + theta2(t) + theta3(t))) - m3*(l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*sin(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*sin(theta1(t) + theta2(t) + theta3(t)))*(l1*cos(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*cos(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*cos(theta1(t) + theta2(t) + theta3(t))) - m3*(l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*sin(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*cos(theta1(t) + theta2(t) + theta3(t)))*(l1*cos(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*cos(theta1(t) + theta2(t)) - l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*sin(theta1(t) + theta2(t) + theta3(t)))

l3*m3*(g*sin(theta1(t) + theta2(t) + theta3(t)) - (l1*cos(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*cos(theta1(t) + theta2(t)) - l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*sin(theta1(t) + theta2(t) + theta3(t)))*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*cos(theta1(t) + theta2(t) + theta3(t)) - (l1*cos(theta1(t))*Derivative(theta1(t), t) + l2*(Derivative(theta1(t), t) + Derivative(theta2(t), t))*cos(theta1(t) + theta2(t)) + l3*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*cos(theta1(t) + theta2(t) + theta3(t)))*(Derivative(theta1(t), t) + Derivative(theta2(t), t) + Derivative(theta3(t), t))*sin(theta1(t) + theta2(t) + theta3(t)))

In [29]:

    

print(diff(f1, diff(diff(theta1, t), t)))
print("")
print(diff(f1, diff(diff(theta2, t), t)))
print("")
print(diff(f1, diff(diff(theta3, t), t)))
print("")


    

    




l1**2*m1*sin(theta1(t))**2 + l1**2*m1*cos(theta1(t))**2 + l1**2*m2*sin(theta1(t))**2 + l1**2*m2*cos(theta1(t))**2 + m3*(l1*cos(theta1(t)) + l2*cos(theta1(t) + theta2(t)) - l3*sin(theta1(t) + theta2(t) + theta3(t)))*(2*l1*cos(theta1(t)) + 2*l2*cos(theta1(t) + theta2(t)) - 2*l3*sin(theta1(t) + theta2(t) + theta3(t)))/2 + m3*(l1*cos(theta1(t)) + l2*cos(theta1(t) + theta2(t)) + l3*cos(theta1(t) + theta2(t) + theta3(t)))*(2*l1*cos(theta1(t)) + 2*l2*cos(theta1(t) + theta2(t)) + 2*l3*cos(theta1(t) + theta2(t) + theta3(t)))/2

m3*(l2*cos(theta1(t) + theta2(t)) - l3*sin(theta1(t) + theta2(t) + theta3(t)))*(2*l1*cos(theta1(t)) + 2*l2*cos(theta1(t) + theta2(t)) - 2*l3*sin(theta1(t) + theta2(t) + theta3(t)))/2 + m3*(l2*cos(theta1(t) + theta2(t)) + l3*cos(theta1(t) + theta2(t) + theta3(t)))*(2*l1*cos(theta1(t)) + 2*l2*cos(theta1(t) + theta2(t)) + 2*l3*cos(theta1(t) + theta2(t) + theta3(t)))/2

-l3*m3*(2*l1*cos(theta1(t)) + 2*l2*cos(theta1(t) + theta2(t)) - 2*l3*sin(theta1(t) + theta2(t) + theta3(t)))*sin(theta1(t) + theta2(t) + theta3(t))/2 + l3*m3*(2*l1*cos(theta1(t)) + 2*l2*cos(theta1(t) + theta2(t)) + 2*l3*cos(theta1(t) + theta2(t) + theta3(t)))*cos(theta1(t) + theta2(t) + theta3(t))/2