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