In [16]:

    

from sympy import *

x, y, theta1, theta2, l1, l2, xs, ys = symbols('x y theta1 theta2 l1 l2 xs ys')


    

In [17]:

    

x = l1*sin(theta1) + l2*sin(theta1+theta2)
y = l1*cos(theta1) + l2*cos(theta1+theta2)


    

In [20]:

    

c = sqrt((x-xs)*(x-xs) + (y-ys)*(y-ys))

print(c)


    

    




sqrt((l1*sin(theta1) + l2*sin(theta1 + theta2) - xs)**2 + (l1*cos(theta1) + l2*cos(theta1 + theta2) - ys)**2)

In [24]:

    

dcdtheta1 = diff(c, theta1)
dcdtheta2 = diff(c, theta2)


    

In [27]:

    

print(simplify(dcdtheta1))
print("")
print(simplify(dcdtheta2))


    

    




(-l1*xs*cos(theta1) + l1*ys*sin(theta1) - l2*xs*cos(theta1 + theta2) + l2*ys*sin(theta1 + theta2))/sqrt((l1*sin(theta1) + l2*sin(theta1 + theta2) - xs)**2 + (l1*cos(theta1) + l2*cos(theta1 + theta2) - ys)**2)

l2*(-l1*sin(theta2) - xs*cos(theta1 + theta2) + ys*sin(theta1 + theta2))/sqrt((l1*sin(theta1) + l2*sin(theta1 + theta2) - xs)**2 + (l1*cos(theta1) + l2*cos(theta1 + theta2) - ys)**2)

In [30]:

    

print(dcdtheta2)


    

    




(l2*(l1*sin(theta1) + l2*sin(theta1 + theta2) - xs)*cos(theta1 + theta2) - l2*(l1*cos(theta1) + l2*cos(theta1 + theta2) - ys)*sin(theta1 + theta2))/sqrt((l1*sin(theta1) + l2*sin(theta1 + theta2) - xs)**2 + (l1*cos(theta1) + l2*cos(theta1 + theta2) - ys)**2)

In [28]:

    

dcdtheta1dtheta1 = diff(dcdtheta1, theta1)
dcdtheta1dtheta2 = diff(dcdtheta1, theta2)
dcdtheta2dtheta1 = diff(dcdtheta2, theta1)
dcdtheta2dtheta2 = diff(dcdtheta2, theta2)


    

In [29]:

    

print(simplify(dcdtheta1dtheta1))
print("")
print(simplify(dcdtheta1dtheta2))
print("")
print(simplify(dcdtheta2dtheta1))
print("")
print(simplify(dcdtheta2dtheta2))
print("")


    

    




(((l1*sin(theta1) + l2*sin(theta1 + theta2) - xs)**2 + (l1*cos(theta1) + l2*cos(theta1 + theta2) - ys)**2)*(l1*xs*sin(theta1) + l1*ys*cos(theta1) + l2*xs*sin(theta1 + theta2) + l2*ys*cos(theta1 + theta2)) - (l1*xs*cos(theta1) - l1*ys*sin(theta1) + l2*xs*cos(theta1 + theta2) - l2*ys*sin(theta1 + theta2))**2)/((l1*sin(theta1) + l2*sin(theta1 + theta2) - xs)**2 + (l1*cos(theta1) + l2*cos(theta1 + theta2) - ys)**2)**(3/2)

l2*((xs*sin(theta1 + theta2) + ys*cos(theta1 + theta2))*((l1*sin(theta1) + l2*sin(theta1 + theta2) - xs)**2 + (l1*cos(theta1) + l2*cos(theta1 + theta2) - ys)**2) + (-l1*sin(theta2) - xs*cos(theta1 + theta2) + ys*sin(theta1 + theta2))*(l1*xs*cos(theta1) - l1*ys*sin(theta1) + l2*xs*cos(theta1 + theta2) - l2*ys*sin(theta1 + theta2)))/((l1*sin(theta1) + l2*sin(theta1 + theta2) - xs)**2 + (l1*cos(theta1) + l2*cos(theta1 + theta2) - ys)**2)**(3/2)

l2*((xs*sin(theta1 + theta2) + ys*cos(theta1 + theta2))*((l1*sin(theta1) + l2*sin(theta1 + theta2) - xs)**2 + (l1*cos(theta1) + l2*cos(theta1 + theta2) - ys)**2) + (-l1*sin(theta2) - xs*cos(theta1 + theta2) + ys*sin(theta1 + theta2))*(l1*xs*cos(theta1) - l1*ys*sin(theta1) + l2*xs*cos(theta1 + theta2) - l2*ys*sin(theta1 + theta2)))/((l1*sin(theta1) + l2*sin(theta1 + theta2) - xs)**2 + (l1*cos(theta1) + l2*cos(theta1 + theta2) - ys)**2)**(3/2)

l2*(-l2*(l1*sin(theta2) + xs*cos(theta1 + theta2) - ys*sin(theta1 + theta2))**2 + ((l1*sin(theta1) + l2*sin(theta1 + theta2) - xs)**2 + (l1*cos(theta1) + l2*cos(theta1 + theta2) - ys)**2)*(-l1*cos(theta2) + xs*sin(theta1 + theta2) + ys*cos(theta1 + theta2)))/((l1*sin(theta1) + l2*sin(theta1 + theta2) - xs)**2 + (l1*cos(theta1) + l2*cos(theta1 + theta2) - ys)**2)**(3/2)