# 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)

``````