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
示例#2
0
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
示例#5
0
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
示例#6
0
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
示例#7
0
# 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)
示例#8
0
# 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} $ ')
示例#9
0
# 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')
示例#10
0
# 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,
示例#11
0
# 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')