Robotic Mechanics and Modeling/Kinematics/Additional Examples for Denavit-Hartenberg Parameters
Example 1 (Spring '20 - Team 1) edit
A student is given a homework problem that makes use of the Denavit-Hartenberg Notation. The problem considers two joints where the offset along the previous z-axis to the common normal is 5 cm, the angle about the previous z-axis from the old x-axis to the new x-axis is 60 degrees, the length of the common normal between the two joints is 7 cm and the angle about the common normal, from the old z-axis to the new z-axis is 45 degrees. The student considers the transformation matrix between these joints presented in lecture. He wants to prove that the transformation matrix can be found from a sequence of X and Z translation and rotation matrices.
import numpy as np
d1=5; theta1=60*np.pi/180; a1=7; alpha1=45*np.pi/180
T_manual= np.array([[np.cos(theta1), -np.sin(theta1)*np.cos(alpha1), np.sin(theta1)*np.sin(alpha1), a1*np.cos(theta1)],
[np.sin(theta1), np.cos(theta1)*np.cos(alpha1), -np.cos(theta1)*np.sin(alpha1), a1*np.sin(theta1)],
[0, np.sin(alpha1), np.cos(alpha1), d1],
[0, 0, 0, 1]])
The student begins by manually typing in the entire transformation matrix.
Z_T = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, d1],[0, 0, 0, 1]])
Z_R = np.array([[np.cos(theta1), -np.sin(theta1), 0, 0],
[np.sin(theta1), np.cos(theta1), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
X_T = np.array([[1, 0 ,0, a1],[0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
X_R = np.array([[1, 0, 0, 0],
[0, np.cos(alpha1), -np.sin(alpha1), 0],
[0, np.sin(alpha1), np.cos(alpha1), 0],
[0, 0, 0, 1]])
Z = np.dot(Z_T,Z_R)
X = np.dot(X_T,X_R)
T = np.dot(Z,X)
Next, the student inputs the Z translation and rotation matrices and the X translation and rotation matrices. The Z matrix is found my multiplying the Z rotation and translation matrices together, and the X matrix is found by calculating the X rotation and translation together. The complete transformation matrix is found by multiplying the Z and X matrices together. Note that these multiplications can all happen in the same step and still yield the same T matrix.
print('T matrix')
print(T_manual)
print('T = Z * X matrix')
print(T)
print('Are these matrices the same?')
print(T==T_manual)
The student prints the results from each matrix to observe that they are exactly the same. The code block below shows the results.
T matrix
[[ 0.5 -0.61237244 0.61237244 3.5 ]
[ 0.8660254 0.35355339 -0.35355339 6.06217783]
[ 0. 0.70710678 0.70710678 5. ]
[ 0. 0. 0. 1. ]]
T = Z * X matrix
[[ 0.5 -0.61237244 0.61237244 3.5 ]
[ 0.8660254 0.35355339 -0.35355339 6.06217783]
[ 0. 0.70710678 0.70710678 5. ]
[ 0. 0. 0. 1. ]]
Are these matrices the same?
[[ True True True True]
[ True True True True]
[ True True True True]
[ True True True True]]
Example 2 (Spring '20 - Team 2) edit
Suppose you have a robotic arm composed of 3 links, joints connecting the links, one fixed joint at the base, and a clamp at the end. Assuming all links share the same pose and given the following: the depth of the previous joint’s z direction to the new common normal as 2 , rotation about the previous joint’s z-axis up to the new common normal as , length of the common normal as 2 , and angle orientation of the new z-axis as , find out where the end effector is. Assume the fixed base to be the origin (0,0,0). Plot your results.
Start with defining the Denavit-Hartenberg Matrix :
import numpy as np
import matplotlib.pyplot as plt
d=2
theta=10*np.pi/180;
a=2
alpha=10*np.pi/180
#Define Denavit-Hartenberg Matrix
T= np.array([[np.cos(theta), -np.sin(theta)*np.cos(alpha), np.sin(theta)*np.sin(alpha), a*np.cos(theta)],
[np.sin(theta), np.cos(theta)*np.cos(alpha), -np.cos(theta)*np.sin(alpha), a*np.sin(theta)],
[0, np.sin(alpha), np.cos(alpha), d],
[0, 0, 0, 1]])
Assign the initial conditions to the variables and compute the location of the joints.
#Define starting Coordinate
End_Coord=np.array([[0,0,0,1]])
End_Coord=End_Coord.T
#Compute position of all joints
J1=End_Coord
J2=np.dot(T,End_Coord)
J3=np.dot(T, np.dot(T,End_Coord))
J4=np.dot(T,np.dot(T, np.dot(T,End_Coord)))
print("The end effector is at location:")
print(J4)
Displaying the location coordinates and plotting the results.
The end effector is at location is:
[1.00063397] [6.08749557] [1. ]]
#Plot
fig=plt.figure()
ax=fig.add_subplot(111)
plt.plot(J1[0], J1[1], 'bo')
plt.plot(J2[0], J2[1], 'ro')
plt.plot(J3[0], J3[1], 'go')
plt.plot(J4[0], J4[1], 'ko')
plt.plot([J1[0], J2[0]],[J1[1], J2[1]], 'b-')
plt.plot([J2[0], J3[0]],[J2[1], J3[1]], 'r-')
plt.plot([J3[0], J4[0]],[J3[1], J4[1]], 'g-')
ax.set_xlim([0, 1])
plt.axis('equal')
ax.set_xlabel('X',size=20)
ax.set_ylabel('Y',size=20);
Example 3 (Spring '20 - Team 3) edit
A bus driver wants to imagine his stops as joints in a mechanism and the straight line paths that he follows on his route between them as the links in that mechanism. He starts from a local origin. The driver uses conventional Denavit Hartengerg parameters where d is the offset along the previous z axis to the common normal, theta is the angle about the previous z axis, from the old one to the new one, a is the length of the common normal (if assuming that this is a revolute joint, this is the radius about the previous z axis), and alpha is the angle about the common normal from the old z axis to the new z axis.
The bus drive has five different stops on his route. The values of d are 0.1, 0.3, 0.1, 0.12, 0.1 in miles. This vertical motion from the factor d is assumed to be easily reached by the driver. The values of a are 1, 2, 3.25, 1, 4 in miles. The values of alpha are 25, 0, 152, 0, 0 in degrees. The values of theta are 45, 0, 26, 256, 45 in degrees.
%reset -f
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np
#Link 1
d1=0.1; theta1=45*np.pi/180; a1=1; alpha1=25*np.pi/180
#Link 2
d2=0.3; theta2=0*np.pi/180; a2=2; alpha2=0*np.pi/180
#Link 3
d3=0.1; theta3=26*np.pi/180; a3=3.25; alpha3=152*np.pi/180
#Link4
d4=0.12; theta4=256*np.pi/180; a4=1; alpha4=0*np.pi/180
#Link5
d5=0.1; theta5=45*np.pi/180; a5=1; alpha5=0*np.pi/180
Link2_1_T=np.array([[np.cos(theta1), -np.sin(theta1)*np.cos(alpha1), np.sin(theta1)*np.sin(alpha1), a1*np.cos(theta1)],
[np.sin(theta1), np.cos(theta1)*np.cos(alpha1), -np.cos(theta1)*np.sin(alpha1), a1*np.sin(theta1)],
[0, np.sin(alpha1), np.cos(alpha1), d1],
[0, 0, 0, 1]])
Link3_2_T=np.array([[np.cos(theta2), -np.sin(theta2)*np.cos(alpha2), np.sin(theta2)*np.sin(alpha2), a2*np.cos(theta2)],
[np.sin(theta2), np.cos(theta2)*np.cos(alpha2), -np.cos(theta2)*np.sin(alpha2), a2*np.sin(theta2)],
[0, np.sin(alpha2), np.cos(alpha2), d2],
[0, 0, 0, 1]])
Link4_3_T=np.array([[np.cos(theta3), -np.sin(theta3)*np.cos(alpha3), np.sin(theta3)*np.sin(alpha3), a3*np.cos(theta3)],
[np.sin(theta3), np.cos(theta3)*np.cos(alpha3), -np.cos(theta3)*np.sin(alpha3), a3*np.sin(theta3)],
[0, np.sin(alpha3), np.cos(alpha3), d3],
[0, 0, 0, 1]])
Link5_4_T=np.array([[np.cos(theta4), -np.sin(theta4)*np.cos(alpha4), np.sin(theta4)*np.sin(alpha4), a4*np.cos(theta4)],
[np.sin(theta4), np.cos(theta4)*np.cos(alpha4), -np.cos(theta4)*np.sin(alpha4), a4*np.sin(theta4)],
[0, np.sin(alpha4), np.cos(alpha4), d4],
[0, 0, 0, 1]])
Link6_5_T=np.array([[np.cos(theta5), -np.sin(theta5)*np.cos(alpha5), np.sin(theta5)*np.sin(alpha5), a5*np.cos(theta5)],
[np.sin(theta5), np.cos(theta5)*np.cos(alpha5), -np.cos(theta5)*np.sin(alpha5), a5*np.sin(theta5)],
[0, np.sin(alpha5), np.cos(alpha5), d5],
[0, 0, 0, 1]])
End_Coord=np.array([[0,0,0,1]])
End_Coord=End_Coord.T
Joint1Link0=End_Coord
Joint2Link1=np.dot(Link2_1_T,End_Coord)
Joint3Link2=np.dot(Link2_1_T, np.dot(Link3_2_T,End_Coord))
Joint4Link3=np.dot(Link2_1_T, np.dot(Link3_2_T, np.dot(Link4_3_T, End_Coord)))
Joint5Link4=np.dot(Link2_1_T, np.dot(Link3_2_T, np.dot(Link4_3_T, np.dot(Link5_4_T, End_Coord))))
Joint6Link5=np.dot(Link2_1_T, np.dot(Link3_2_T, np.dot(Link4_3_T, np.dot(Link5_4_T, np.dot(Link6_5_T, End_Coord)))))
x = [Joint1Link0[0], Joint2Link1[0], Joint3Link2[0], Joint4Link3[0], Joint5Link4[0], Joint6Link5[0]]
y = [Joint1Link0[1], Joint2Link1[1], Joint3Link2[1], Joint4Link3[1], Joint5Link4[1], Joint6Link5[1]]
z = [Joint1Link0[2], Joint2Link1[2], Joint3Link2[2], Joint4Link3[2], Joint5Link4[2], Joint6Link5[2]]
fig=plt.figure()
ax=fig.add_subplot(111)
plt.plot(x[0], y[0], 'bo')
plt.plot(x[1], y[1], 'ro')
plt.plot(x[2], y[2], 'go')
plt.plot(x[3], y[3], 'ko')
plt.plot(x[4], y[4], 'yo')
plt.plot(x[5], y[5], 'yo')
plt.plot([x[0], x[1]],[y[0], y[1]], 'b-')
plt.plot([x[1], x[2]],[y[1], y[2]], 'r-')
plt.plot([x[2], x[3]],[y[2], y[3]], 'g-')
plt.plot([x[3], x[4]],[y[3], y[4]], 'k-')
plt.plot([x[4], x[5]],[y[4], y[5]], 'y-')
ax.set_xlim([0, 1])
plt.axis('equal')
ax.set_xlabel('X',size=20)
ax.set_ylabel('Y',size=20)
ax.set_title('Top-Down Projection',size=20)
fig=plt.figure()
ax=fig.add_subplot(111)
plt.plot(x[0], z[0], 'bo')
plt.plot(x[1], z[1], 'ro')
plt.plot(x[2], z[2], 'go')
plt.plot(x[3], z[3], 'ko')
plt.plot(x[4], z[4], 'yo')
plt.plot(x[5], z[5], 'yo')
plt.plot([x[0], x[1]],[z[0], z[1]], 'b-')
plt.plot([x[1], x[2]],[z[1], z[2]], 'r-')
plt.plot([x[2], x[3]],[z[2], z[3]], 'g-')
plt.plot([x[3], x[4]],[z[3], z[4]], 'k-')
plt.plot([x[4], x[5]],[z[4], z[5]], 'y-')
ax.set_xlim([0, 1])
plt.axis('equal')
ax.set_xlabel('X',size=20)
ax.set_ylabel('Z',size=20)
ax.set_title('Side 1 Projection',size=20)
fig=plt.figure()
ax=fig.add_subplot(111)
plt.plot(y[0], z[0], 'bo')
plt.plot(y[1], z[1], 'ro')
plt.plot(y[2], z[2], 'go')
plt.plot(y[3], z[3], 'ko')
plt.plot(y[4], z[4], 'yo')
plt.plot(y[5], z[5], 'yo')
plt.plot([y[0], y[1]],[z[0], z[1]], 'b-')
plt.plot([y[1], y[2]],[z[1], z[2]], 'r-')
plt.plot([y[2], y[3]],[z[2], z[3]], 'g-')
plt.plot([y[3], y[4]],[z[3], z[4]], 'k-')
plt.plot([y[4], y[5]],[z[4], z[5]], 'y-')
ax.set_xlim([0, 1])
plt.axis('equal')
ax.set_xlabel('Y',size=20)
ax.set_ylabel('Z',size=20)
ax.set_title('Side 2 Projection',size=20)
fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.plot3D(x[0], y[0], z[0], 'bo')
ax.plot3D(x[1], y[1], z[1], 'ro')
ax.plot3D(x[2], y[2], z[2], 'go')
ax.plot3D(x[3], y[3], z[3], 'ko')
ax.plot3D(x[4], y[4], z[4], 'yo')
ax.plot3D(x[5], y[5], z[5], 'yo')
x1,y1,z1 = np.array([x[0], x[1]]),np.array([y[0], y[1]]),np.array([z[0], z[1]])
x2,y2,z2 = np.array([x[1], x[2]]),np.array([y[1], y[2]]),np.array([z[1], z[2]])
x3,y3,z3 = np.array([x[2], x[3]]),np.array([y[2], y[3]]),np.array([z[2], z[3]])
x4,y4,z4 = np.array([x[3], x[4]]),np.array([y[3], y[4]]),np.array([z[3], z[4]])
x5,y5,z5 = np.array([x[4], x[5]]),np.array([y[4], y[5]]),np.array([z[4], z[5]])
ax.plot_wireframe(x1, y1, z1, color = 'b')
ax.plot_wireframe(x2, y2, z2, color = 'r')
ax.plot_wireframe(x3, y3, z3, color = 'g')
ax.plot_wireframe(x4, y4, z4, color = 'k')
ax.plot_wireframe(x5, y5, z5, color = 'y')
ax.text(Joint1Link0[0][0]+0.1, Joint1Link0[1][0], Joint1Link0[2][0],
'J1L0',None,size=20)
ax.text(Joint2Link1[0][0]+0.1, Joint2Link1[1][0], Joint2Link1[2][0],
'J2L1',None,size=20)
ax.text(Joint3Link2[0][0]+0.1, Joint3Link2[1][0], Joint3Link2[2][0],
'J3L2',None,size=20)
ax.text(Joint4Link3[0][0]+0.1, Joint4Link3[1][0], Joint4Link3[2][0],
'J4L3',None,size=20)
ax.text(Joint5Link4[0][0]+0.1, Joint5Link4[1][0], Joint5Link4[2][0],
'J5L4',None,size=20)
ax.text(Joint6Link5[0][0]+0.1, Joint6Link5[1][0], Joint6Link5[2][0],
'J6L5',None,size=20)
ax.set_xlabel('X',size=20)
ax.set_ylabel('Y',size=20)
ax.set_zlabel('Z',size=20)
ax.view_init(30,-115)
We can also calculate the distance between the first and last bus stop.
d_x = (x[5]-x[1])**2
d_y = (y[5]-y[1])**2
d_z = (z[5]-z[1])**2
d = np.sqrt(d_x + d_y + d_z)
print("The distance between the first and last bus stop is:")
print(d)
The distance between the first and last bus stop is 7.68 miles.
</gallery>
Example 4 (Spring '20 - Team 4) edit
You are a worker in a factory that uses robotic arms to move boxes from one conveyor belt to another that is located on the same elevation. During a quality control check, you have reason to believe that the boxes are taken off the first conveyor belt and not put in the correct second conveyor belt. Given the angles and distances between each link, verify that the boxes are being put in the right location
from numpy import cos, sin, pi, array, transpose, dot,round
import matplotlib.pyplot as plt
#Link 1
d1=0; theta1=20*pi/180; a1=1; alpha1=0*pi/180
#Link 2
d2=0; theta2=15*pi/180; a2=1; alpha2=0*pi/180
#Link 3
d3=0; theta3=45*pi/180; a3=1; alpha3=0*pi/180
link2_1_T= array([
[cos(theta1), -sin(theta1)*cos(alpha1), sin(theta1)*sin(alpha1), a1*cos(theta1)],
[sin(theta1), cos(theta1)*cos(alpha1) , -cos(theta1)*sin(alpha1), a1*sin(theta1)],
[0 ,sin(alpha1), cos(alpha1), d1],
[0 , 0, 0, 1]])
link3_2_T=array([
[cos(theta2), -sin(theta2)*cos(alpha2), sin(theta2)*sin(alpha2), a2*cos(theta2)],
[sin(theta2), cos(theta2)*cos(alpha2) , -cos(theta2)*sin(alpha2), a2*sin(theta2)],
[0 ,sin(alpha2), cos(alpha2), d2],
[0 , 0, 0, 1]])
link4_3_T=array([
[cos(theta3), -sin(theta3)*cos(alpha3), sin(theta3)*sin(alpha3), a3*cos(theta3)],
[sin(theta3), cos(theta3)*cos(alpha3) , -cos(theta3)*sin(alpha3), a3*sin(theta3)],
[0 , sin(alpha3) , cos(alpha3) , d3],
[0 , 0, 0, 1]])
end_Coord=transpose(array([[0,0,0,1]]))
joint1Link0=end_Coord
joint2Link1=dot(link2_1_T,joint1Link0)
joint3Link2=dot(link2_1_T,dot(link3_2_T,joint1Link0))
joint4Link3=dot(link2_1_T,dot(link3_2_T,dot(link4_3_T,joint1Link0)))
fig = plt.figure()
ax = fig.add_subplot(111)
x_ar = [joint1Link0[0][0],joint2Link1[0][0],joint3Link2[0][0],joint4Link3[0][0]]
y_ar = [joint1Link0[1][0],joint2Link1[1][0],joint3Link2[1][0],joint4Link3[1][0]]
plt.plot(x_ar,y_ar,'-o')
plt.grid()
ax.set_xlim([0, 1])
plt.axis('equal')
ax.set_xlabel('X',size=20)
ax.set_ylabel('Y',size=20)
display(round(joint4Link3,2)) #final position relative to initial position
This displays the expected answer, where the third entry is the height (each link is modeled as 0.1 meters high) and the fourth is to match it with the DH notation.
array([[1.93],
[1.9 ],
[0. ],
[1. ]])
Example (Team 5,Spring 20) edit
edit
%reset -f
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
# length and rotation of joints
d1=0; theta1=45*np.pi/180; a1=1; alpha1=0
d2=0; theta2=0*np.pi/180; a2=1; alpha2=0
d3=0; theta3=45*np.pi/180; a3=1; alpha3=0
d4=0; theta4=0*np.pi/180; a4=1; alpha4=0
# Modified DH matrices for each joint
J2J1T = np.array([[np.cos(theta1), -np.sin(theta1), 0, a1],
[np.sin(theta1)*np.cos(alpha1), np.cos(theta1)*np.cos(alpha1), -np.sin(alpha1), -d1*np.sin(alpha1)],
[np.sin(theta1)*np.sin(alpha1), np.cos(theta1)*np.sin(alpha1), np.cos(alpha1), d1*np.cos(alpha1)],
[0, 0, 0, 1]])
J3J2T = np.array([[np.cos(theta2), -np.sin(theta2), 0, a2],
[np.sin(theta2)*np.cos(alpha2), np.cos(theta2)*np.cos(alpha2), -np.sin(alpha2), -d2*np.sin(alpha2)],
[np.sin(theta2)*np.sin(alpha2), np.cos(theta2)*np.sin(alpha2), np.cos(alpha2), d2*np.cos(alpha2)],
[0, 0, 0, 1]])
J4J3T = np.array([[np.cos(theta3), -np.sin(theta3), 0, a3],
[np.sin(theta3)*np.cos(alpha3), np.cos(theta3)*np.cos(alpha3), -np.sin(alpha3), -d3*np.sin(alpha3)],
[np.sin(theta3)*np.sin(alpha3), np.cos(theta3)*np.sin(alpha3), np.cos(alpha3), d3*np.cos(alpha3)],
[0, 0, 0, 1]])
J5J4T = np.array([[np.cos(theta4), -np.sin(theta4), 0, a4],
[np.sin(theta4)*np.cos(alpha4), np.cos(theta4)*np.cos(alpha4), -np.sin(alpha4), -d3*np.sin(alpha4)],
[np.sin(theta4)*np.sin(alpha4), np.cos(theta4)*np.sin(alpha4), np.cos(alpha4), d3*np.cos(alpha4)],
[0, 0, 0, 1]])
# building the joints from each previous joint and the Modified DH matrices
Joint1 = np.array([[0, 0, 0, 1]]).T
Joint2 = np.dot(J2J1T,Joint1)
Joint3 = np.dot(np.dot(J2J1T,J3J2T),Joint1)
Joint4 = np.dot(np.dot(np.dot(J2J1T,J3J2T),J4J3T),Joint1)
Joint5 = np.dot(np.dot(np.dot(np.dot(J2J1T,J3J2T),J4J3T),J5J4T),Joint1)
fig=plt.figure()
ax=fig.add_subplot(111)
# plotting the joints
ax.plot([Joint1[0][0],Joint2[0][0]],[Joint1[1][0],Joint2[1][0]],'bo-')
ax.plot([Joint2[0][0],Joint3[0][0]],[Joint2[1][0],Joint3[1][0]],'ro-')
ax.plot([Joint3[0][0],Joint4[0][0]],[Joint3[1][0],Joint4[1][0]],'go-')
ax.plot([Joint4[0][0],Joint5[0][0]],[Joint4[1][0],Joint5[1][0]],'yo-')
ax.set_xlim([0, 1])
plt.axis('equal')
ax.set_xlabel('X',size=20)
ax.set_ylabel('Y',size=20)
ax.set_title('Top-Down Projection',size=20)
Example 6 (Spring '20 - Team 6) edit
The Canadarm aboard the ISS needs to extend and rotate itself in order to dock with the new cargo capsule, built and developed by the Rutgers Rocket Propulsion Lab. The Canadarm needs to rotate 30 degrees about the x-axis and then translate 3 meters about the x-direction, then it needs to rotate -45 degrees and translate 2 meters along the y-axis. It then will rotate the second link 45 degrees about the y-axis while translating 1 meter, followed by a 25 degree rotation about the x-axis and a 3 meter translation. The 3rd link has to rotate -20 degrees about the x, translate 2 meters, then rotate -30 degrees about the y while translating 3 meters. The final link needs to rotate 60 degrees about the x, translate 2 meters, then rotate another 60 degrees but about the y, and translate 3 meters. This will result in the Canadarms docking clamp to be positioned perfectly with the spacecrafts docking mechanism.
First Link: 30 Rotation about x-axis with 3 m translation ---> -45 Rotation about y-axis with 2 m translation Second Link: 45 Rotation about y-axis with 1 m translation ---> 25 Rotation about x-axis with 3 m translation Third Link: -20 Rotation about x-axis with 2 m translation ---> -30 Rotation about y-axis with 3 m translation Fourth Link: 60 Rotation about x-axis with 2 m translation ---> 60 Rotation about y-axis with 3 meter translation
%reset -f
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np
# Link1 (theta is rotation angle with x axis, a is translation distance along x axis, Beta is rotation angle with y axis, and y is translation distance along y axis)
a1=3;theta1=30*np.pi/180; y1=2;Beta1=-45*np.pi/180;
# Link2
a2=3;theta2=25*np.pi/180; y2=1;Beta2=45*np.pi/180;
# Link3
a3=2;theta3=-20*np.pi/180; y3=3;Beta3=-30*np.pi/180;
# Link4
a4=2;theta4=60*np.pi/180; y4=3;Beta4=60*np.pi/180;
# D-H Matrices
Link2_1_T=np.array([[np.cos(Beta1), 0, np.sin(Beta1), a1],
[np.sin(Beta1)*np.sin(theta1), np.cos(theta1), -np.sin(theta1)*np.cos(Beta1), y1*np.cos(theta1)],
[-np.sin(Beta1)*np.cos(theta1), np.sin(theta1), np.cos(Beta1)*np.cos(theta1), y1*np.sin(theta1)],
[0, 0, 0, 1]])
Link3_2_T=np.array([[np.cos(Beta2), 0, np.sin(Beta2), a2],
[np.sin(Beta2)*np.sin(theta2), np.cos(theta2), -np.sin(theta2)*np.cos(Beta2), y2*np.cos(theta2)],
[-np.sin(Beta2)*np.cos(theta2), np.sin(theta2), np.cos(Beta2)*np.cos(theta2), y2*np.sin(theta2)],
[0, 0, 0, 1]])
Link4_3_T=np.array([[np.cos(Beta3), 0, np.sin(Beta3), a3],
[np.sin(Beta3)*np.sin(theta3), np.cos(theta3), -np.sin(theta3)*np.cos(Beta3), y3*np.cos(theta3)],
[-np.sin(Beta3)*np.cos(theta3), np.sin(theta3), np.cos(Beta3)*np.cos(theta3), y3*np.sin(theta3)],
[0, 0, 0, 1]])
Link5_4_T=np.array([[np.cos(Beta4), 0, np.sin(Beta4), a4],
[np.sin(Beta4)*np.sin(theta4), np.cos(theta4), -np.sin(theta4)*np.cos(Beta4), y4*np.cos(theta4)],
[-np.sin(Beta4)*np.cos(theta4), np.sin(theta4), np.cos(Beta4)*np.cos(theta4), y4*np.sin(theta4)],
[0, 0, 0, 1]])
End_Coord=np.array([[0,0,0,1]])
End_Coord=End_Coord.T
# Coordinates of each joint
Joint1Link0=End_Coord
Joint2Link1=np.dot(Link2_1_T,Joint1Link0)
Joint3Link2=np.dot(np.dot(Link2_1_T,Link3_2_T),Joint1Link0)
Joint4Link3=np.dot(np.dot(np.dot(Link2_1_T,Link3_2_T),Link4_3_T),Joint1Link0)
Joint5Link4=np.dot(np.dot(np.dot(np.dot(Link2_1_T,Link3_2_T),Link4_3_T),Link5_4_T),Joint1Link0)
fig=plt.figure()
ax=fig.add_subplot(111)
# plot of Links in Top-Down view
plt.plot([Joint1Link0[0],Joint2Link1[0]],[Joint1Link0[1],Joint2Link1[1]],'bo-')
plt.plot([Joint2Link1[0],Joint3Link2[0]],[Joint2Link1[1],Joint3Link2[1]],'ro-')
plt.plot([Joint3Link2[0],Joint4Link3[0]],[Joint3Link2[1],Joint4Link3[1]],'go-')
plt.plot([Joint4Link3[0],Joint5Link4[0]],[Joint4Link3[1],Joint5Link4[1]],'yo-')
plt.plot()
ax.set_xlim([0, 1])
plt.axis('equal')
ax.set_xlabel('X',size=20)
ax.set_ylabel('Y',size=20)
ax.set_title('Top-Down Projection',size=20)
fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
# Plot of links in 3D view
ax.plot([Joint1Link0[0][0],Joint2Link1[0][0]],[Joint1Link0[1][0],Joint2Link1[1][0]],[Joint1Link0[2][0],Joint2Link1[2][0]],'bo-')
ax.plot([Joint2Link1[0][0],Joint3Link2[0][0]],[Joint2Link1[1][0],Joint3Link2[1][0]],[Joint2Link1[2][0],Joint3Link2[2][0]],'ro-')
ax.plot([Joint3Link2[0][0],Joint4Link3[0][0]],[Joint3Link2[1][0],Joint4Link3[1][0]],[Joint3Link2[2][0],Joint4Link3[2][0]],'go-')
ax.plot([Joint4Link3[0][0],Joint5Link4[0][0]],[Joint4Link3[1][0],Joint5Link4[1][0]],[Joint4Link3[2][0],Joint5Link4[2][0]],'yo-')
ax.set_xlabel('X',size=20)
ax.set_ylabel('Y',size=20)
ax.set_zlabel('Z',size=20)
ax.view_init(45,-145)