def calcORGs(q1, q2, q3, q4, q5, q6): th1 = dR.conv2Rad(q1) th2 = dR.conv2Rad(q2) th3 = dR.conv2Rad(q3) th4 = dR.conv2Rad(q4) th5 = dR.conv2Rad(q5) th6 = dR.conv2Rad(q6) Trans_0to1 = RotZ(th1) Trans_1to2 = np.dot(RotX(dR.conv2Rad(-90)), RotZ(th2)) Trans_2to3 = D_q(a2,0,0) Trans_3to4 = np.dot(D_q(0,0,d3), RotZ(th3)) Trans_4to5 = np.dot(RotX(dR.conv2Rad(-90)), D_q(a3,0,0)) Trans_5to6 = np.dot(D_q(0,0,d4), RotZ(th4)) Trans_6to7 = np.dot(RotX(dR.conv2Rad(90)), RotZ(th5)) Trans_7to8 = np.dot(RotX(dR.conv2Rad(-90)), RotZ(th6)) Trans_0to2 = np.dot(Trans_0to1, Trans_1to2) Trans_0to3 = np.dot(Trans_0to2, Trans_2to3) Trans_0to4 = np.dot(Trans_0to3, Trans_3to4) Trans_0to5 = np.dot(Trans_0to4, Trans_4to5) Trans_0to6 = np.dot(Trans_0to5, Trans_5to6) Trans_0to7 = np.dot(Trans_0to6, Trans_6to7) Trans_0to8 = np.dot(Trans_0to7, Trans_7to8) ORG_1 = np.dot(Trans_0to1, ORG_0) ORG_2 = np.dot(Trans_0to2, ORG_0) ORG_3 = np.dot(Trans_0to3, ORG_0) ORG_4 = np.dot(Trans_0to4, ORG_0) ORG_5 = np.dot(Trans_0to5, ORG_0) ORG_6 = np.dot(Trans_0to6, ORG_0) ORG_7 = np.dot(Trans_0to7, ORG_0) ORG_8 = np.dot(Trans_0to8, ORG_0) return ORG_1, ORG_2, ORG_3, ORG_4, ORG_5, ORG_6, ORG_7, ORG_8
def calcORGs(a1, a2, a3): th1 = dR.conv2Rad(a1) th2 = dR.conv2Rad(a2) th3 = dR.conv2Rad(a3) Trans_0to1 = np.r_[np.c_[dR.RotZ(th1), np.zeros(3)], [[0,0,0,1]]] Trans_1to2 = np.dot(dR.D_q(L1,0,0), np.r_[np.c_[dR.RotZ(th2), np.zeros(3)], [[0,0,0,1]]]) Trans_2to3 = np.dot(dR.D_q(L2,0,0), np.r_[np.c_[dR.RotZ(th3), np.zeros(3)], [[0,0,0,1]]]) Trans_0to2 = np.dot(Trans_0to1, Trans_1to2) Trans_0to3 = np.dot(Trans_0to2, Trans_2to3) ORG_1 = np.dot(Trans_0to1, ORG_0) ORG_2 = np.dot(Trans_0to2, ORG_0) ORG_3 = np.dot(Trans_0to3, ORG_0) return ORG_1, ORG_2, ORG_3
def calcBCoordi(rotAngle, ORG, hat_X, hat_Y, hat_Z): Rot_AtoB = dR.RotZ(dR.conv2Rad(rotAngle)) P_atA = np.dot(Rot_AtoB, P_atB) BORG = np.dot(Rot_AtoB, AORG) hat_X_atB = np.dot(Rot_AtoB, hat_X) hat_Y_atB = np.dot(Rot_AtoB, hat_Y) hat_Z_atB = np.dot(Rot_AtoB, hat_Z) return BORG, hat_X_atB, hat_Y_atB, hat_Z_atB, P_atA
def calcBCoordi(rotAngle, ORG, hat_X, hat_Y, hat_Z):
Rot_AtoB = dR.RotZ(dR.conv2Rad(rotAngle))
P_atA = np.dot(Rot_AtoB, P_atB)
BORG = np.dot(Rot_AtoB, AORG)
hat_X_atB = np.dot(Rot_AtoB, hat_X)
hat_Y_atB = np.dot(Rot_AtoB, hat_Y)
hat_Z_atB = np.dot(Rot_AtoB, hat_Z)
return BORG, hat_X_atB, hat_Y_atB, hat_Z_atB, P_atA
def calcORGs(a1, a2, a3): th1 = dR.conv2Rad(a1) d2 = a2 th3 = dR.conv2Rad(a3) Trans_0to1 = dR.D_q(0,0,L1) Trans_1to2 = np.r_[np.c_[dR.RotZ(th1), np.zeros(3)], [[0,0,0,1]]] Trans_2to3 = np.dot( np.r_[np.c_[dR.RotX(dR.conv2Rad(90)), np.zeros(3)], [[0,0,0,1]]], dR.D_q(0,0,d2) ) Trans_3to4 = np.dot( dR.D_q(0,0,L2), np.r_[np.c_[dR.RotZ(th3), np.zeros(3)], [[0,0,0,1]]] ) Trans_0to2 = np.dot(Trans_0to1, Trans_1to2) Trans_0to3 = np.dot(Trans_0to2, Trans_2to3) Trans_0to4 = np.dot(Trans_0to3, Trans_3to4) ORG_1 = np.dot(Trans_0to1, ORG_0) ORG_2 = np.dot(Trans_0to2, ORG_0) ORG_3 = np.dot(Trans_0to3, ORG_0) ORG_4 = np.dot(Trans_0to4, ORG_0) return ORG_1, ORG_2, ORG_3, ORG_4
def calcORGs(a1, a2, a3):
th1 = dR.conv2Rad(a1)
th2 = dR.conv2Rad(a2)
th3 = dR.conv2Rad(a3)
Trans_0to1 = np.r_[np.c_[dR.RotZ(th1), np.zeros(3)], [[0, 0, 0, 1]]]
Trans_1to2 = np.dot(
dR.D_q(L1, 0, 0), np.r_[np.c_[dR.RotZ(th2), np.zeros(3)],
[[0, 0, 0, 1]]])
Trans_2to3 = np.dot(
dR.D_q(L2, 0, 0), np.r_[np.c_[dR.RotZ(th3), np.zeros(3)],
[[0, 0, 0, 1]]])
Trans_0to2 = np.dot(Trans_0to1, Trans_1to2)
Trans_0to3 = np.dot(Trans_0to2, Trans_2to3)
ORG_1 = np.dot(Trans_0to1, ORG_0)
ORG_2 = np.dot(Trans_0to2, ORG_0)
ORG_3 = np.dot(Trans_0to3, ORG_0)
return ORG_1, ORG_2, ORG_3
# Introduction to Robotics 3rd Edition by Craig
# Example 2-4.
# P1_atA. We wish rotate it about hat_Z by 30 degrees and
# translate it 10 units in hat_X_A, and 5 units in hat_Y_A.
# Find P2_atA, where P1_atA = [3, 7, 0].
import matplotlib.pyplot as plt
import drawRobotics as dR
import numpy as np
P1_atA = np.array([3, 7, 0, 1])
Dq = np.array([10,5,0])
operatorT = np.r_[np.c_[dR.RotZ(dR.conv2Rad(30)), Dq], [np.array([0,0,0,1])] ]
P2_atA = np.dot(operatorT, P1_atA)
AORG = np.array([0,0,0])
hat_X_A, hat_Y_A, hat_Z_A = np.array([1,0,0]), np.array([0,1,0]), np.array([0,0,1])
print('P1_atA = ', P1_atA)
print('Dq = ', Dq)
print('operatorT = ', operatorT)
print('P2_atA = ', P2_atA)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
dR.drawPointWithAxis(ax, AORG, hat_X_A, hat_Y_A, hat_Z_A, pointEnable=False, vectorLength=3)
# Introduction to Robotics 3rd Edition by Craig
# Example 2-3.
# There is a vector P1_atA.
# We wish to compute the vector obtained by rotating this vector about hat_Z by 30 degrees.
# Call the new vector P2_atA.
import matplotlib.pyplot as plt
import drawRobotics as dR
import numpy as np
P1_atA = np.array([0, 2, 0])
AORG = np.array([0, 0, 0])
hat_X_A, hat_Y_A, hat_Z_A = np.array([1,0,0]), np.array([0,1,0]), np.array([0,0,1])
RotZ = dR.RotZ(dR.conv2Rad(30))
P2_atA = np.dot(RotZ, P1_atA)
print('P1_atA = ', P1_atA)
print('RotZ = ', RotZ)
print('P2_atA = ', P2_atA)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
dR.drawPointWithAxis(ax, AORG, hat_X_A, hat_Y_A, hat_Z_A, pointEnable=False)
dR.drawVector(ax, AORG, P2_atA, arrowstyle='-|>', proj=False, annotationString=' $ ^{A}P_{2} $ ')
dR.drawVector(ax, AORG, P1_atA, arrowstyle='-|>', lineColor='c', proj=False, lineStyle='--',
annotationString=' $ ^{A}P_{1} $ ')
# Introduction to Robotics 3rd Edition by Craig
# Example 2-1.
# Figure 2.6 shows a frame {B} that is rotated relative to frame {A} about Z by 30 degrees.
import matplotlib.pyplot as plt
import drawRobotics as dR
import numpy as np
P_atB = np.array([0,2,0])
Rot_AtoB = dR.RotZ(dR.conv2Rad(30))
P_atA = np.dot(Rot_AtoB, P_atB)
print('P_atB = ', P_atB)
print('Rot_AtoB = ', Rot_AtoB)
print('P_atA = Rot_AtoB * P_atA = ', P_atA)
AORG = np.array([0,0,0])
hat_X_atA = np.array([1,0,0])
hat_Y_atA = np.array([0,1,0])
hat_Z_atA = np.array([0,0,1])
BORG = np.dot(Rot_AtoB, AORG)
hat_X_atB = np.dot(Rot_AtoB, hat_X_atA)
hat_Y_atB = np.dot(Rot_AtoB, hat_Y_atA)
hat_Z_atB = np.dot(Rot_AtoB, hat_Z_atA)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# There is a vector P1_atA.
# We wish to compute the vector obtained by rotating this vector about hat_Z by 30 degrees.
# Call the new vector P2_atA.
import matplotlib.pyplot as plt
import drawRobotics as dR
import numpy as np
P1_atA = np.array([0, 2, 0])
AORG = np.array([0, 0, 0])
hat_X_A, hat_Y_A, hat_Z_A = np.array([1, 0,
0]), np.array([0, 1,
0]), np.array([0, 0, 1])
RotZ = dR.RotZ(dR.conv2Rad(30))
P2_atA = np.dot(RotZ, P1_atA)
print('P1_atA = ', P1_atA)
print('RotZ = ', RotZ)
print('P2_atA = ', P2_atA)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
dR.drawPointWithAxis(ax, AORG, hat_X_A, hat_Y_A, hat_Z_A, pointEnable=False)
dR.drawVector(ax,
AORG,
P2_atA,
# Introduction to Robotics 3rd Edition by Craig
# Example 2-4.
# P1_atA. We wish rotate it about hat_Z by 30 degrees and
# translate it 10 units in hat_X_A, and 5 units in hat_Y_A.
# Find P2_atA, where P1_atA = [3, 7, 0].
import matplotlib.pyplot as plt
import drawRobotics as dR
import numpy as np
P1_atA = np.array([3, 7, 0, 1])
Dq = np.array([10, 5, 0])
operatorT = np.r_[np.c_[dR.RotZ(dR.conv2Rad(30)), Dq],
[np.array([0, 0, 0, 1])]]
P2_atA = np.dot(operatorT, P1_atA)
AORG = np.array([0, 0, 0])
hat_X_A, hat_Y_A, hat_Z_A = np.array([1, 0,
0]), np.array([0, 1,
0]), np.array([0, 0, 1])
print('P1_atA = ', P1_atA)
print('Dq = ', Dq)
print('operatorT = ', operatorT)
print('P2_atA = ', P2_atA)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')