Position derivative



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)