Robotic Mechanics and Modeling/Kinematics/Additional Examples for Jacobian Matrices for Robotics

Example 1 (Spring '20 - Team 1)

Amazon Robot picking up a package.

You are working for Amazon and need to go on a break. The company has developed some robots that can do basic tasks, so while you are gone, it can do some of the heavy lifting for you. The robot is a mobile robot with a 3-revolute joint arm which is similar to the sketch shown in the figure.

We want to find the end-effector velocity from the joint positions and rates slightly before the robot picks up a certain package. In order from link 1 to 3, the lengths are 4 $m$, 2.5 $m$, and 3 $m$, the angles ($\theta$) with respect to the x-axis or the floor are 30 degrees, 45 degrees, and -50 degrees, and the joint velocities are 2 degrees/sec, -2 degrees/sec, and -6 degrees/sec, respectively.

%reset -f
import numpy as np
import sympy as sp

#Link Angles declared as symbols in order to perform partial derivatives
theta1, theta2, theta3 = sp.symbols('theta1 theta2 theta3')
#Link Lengths (m)
l1=4 
l2=2.5 
l3=3 

f1_f0_T=np.array([[sp.cos(theta1), -sp.sin(theta1), 0, 0],
             [sp.sin(theta1), sp.cos(theta1), 0, 0],
             [0, 0, 1, 0],
             [0, 0, 0, 1]])

f2_f1_T=np.array([[sp.cos(theta2), -sp.sin(theta2), 0, l1],
            [sp.sin(theta2), sp.cos(theta2),0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])

f3_f2_T=np.array([[sp.cos(theta3), -sp.sin(theta3), 0, l2],
            [sp.sin(theta3), sp.cos(theta3),0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])

f4_f3_T=np.array([[1, 0, 0, l3],
            [0, 1, 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])

End_Coord=np.transpose(np.array([[0,0,0,1]]))
X=np.dot(f1_f0_T,np.dot(f2_f1_T,np.dot(f3_f2_T,np.dot(f4_f3_T,End_Coord))))
X=sp.simplify(X)
print("X:\n",X)

#End Effector Position with angular partial derivatives separated since sp.subs does not work on arrays in numpy
dx_dth1=sp.diff(X[0],theta1)
dx_dth2=sp.diff(X[0],theta2)
dx_dth3=sp.diff(X[0],theta3)
dy_dth1=sp.diff(X[1],theta1)
dy_dth2=sp.diff(X[1],theta2)
dy_dth3=sp.diff(X[1],theta3)
dx_dth1=dx_dth1.subs([(theta1,np.pi*30/180),(theta2,np.pi*45/180),(theta3,-np.pi*50/180)])
dx_dth2=dx_dth2.subs([(theta1,np.pi*30/180),(theta2,np.pi*45/180),(theta3,-np.pi*50/180)])
dx_dth3=dx_dth3.subs([(theta1,np.pi*30/180),(theta2,np.pi*45/180),(theta3,-np.pi*50/180)])
dy_dth1=dy_dth1.subs([(theta1,np.pi*30/180),(theta2,np.pi*45/180),(theta3,-np.pi*50/180)])
dy_dth2=dy_dth2.subs([(theta1,np.pi*30/180),(theta2,np.pi*45/180),(theta3,-np.pi*50/180)])
dy_dth3=dy_dth3.subs([(theta1,np.pi*30/180),(theta2,np.pi*45/180),(theta3,-np.pi*50/180)])

#Partial derivatives inserted into the Jacobian
J=np.array([[dx_dth1,dx_dth2,dx_dth3],
          [dy_dth1,dy_dth2,dy_dth3]])
print("J:\n",J)

#Joint Angular Velocities (radians/sec)
theta1dot=np.pi*2/180
theta2dot=-np.pi*2/180
theta3dot=-np.pi*6/180
thetadot=np.transpose(np.array([[theta1dot, theta2dot, theta3dot]]))

#End Effector Velocity from Joint Rates
f0_v_4=np.dot(J,thetadot)
print("f0_v_4:\n",f0_v_4)

First, we needed to form the transformation matrices between each joint to the next and multiply them, starting with the last transformation matrix and the end coordinates. This gave us the change in position coordinates. X:

[[4.0*cos(theta1) + 2.5*cos(theta1 + theta2) + 3.0*cos(theta1 + theta2 + theta3)], [4.0*sin(theta1) + 2.5*sin(theta1 + theta2) + 3.0*sin(theta1 + theta2 + theta3)], [0], [1]]

Then we had to differentiate each coordinate with respect to each joint angle, symbolically, and have their numerical joint angle values substituted, which needed to be done before inserting them into the Jacobian Matrix. J:

[[-5.68266935094477 -3.68266935094477 -1.26785478522210]
[6.83007258900401 3.36597097386625 2.71892336110995]]

The Jacobian Matrix was then dotted with the joint angular velocities which gave us our final result for the End Effector velocity components. As you can see, the x-component is positive (0.0630

${\frac {m}{s}}$) and the y-component is negative (-0.1638 ${\frac {m}{s}}$)  . This is expected, since we want the robot to inch forward while descending its arm down to the package to then clamp onto it.

f0_v_4:

[[0.0629562725559737]
[-0.163805030948942]]

Example 2 (Spring '20 - Team 2)

A scuba diver and his buddy are at the bottom of the ocean cleaning up plastics and trash that has accumulated there. They have with them a two-link trash grabber for efficiency and reachability. They start to wonder how the speed of end effector changes as they change the speed of the handle and joints in hopes they can pick up more trash before the end of their dive. Find the Jacobian matrix and the end effector velocity of the trash grabber if the length of the first link is $\mathbb {1} \ m$, the length of the second link is $\mathbb {0} .8\ m$, the current angle of the joints are $\mathbb {1} 0^{\circ }$ and $\mathbb {2} 5^{\circ }$respectively, and the angular velocities are $\mathbb {5} \,rad/sec$ and $\mathbb {1} 0\,rad/sec$.

We start with describing the problem and stating the given information as comments.

%reset -f
import numpy as np
import sympy as sp

# l1 = 1.0 m, theta1 = 10 degrees
# l2 = 0.8 m, theta2 = 25 degrees
# theta1dot = 5 rad/seconds, theta2dot = 10 rad/seconds

Initialize the variables and setting the arrays for transformation matrix.

# Initializing variables in sympy
theta1, theta2 = sp.symbols('theta1 theta2')
l1, l2 = sp.symbols('l1 l2')
theta1dot, theta2dot = sp.symbols('theta1dot theta2dot')

# Matrix operations from one frame to another
f1_f0_T=np.array([[sp.cos(theta1), -sp.sin(theta1), 0, 0],
                  [sp.sin(theta1),  sp.cos(theta1), 0, 0],
                  [0, 0, 1, 0],
                  [0, 0, 0, 1]])

f2_f1_T=np.array([[sp.cos(theta2), -sp.sin(theta2), 0, l1],
                  [sp.sin(theta2),  sp.cos(theta2),0, 0],
                  [0, 0, 1, 0],
                  [0, 0, 0, 1]])

f3_f2_T=np.array([[1, 0, 0, l2],
                  [0, 1, 0, 0],
                  [0, 0, 1, 0],
                  [0, 0, 0, 1]])

End_Coord=np.transpose(np.array([[0,0,0,1]]))

Next steps include:

Writing the kinematic equations

Computing the Jacobian Matrix and

Creating an array for the angular velocities.

# Kinematics equations
X = np.dot(f1_f0_T, np.dot(f2_f1_T, np.dot(f3_f2_T, End_Coord)))
X = sp.simplify(X)

# Finding the Jacobian matrix
J = np.array([[sp.diff(X[0],theta1), sp.diff(X[0],theta2)],
              [sp.diff(X[1],theta1), sp.diff(X[1],theta2)]])

# Creating array of angular velocity
thetadot = np.array([[theta1dot, theta2dot]])
thetadot = thetadot.T

# Calculating velocity of End Effector relative to frame 0
f0_v_3 = np.dot(J, thetadot)

# Resulting expression
print("Expression for X:\n", X)
print("Expression for J:\n",J)
print("Expression for f0_v_3:\n", f0_v_3)

Finally, designating the values for each of the given parameters and calculating the results for the velocity for the End Effector and the resulting Jacobian Matrix.

# Inserting Values and evaluating expressions
sub = np.array([(l1, 1), (l2, 0.8), (theta1, 10*np.pi/180), (theta2, 25*np.pi/180), (theta1dot, 5), (theta2dot, 10)])
X = X.subs(sub)

J = sp.simplify(J)
J = J.subs(sub)

f0_v_3 = sp.simplify(f0_v_3)
f0_v_3 = f0_v_3.subs(sub)

The resulting Kinematic equation, Jacobian Matrix and the velocity are:

## Results

# Expression for Kinematic Equation
Expression for X:
 [[l1*cos(theta1) + l2*cos(theta1 + theta2)], [l1*sin(theta1) + l2*sin(theta1 + theta2)], [0], [1]]
# Expression for Jacobian MAtrix
Expression for J:
 [[-l1*sin(theta1) - l2*sin(theta1 + theta2) -l2*sin(theta1 + theta2)]
 [l1*cos(theta1) + l2*cos(theta1 + theta2) l2*cos(theta1 + theta2)]]
# Expression for f0_v_3:
 [[-l2*theta2dot*sin(theta1 + theta2) + theta1dot*(-l1*sin(theta1) - l2*sin(theta1 + theta2))]
 [l2*theta2dot*cos(theta1 + theta2) + theta1dot*(l1*cos(theta1) + l2*cos(theta1 + theta2))]]
X:
 [[1.64012938844340], [0.632509326747767], [0], [1]]
J:
 [[-0.632509326747767, -0.458861149080837], [1.64012938844340, 0.655321635431193]]
f0_v_3:
 [[-7.75115812454720], [14.7538632965289]]

Example 3 (Spring '20 - Team 3)

A bus driver is at the end of his route. He imagines his route as a series of movements from a two-link planar manipulator. The driver wants to find what his ending velocity will be at the very end of his route and wants to see how this will change as he varies parameters of his bus's movement. To do so he first sets up the Jacobian matrix of his system.

%reset -f
import numpy as np
import sympy as sp

theta1, theta2 = sp.symbols('theta1 theta2')
l1, l2 = sp.symbols('l1 l2')

f1_f0_T=np.array([[sp.cos(theta1), -sp.sin(theta1), 0, 0],
             [sp.sin(theta1), sp.cos(theta1), 0, 0],
             [0, 0, 1, 0],
             [0, 0, 0, 1]])

f2_f1_T=np.array([[sp.cos(theta2), -sp.sin(theta2), 0, l1],
            [sp.sin(theta2), sp.cos(theta2),0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])

f3_f2_T=np.array([[1, 0, 0, l2],
            [0, 1, 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])

End_Coord=np.transpose(np.array([[0,0,0,1]]))
X= np.dot(f1_f0_T, np.dot(f2_f1_T, np.dot(f3_f2_T, End_Coord)))
X=sp.simplify(X)
print("X:\n",X)

J=np.array([[sp.diff(X[0],theta1), sp.diff(X[0],theta2)],
            [sp.diff(X[1],theta1), sp.diff(X[1],theta2)]])
print("J:\n",J)

theta1dot, theta2dot = sp.symbols('theta1dot theta2dot')
thetadot=np.transpose(np.array([[theta1dot, theta2dot]]))

f0_v_3=np.dot(J, thetadot)
print("f0_v_3:\n",f0_v_3)

X: [[l1*cos(theta1) + l2*cos(theta1 + theta2)], [l1*sin(theta1) + l2*sin(theta1 + theta2)], [0], [1]]

J: [[-l1*sin(theta1) - l2*sin(theta1 + theta2) -l2*sin(theta1 + theta2)]
[l1*cos(theta1) + l2*cos(theta1 + theta2) l2*cos(theta1 + theta2)]]

f0_v_3: [[-l2*theta2dot*sin(theta1 + theta2) + theta1dot*(-l1*sin(theta1) - l2*sin(theta1 + theta2))]
[l2*theta2dot*cos(theta1 + theta2) + theta1dot*(l1*cos(theta1) + l2*cos(theta1 + theta2))]]

He wants to know what will happen we he decides to pick values of $l_{1}$and $l_{2}$,${\dot {\theta _{1}}}$ and ${\dot {\theta _{2}}}$ and hold $\theta _{1}$constant and vary $\theta _{2}$. He starts with $\theta _{1}$as 0 and theta 2 as zero. He varies $\theta _{2}$to the following angles: 0,5,10,15,20,30,60,80,100,120,130,150,170,175,180 and notes his velocity in the x and y direction. He does this same thing when $\theta _{1}$equals 45 degrees, notes the values of his velocities and plots them versus the angle of $\theta _{2}$ that he runs through.

# Here, l1 =1, l2=1, theta1dot=1,  theta2dot=1

replacements = [(l1,1), (l2,1), (theta1,45*np.pi/180),(theta2,180*np.pi/180),(theta1dot, 1),(theta2dot,1)]
J = sp.simplify(J)
f0_v_3 = sp.simplify(f0_v_3)


([[X],[J],[f0_v_3]]) = ([np.array(X.subs(replacements)).astype(np.float64)],
                       [np.array(J.subs(replacements)).astype(np.float64)]
                       [np.array(f0_v_3.subs(replacements)).astype(np.float64)])

print(X)
print(J)
print(f0_v_3)

Jacobian velocities using theta1=0

#Theta1 = 0

%reset -f
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
x = np.array([[0,5,10,15,20,30,60,80,100,120,130,150,170,175,180]])
y = np.array([[0,-0.174311,-0.3472963,-0.51763,-0.684042,-1,-1.73205508,-1.9696155,-1.96915,-1.73205081,-1.5320889,-1,-0.34729636,-0.17431149,2.45E-16]])
z = np.array([[3,2.9923894,2.9696155,2.93185,2.879385,2.7320508,2,1.34729636,0.6527063,4.44E-16,-0.2855752,-0.73205081,-0.9696151,-0.9923894,-1]])

ax.scatter(x,y,z)
ax.set_xlabel('X',size=20)
ax.set_ylabel('Y',size=20)
ax.set_zlabel('Z',size=20)
ax.view_init(50,-115)

Jacobian velocities using theta1=0

#Theta1 = 45


%reset -f
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
x = np.array([[0,5,10,15,20,30,60,80,100,120,130,150,170,175,180]])
y = np.array([[-2.12132034,-2.23919567,-2.34541087,-2.43915759,-2.51972236,-2.6389584,-2.6389584,-2.3451087,-1.8542596,-1.224744,-0.88141827,-0.1894689,0.4404609,0.5786844,0.70710678]])
z = np.array([[2.12132034,1.992682,1.8542596,1.7071068,1.5523433,1.22474487,0.1894686,-0.4404609,-0.9311973,-1.22474487,-1.28528621,-1.2247448,-0.9311973,-0.8249211,-0.70710678]])


ax.scatter(x,y,z)
ax.set_xlabel('X',size=20)
ax.set_ylabel('Y',size=20)
ax.set_zlabel('Z',size=20)
ax.view_init(50,-115)

Example 4 (Spring '20 - Team 4)

Assume a robotic arm with two links each ${\mathsf {1}}\ {\mathsf {m}}$ long is moving with constant angular velocities. The initial angles of each of the two joints is known at ${\bigl (}{\mathsf {15,25}}{\bigr )}\ {\mathsf {degrees}}$ and their angular velocities are known to be ${\bigl (}{\mathsf {30,-50}}{\bigr )}\ {\mathsf {degrees/sec}}$ respectively. What is the maximum speed the end effector experiences assuming a ${\mathsf {10}}\ {\mathsf {second}}$ sample interval?

%reset -f
import numpy as np
import sympy as sp

#declaration of symbols and vectors
theta1, theta2 = sp.symbols('theta1 theta2')
l1, l2 = sp.symbols('l1 l2')

f1_f0_T=np.array([[sp.cos(theta1), -sp.sin(theta1), 0, 0],
             [sp.sin(theta1), sp.cos(theta1), 0, 0],
             [0, 0, 1, 0],
             [0, 0, 0, 1]])

f2_f1_T=np.array([[sp.cos(theta2), -sp.sin(theta2), 0, l1],
            [sp.sin(theta2), sp.cos(theta2),0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])

f3_f2_T=np.array([[1, 0, 0, l2],
            [0, 1, 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])

End_Coord=np.transpose(np.array([[0,0,0,1]]))
X= np.dot(np.dot(np.dot(f1_f0_T,f2_f1_T),f3_f2_T),End_Coord)
X=sp.simplify(X)

#calculate jacobian
J=[[sp.diff(X[0], theta1),sp.diff(X[0], theta2)],
   [sp.diff(X[1], theta1),sp.diff(X[1], theta2)]]

theta1dot, theta2dot = sp.symbols('theta1dot theta2dot')
thetadot=np.transpose(np.array([[theta1dot, theta2dot]]))
#Vel Vector of EE relative to ref frame 0
f0_v_3=np.dot(J, thetadot)

#number of data points (can be changed for higher accuracy)
n = 50
index = np.linspace(0,n-1,n)
index = index.astype(int)#force into integer type for index calls
delT = 10 #seconds, total length of movement
dt = delT/n #time step
time = index*dt#time vector

thetaVec = np.zeros([n,2])#create empty theta vector
thetaVec[1] = [15.0*(np.pi/180),25.0*(np.pi/180)] #radians, initial values for theta
thetaDotVec = np.array([30.0*(np.pi/180),-50.0*(np.pi/180)]) #rad/s, constant
dtheta = dt*thetaDotVec #rad, change of theta per time unit
velVec = np.zeros([n,2])#create empty velocity vector
lVec = [1,1] #meters, length of links
maxindex = -1 #index location of max velocity, -1 returns when error occurs
maxval = 0 #value of maximum velocity
#plugging in known values and creating a function
xvel = f0_v_3[0][0]
xvel = xvel.subs({l1:lVec[0],l2:lVec[1],theta1dot:thetaDotVec[0],theta2dot:thetaDotVec[1]})
#substitute variables for known values
xvel = sp.lambdify([theta1,(theta2)],xvel)
#convert to function that takes in theta values

#do same for y vector
yvel = f0_v_3[1][0]
yvel = yvel.subs({l1:lVec[0],l2:lVec[1],theta1dot:thetaDotVec[0],theta2dot:thetaDotVec[1]})
yvel = sp.lambdify([theta1,(theta2)],yvel)

#for loop for each position during motion
for i in index:
    theta1i = thetaVec[i][0]
    theta2i = thetaVec[i][1]
    #finding velocity
    xveli = xvel(theta1i,(theta2i))
    xveli = float(xveli)
    yveli = yvel(theta1i,(theta2i))
    yveli = float(yveli)
    veltemp = np.array([xveli,yveli])
    magi = np.linalg.norm(veltemp) #find magnitude of vector
    #Check for maximum value
    if magi > maxval:
        maxval = magi
        maxindex = i
    
    #save velocity vector in array
    velVec[i] = veltemp
    if i != n-1:
        #adjust theta vectors
        thetaVec[i+1] = thetaVec[i]+dtheta
        
print(maxval)
print(time[maxindex])

It should be noted that the velocity equations are created by using a Jacobian matrix and the code also saves each velocity vector in the ${\mathsf {velVec}}$ variable. This can be used to plot the end effector velocity with respect to the time vector, ${\mathsf {time}}$. The output is the maximum speed (magnitude of the velocity, ${\mathsf {m/s}}$) and the time at which it occurs at (${\mathsf {s}}$), shown below.

0.8727
3.6

Example (Team 5,Spring 20)

An example for Jocobian Matrix

A robotic arm with link lengths l1 = 5 $in.$, l2 = 3 $in.$, l3 = 1.5 $in.$, initial angles theta1 = 30 degrees, theta2 = 45 degrees, theta3 = 15 degrees and angular velocities theta1dot = 15 degrees/s, theta2dot = 10 degrees/s, theta3dot = 5 degrees/s is feeding someone sitting at a table. Calculate the velocity of the utensil to ensure it is not dangerous to the person.

%reset -f
import numpy as np
import sympy as sp

theta1, theta2, theta3=sp.symbols('theta1 theta2 theta3')
l1, l2, l3=sp.symbols('l1 l2 l3')

f1_f0_T=np.array([[sp.cos(theta1), -sp.sin(theta1), 0, 0],
             [sp.sin(theta1), sp.cos(theta1), 0, 0],
             [0, 0, 1, 0],
             [0, 0, 0, 1]])

f2_f1_T=np.array([[sp.cos(theta2), -sp.sin(theta2), 0, l1],
            [sp.sin(theta2), sp.cos(theta2), 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])

f3_f2_T=np.array([[sp.cos(theta3), -sp.sin(theta3),0, l2],
            [sp.sin(theta3), sp.cos(theta3), 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])

f4_f3_T=np.array([[1, 0, 0, l3],
                 [0, 1, 0, 0],
                 [0, 0, 1, 0],
                 [0, 0, 0, 1]])


End_Coord=np.transpose(np.array([[0,0,0,1]]))
X=np.dot(f1_f0_T,(np.dot(f2_f1_T,(np.dot(f3_f2_T,np.dot(f4_f3_T,End_Coord))))))
X=sp.simplify(X)
print('X:\n',X)

J=np.array([[sp.diff(X[0],theta1),sp.diff(X[0],theta2),sp.diff(X[0],theta3)],
            [sp.diff(X[1],theta1),sp.diff(X[1],theta2),sp.diff(X[1],theta3)]])
print('J:\n',J)

theta1dot, theta2dot, theta3dot=sp.symbols('theta1dot theta2dot theta3dot')
thetadot=np.transpose(np.array([[theta1dot, theta2dot, theta3dot]]))

substitutions=np.array([(theta1,np.pi*30/180), (theta2,np.pi*45/180), (theta3,np.pi*15), (l1,5), (l2,3), (l3,1.5), 
                (theta1dot,np.pi*15/180), (theta2dot,np.pi*10/180), (theta3dot,np.pi*5/180)])

f0_v_4=np.dot(J,thetadot)
f0_v_4=sp.simplify(f0_v_4)
f0_v_4=f0_v_4.subs(substitutions)
print('f0_v_4:\n',f0_v_4)

Output: X:

[[l1*cos(theta1) + l2*cos(theta1 + theta2) + l3*cos(theta1 + theta2 + theta3)], [l1*sin(theta1) + l2*sin(theta1 + theta2) + l3*sin(theta1 + theta2 + theta3)], [0], [1]]

J:

[[-l1*sin(theta1) - l2*sin(theta1 + theta2) - l3*sin(theta1 + theta2 + theta3)
 -l2*sin(theta1 + theta2) - l3*sin(theta1 + theta2 + theta3)
 -l3*sin(theta1 + theta2 + theta3)]
[l1*cos(theta1) + l2*cos(theta1 + theta2) + l3*cos(theta1 + theta2 + theta3)
 l2*cos(theta1 + theta2) + l3*cos(theta1 + theta2 + theta3)
 l3*cos(theta1 + theta2 + theta3)]]

f0_v_4:

[[-1.16025604946161], [1.26914193776358]]

Example 6 (Spring '20 - Team 6)

A scale model of the ISS robotic arm is being designed by Rutgers university students. The model is a three-link manipulator with links 1,2, & 3 having lengths of 6m, 4m & 2m respectively. The angles between the links are 30 degrees, 45 degrees, and -60 degrees and the velocities at the joint are 1 degrees/s , 5 degrees/s and -5 degrees/s. The team is looking to find the velocity of the end-effector through the use of a Jacobian matrix.

%reset -f
import numpy as np
import sympy as sp

theta1, theta2, theta3 = sp.symbols('theta1 theta2 theta3')
l1 = 6 
l2 = 4
l3 = 2

f1_f0_T=np.array([[sp.cos(theta1), -sp.sin(theta1), 0, 0],
             [sp.sin(theta1), sp.cos(theta1), 0, 0],
             [0, 0, 1, 0],
             [0, 0, 0, 1]])

f2_f1_T=np.array([[sp.cos(theta2), -sp.sin(theta2), 0, l1],
            [sp.sin(theta2), sp.cos(theta2),0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])

f3_f2_T=np.array([[sp.cos(theta3), -sp.sin(theta3), 0, l2],
            [sp.sin(theta2), sp.cos(theta2),0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])

f4_f3_T=np.array([[1, 0, 0, l3],
            [0, 1, 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])

End_Coord=np.transpose(np.array([[0,0,0,1]]))
X=np.dot(np.dot(np.dot(np.dot(f1_f0_T,f2_f1_T),f3_f2_T),f4_f3_T),End_Coord) #Enter Code Here
X=sp.simplify(X)
print("X:\n",X)

dx = np.array([sp.diff(X[0],theta1),sp.diff(X[0],theta2),sp.diff(X[0],theta3)])
dy = np.array([sp.diff(X[1],theta1),sp.diff(X[1],theta2),sp.diff(X[1],theta3)])        

dx[0]=dx[0].subs([(theta1,np.pi*30/180),(theta2,np.pi*45/180),(theta3,-np.pi*60/180)])
dx[1]=dx[1].subs([(theta1,np.pi*30/180),(theta2,np.pi*45/180),(theta3,-np.pi*60/180)])
dx[2]=dx[2].subs([(theta1,np.pi*30/180),(theta2,np.pi*45/180),(theta3,-np.pi*60/180)])
dy[0]=dx[0].subs([(theta1,np.pi*30/180),(theta2,np.pi*45/180),(theta3,-np.pi*60/180)])
dy[1]=dx[1].subs([(theta1,np.pi*30/180),(theta2,np.pi*45/180),(theta3,-np.pi*60/180)])
dy[2]=dx[2].subs([(theta1,np.pi*30/180),(theta2,np.pi*45/180),(theta3,-np.pi*60/180)])

dx = np.array([dx[0],dx[1],dx[2]])
dy = np.array([dy[0],dy[1],dy[2]])

#Jacobian Matrix of partial derivatives
J=np.array([dx,dy])
print(J)
#Joint Angular Velocity (radians/sec)
theta1dot=np.pi*1/180
theta2dot=np.pi*5/180
theta3dot=-np.pi*5/180
thetadot=np.transpose(np.array([[theta1dot, theta2dot,theta3dot]]))

#End Effector Velocity respect to origin
f0_v_4=np.dot(J,thetadot) 
print("f0_v_4:\n",f0_v_4)

X:
 [[5*cos(theta1) + 4*cos(theta1 + theta2) + cos(theta1 + 2*theta2) + cos(theta1 + theta2 - theta3) + cos(theta1 + theta2 + theta3)], [5*sin(theta1) + 4*sin(theta1 + theta2) + sin(theta1 + 2*theta2) + sin(theta1 + theta2 - theta3) + sin(theta1 + theta2 + theta3)], [0], [1]]

J:
[[-8.19565453522978 -6.56167993901422 0.448287736084027]
 [-8.19565453522978 -6.56167993901422 0.448287736084027]]

f0_v_4:
 [[-0.754776237939948]
 [-0.754776237939948]]