def IKTestAtLeastOne(text):
global thetas
np.set_printoptions(precision=2)
# draw specified point
x, y, z = text.split(" ")
print("x: {}, y: {}, z: {}".format(x, y, z))
desiredToolPos = np.array([int(x), int(y), int(z)])
# initial values and random theta directions
maxRotation = 5 # specifies max degrees any joint is allowed to change by in a np.single frame
thresholdDistance = 50 # how precise we want to be
initialThetas = thetas
currentToolPos = ForwardK.getEndEffectorData(initialThetas)[
0] # initial position
currentDistance = np.sqrt(
(desiredToolPos[0] - currentToolPos[0])**2 +
(desiredToolPos[1] - currentToolPos[1])**2 +
(desiredToolPos[2] - currentToolPos[2])**2) # initial distance
initialDistance = currentDistance
while currentDistance > thresholdDistance:
noChange = np.full(6, False)
for i in range(0, 3):
thetasCopyAdd = np.copy(
thetas
) # these arrays of thetas reset for each joint to evaluate each angle individually
thetasCopySub = np.copy(thetas)
thetasCopyAdd[i] += 5
thetasCopySub[i] -= 5
testDistanceAdd = getToolPositionDistance(thetasCopyAdd,
desiredToolPos)[1]
if testDistanceAdd < currentDistance:
thetas[i] = thetasCopyAdd[i]
else:
testDistanceSub = getToolPositionDistance(
thetasCopySub, desiredToolPos)[1]
if testDistanceSub < currentDistance:
thetas[i] = thetasCopySub[i]
else:
noChange[i] = True
# set new current distance and update canvas with new joint positions
currentJointPositions, baseTransforms = ForwardK.updateMatrices(
thetas)[0::2]
currentToolPos = currentJointPositions[len(currentJointPositions) - 1]
currentDistance = np.sqrt((desiredToolPos[0] - currentToolPos[0])**2 +
(desiredToolPos[1] - currentToolPos[1])**2 +
(desiredToolPos[2] - currentToolPos[2])**2)
# update amount allowed to rotate
rotation = ((currentDistance / initialDistance) * maxRotation)**2 + 1
print("")
print("current Distance:")
print(currentDistance)
drawArm(currentJointPositions, baseTransforms)
ax.scatter(int(x), int(y), int(z), s=70)
plt.pause(.005)
def findEndThetas():
global thetas
# initial values and random theta directions
maxRotation = 2 # specifies max degrees any joint is allowed to change by in a single frame
rotation = maxRotation
thresholdDistance = 50 # how precise we want to be
initialThetas = thetas
currentToolPos = ForwardK.getEndEffectorData(initialThetas)[
0] # initial position
currentDistance = np.sqrt(
(desiredToolPos[0] - currentToolPos[0])**2 +
(desiredToolPos[1] - currentToolPos[1])**2 +
(desiredToolPos[2] - currentToolPos[2])**2) # initial distance
initialDistance = currentDistance
testDistance = 0
while currentDistance > thresholdDistance:
noChange = np.full(6, False)
for i in range(len(thetas)):
thetasCopyAdd = np.copy(
thetas
) # these arrays of thetas reset for each joint to evaluate each angle individually
thetasCopySub = np.copy(thetas)
thetasCopyAdd[i] += 5
thetasCopySub[i] -= 5
testDistanceAdd = getToolPositionDistance(thetasCopyAdd,
desiredToolPos)[1]
if testDistanceAdd < currentDistance:
thetas[i] = thetasCopyAdd[i]
else:
testDistanceSub = getToolPositionDistance(
thetasCopySub, desiredToolPos)[1]
if testDistanceSub < currentDistance:
thetas[i] = thetasCopySub[i]
else:
noChange[i] = True
# set new current distance and update canvas with new joint positions
currentJointPositions, baseTransforms = ForwardK.updateMatrices(
thetas)[0::2]
currentToolPos = currentJointPositions[len(currentJointPositions) - 1]
currentDistance = np.sqrt((desiredToolPos[0] - currentToolPos[0])**2 +
(desiredToolPos[1] - currentToolPos[1])**2 +
(desiredToolPos[2] - currentToolPos[2])**2)
# update amount allowed to rotate
rotation = ((currentDistance / initialDistance) * maxRotation)**2 + 1
thetaend = thetas
return thetaend
def toggleTrailLines(event):
global trailLinesBool
if trailLinesBool:
trailLinesBool = False
else:
trailLinesBool = True
jointPositions, baseTransforms = ForwardK.updateMatrices(thetas)[0::2]
drawArm(jointPositions, baseTransforms)
def updateFromSlider(val):
thetas[0] = sliderTheta1.val
thetas[1] = sliderTheta2.val
thetas[2] = sliderTheta3.val
thetas[3] = sliderTheta4.val
thetas[4] = sliderTheta5.val
thetas[5] = sliderTheta6.val
# call updateMatrices and graph points, lines, axes, and labels at new joint positions
jointPositions, baseTransforms = ForwardK.updateMatrices(thetas)[0::2]
drawArm(jointPositions, baseTransforms)
def start(text):
global thetastart
global thetaend
global trail
thetaend = findEndThetas()
traj = mr.JointTrajectory(thetastart, thetaend, Tf, N, method)
jointPositionMat = np.array(
[ForwardK.updateMatrices(traj[i])[0] for i in range(N)])
for i in range(N):
drawEverything(jointPositionMat[i])
print(jointPositionMat[i])
print("pos {}".format(i + 1))
plt.pause(0.005)
# reset everything TODO fix errors this might be causing
thetastart = thetaend
thetaend = np.zeros(6)
trail = np.empty([1, 3])
def getToolPositionDistance(thetas, desiredToolPos):
currentToolPos = ForwardK.getEndEffectorData(thetas)[0]
currentDistance = np.sqrt((desiredToolPos[0] - currentToolPos[0])**2 +
(desiredToolPos[1] - currentToolPos[1])**2 +
(desiredToolPos[2] - currentToolPos[2])**2)
return currentToolPos, currentDistance
diffz = int(abs(prevEndPosition[2] - currentEndPosition[2]))
if diffx != 0 and diffy != 0 and diffz != 0:
trail = np.append(trail, [currentEndPosition], axis=0)
prevEndPosition = currentEndPosition
ax.scatter(trail[:, 0], trail[:, 1], trail[:, 2], s=10, color="orange")
for i in range(1, len(trail)):
ax.text(trail[i, 0],
trail[i, 1],
trail[i, 2],
i,
fontsize=10,
color='red')
prevEndPosition = currentEndPosition
initialJointPositions = ForwardK.updateMatrices(thetastart)[0]
print(initialJointPositions)
drawEverything(initialJointPositions)
def start(text):
global thetastart
global thetaend
global trail
thetaend = findEndThetas()
traj = mr.JointTrajectory(thetastart, thetaend, Tf, N, method)
jointPositionMat = np.array(
[ForwardK.updateMatrices(traj[i])[0] for i in range(N)])
for i in range(N):
drawEverything(jointPositionMat[i])
print(jointPositionMat[i])
def invPose(xEnd, yEnd, zEnd, alpha, beta, gamma):
# xSw = -lenEnd*np.sin(beta) + xEnd
# ySw = lenEnd*np.cos(beta)*np.sin(alpha) + yEnd
# zSw = -lenEnd*np.cos(alpha)*np.cos(beta) + zEnd
transform0End = np.array(
[[
np.cos(beta) * np.cos(gamma), -np.cos(beta) * np.sin(gamma),
np.sin(beta), xEnd
],
[
np.cos(alpha) * np.sin(gamma) +
np.cos(gamma) * np.sin(alpha) * np.sin(beta),
np.cos(alpha) * np.cos(gamma) -
np.sin(alpha) * np.sin(beta) * np.sin(gamma),
-np.cos(beta) * np.sin(alpha), yEnd
],
[
-np.cos(alpha) * np.cos(gamma) * np.sin(beta) +
np.sin(alpha) * np.sin(gamma),
np.cos(alpha) * np.sin(beta) * np.sin(gamma) +
np.cos(gamma) * np.sin(alpha),
np.cos(alpha) * np.cos(beta), zEnd
], [0, 0, 0, 1]])
transformEndSw = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -lenEnd],
[0, 0, 0, 1]])
transform0Sw = transform0End @ transformEndSw
sphericalPos = np.vstack(transform0Sw[:-1, 3])
xSw = sphericalPos[0]
ySw = sphericalPos[1]
zSw = sphericalPos[2]
hSw = zSw - lengths[0]
if np.sqrt(xSw**2 + ySw**2 + hSw**2) > sum(lengths[1:]):
print("Desired position and orientation not in workspace.")
else:
RSw = np.sqrt(xSw**2 + ySw**2)
rSw = np.sqrt(hSw**2 + RSw**2)
alpha2 = np.arcsin(hSw / rSw)
# First three joint angles responsible for placing end effector
# Using law of cosines:
theta1 = np.arctan2(ySw, xSw)
theta2 = np.arccos((lengths[1]**2 - lengths[2]**2 + rSw**2) /
(2 * lengths[1] * rSw)) + alpha2
theta3 = np.arccos((lengths[2]**2 + lengths[1]**2 - rSw**2) /
(2 * lengths[1] * lengths[2]))
# Final three joint angles specify rotation only
# Multi stuff:
dirEnd = Vector(xEnd, yEnd, zEnd, xSw, ySw, zSw, lenEnd)
rElbow = lengths[1] * math.cos(theta2)
xElbow = rElbow * math.cos(theta1)
yElbow = rElbow * math.sin(theta1)
zElbow = lengths[0] + lengths[1] * math.sin(theta2)
dirForearm = Vector(xEnd, yEnd, zEnd, xElbow, yElbow, zElbow,
lengths[2])
print(dirForearm.get_direction())
# remove list att from th 1 2 and 3
theta1 = float(theta1)
theta2 = float(theta2)
theta3 = float(theta3)
# Using ZYZ rotation matrix:
theta4 = np.arctan2(np.sqrt(1 - np.cos(beta)**2), np.cos(beta))
theta5 = np.arctan2(
np.sin(alpha) * np.sin(beta),
np.cos(alpha) * np.sin(beta))
theta6 = np.arctan2(
np.sin(beta) * np.sin(gamma), -np.cos(gamma) * np.sin(beta))
print(f'\nxSw: {xSw}, ySw: {ySw}, zSw: {zSw}')
print(
f'\nAngles in radians:\ntheta1: {theta1}\ntheta2: {theta2}\ntheta3: {theta3}\ntheta4: {theta4}\ntheta5: {theta5}\ntheta6: {theta6}'
)
print(
f'\nAngles in degrees:\ntheta1: {theta1*180/np.pi}\ntheta2: {theta2*180/np.pi}\ntheta3: {theta3*180/np.pi}\ntheta4: {theta4*180/np.pi}\ntheta5: {theta5*180/np.pi}\ntheta6: {theta6*180/np.pi}'
)
thetas = np.array([theta1, theta2, theta3, theta4, theta5, theta6])
thetas = thetas * 180 / np.pi
#recalculate forward kinematics to compare
jointPos, jointRot, _ = fk.updateMatrices(thetas)
print(f"jointPos[5]: {jointPos[5]}")
print(f"jointPos[6]: {jointPos[6]}")
print(f"jointRot[5]: {jointRot[5]}"
) # todo: otolpos and toolrot don't match with original values
print(f"jointRot[6]: {jointRot[6]}")
print(f"\ntransform0End: {transform0End}")
print(f"xError: {xEnd - jointPos[5][0]}")
print(f"xError: {yEnd - jointPos[5][1]}")
print(f"xError: {zEnd - jointPos[5][2]}")
def IKTestIndividual(text):
global thetas
np.set_printoptions(precision=2)
# draw specified point
x, y, z = text.split(" ")
print("x: {}, y: {}, z: {}".format(x, y, z))
desiredToolPos = np.array([int(x), int(y), int(z)])
# initial values and random theta directions
maxRotation = 100 # specifies max degrees any joint is allowed to change by in a single frame
thresholdDistance = 50 # how precise we want to be
initialThetas = thetas
thetaWeights = np.random.randn(6)
thetaWeightsNext = np.zeros(6)
currentToolPos = ForwardK.getEndEffectorData(initialThetas)[
0] # initial position
currentDistance = np.sqrt(
(desiredToolPos[0] - currentToolPos[0])**2 +
(desiredToolPos[1] - currentToolPos[1])**2 +
(desiredToolPos[2] - currentToolPos[2])**2) # initial distance
while currentDistance > thresholdDistance:
# create new changes in theta
testThetaDirs = np.vstack([thetas] * len(thetas))
np.fill_diagonal(testThetaDirs,
np.diagonal(testThetaDirs) + thetaWeights)
print(testThetaDirs)
# test changes in theta
for i in range(len(testThetaDirs)):
# get new end effector position as a result of only one theta changing
testToolPosition = ForwardK.getEndEffectorData(testThetaDirs[i])[0]
# find new distance for each theta to see how to see how to set weights
newTestDistance = np.sqrt(
(desiredToolPos[0] - testToolPosition[0])**2 +
(desiredToolPos[1] - testToolPosition[1])**2 +
(desiredToolPos[2] - testToolPosition[2])**2)
thetaWeightsNext[i] = (
1 - newTestDistance / currentDistance
) # flip weight sign if new distance is bigger
thetaDeltas = thetaWeightsNext * maxRotation
thetaWeights = thetaDeltas + thetaWeights
# print("theta weights: ")
# # print(thetaWeights)
# thetaDeltas = thetaWeights * maxRotation
# print("theta deltas: ")
# print(thetaDeltas)
# update joint angles and find new cumulative end effector and joint positions
thetas = thetaDeltas + thetas
currentJointPositions, baseTransforms = ForwardK.updateMatrices(
thetas)[0::2] # 0 is joint positions
# current tool position is all we currently care about for finding final joint angles
# TODO figure out how to account for all other joint movements
currentToolPos = currentJointPositions[
len(currentJointPositions) -
1] # tool position is last in the array of joint positions
currentDistance = np.sqrt((desiredToolPos[0] - currentToolPos[0])**2 +
(desiredToolPos[1] - currentToolPos[1])**2 +
(desiredToolPos[2] - currentToolPos[2])**2)
drawArm(currentJointPositions, baseTransforms)
ax.scatter(int(x), int(y), int(z), s=70)
plt.pause(.005)
def IKTestTogether(text):
global thetas
np.set_printoptions(precision=2)
# draw specified point
x, y, z = text.split(" ")
print("x: {}, y: {}, z: {}".format(x, y, z))
desiredToolPos = np.array([int(x), int(y), int(z)])
# initial values and random theta directions
maxRotation = 5 # specifies max degrees any joint is allowed to change by in a single frame
rotation = maxRotation
thresholdDistance = 50 # how precise we want to be
initialThetas = thetas
currentToolPos = ForwardK.getEndEffectorData(initialThetas)[
0] # initial position
currentDistance = np.sqrt(
(desiredToolPos[0] - currentToolPos[0])**2 +
(desiredToolPos[1] - currentToolPos[1])**2 +
(desiredToolPos[2] - currentToolPos[2])**2) # initial distance
initialDistance = currentDistance
testDistance = 0
while currentDistance > thresholdDistance:
numTries = 0
while numTries < 50:
testChanges = np.random.rand(6)
testThetas = thetas + testChanges * rotation
currentJointPositions, baseTransforms = ForwardK.updateMatrices(
testThetas)[0::2] # 0 is joint positions
currentToolPos = currentJointPositions[
len(currentJointPositions) -
1] # tool position is last in the array of joint positions
testDistance = np.sqrt((desiredToolPos[0] - currentToolPos[0])**2 +
(desiredToolPos[1] - currentToolPos[1])**2 +
(desiredToolPos[2] - currentToolPos[2])**2)
if testDistance < currentDistance:
break
else:
numTries += 1
if numTries is 50:
for i in range(len(thetas)):
testThetasCopy = thetas
numSingleTries = 0
while numSingleTries < 50:
testSingleChange = np.random.rand(1) # 0.734
testSingleTheta = thetas[
i] + testSingleChange * rotation # 360 + 0.734 * rotation
testThetasCopy[i] = testSingleTheta # 382copy = 382
# testThetasCopy = [360 382 360 360 360 360] only changing one at a time
currentJointPositions, baseTransforms = ForwardK.updateMatrices(
testThetasCopy)[0::2]
currentToolPos = currentJointPositions[
len(currentJointPositions) - 1]
testDistanceSingle = np.sqrt(
(desiredToolPos[0] - currentToolPos[0])**2 +
(desiredToolPos[1] - currentToolPos[1])**2 +
(desiredToolPos[2] - currentToolPos[2])**2)
if testDistanceSingle < currentDistance:
testThetas[
i] = testSingleTheta # if distance is less replace next theta for that joint
break
else:
numSingleTries += 1
if numSingleTries is 50:
testThetas[i] = thetas[
i] # if distance is never less don't move theta for that joint
thetas = testThetas
# set new current distance
currentJointPositions, baseTransforms = ForwardK.updateMatrices(
thetas)[0::2]
currentToolPos = currentJointPositions[len(currentJointPositions) - 1]
currentDistance = np.sqrt((desiredToolPos[0] - currentToolPos[0])**2 +
(desiredToolPos[1] - currentToolPos[1])**2 +
(desiredToolPos[2] - currentToolPos[2])**2)
# update amount allowed to rotate
rotation = (currentDistance / initialDistance) * maxRotation + 1
drawArm(currentJointPositions, baseTransforms)
ax.scatter(int(x), int(y), int(z), s=70)
plt.pause(.005)
prevEndPosition = currentEndPosition
ax.scatter(trail[:, 0], trail[:, 1], trail[:, 2], s=10, color="orange")
for i in range(1, len(trail)):
ax.text(trail[i, 0], trail[i, 1], trail[i, 2], i, color='red')
prevEndPosition = currentEndPosition
def drawTrailLines():
global trailLinesBool
global trail
if trailLinesBool:
ax.plot3D(trail[1:, 0], trail[1:, 1], trail[1:, 2], color="orange")
# get initial joint positions
jointPositions, jointRotationMatrices, baseTransforms = ForwardK.updateMatrices(
thetas)
# Graph joint positions
fig = plt.figure(figsize=(5, 6))
ax = fig.add_subplot(projection='3d')
plt.subplots_adjust(bottom=0.4)
maxZ = 400
minZ = -400
plotPointsLines(jointPositions)
drawLabels(jointPositions)
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_zlabel("z")
ax.view_init(azim=30)
plotAxes(jointPositions, baseTransforms)
# Add slider for each axis
z = np.zeros(N)
m = 0
for i in range(N):
i = i*10 #与采样组匹配修改
#存储关节角位置
with open('data/wishCircle_theta1.txt', 'a') as output_to_write:
for k in range(l):
theta_data = Theta[i,k]
#theta_data = round(Theta[i,k],6) #保存6位精度
output_to_write.write(str(theta_data))
output_to_write.write(' ')
output_to_write.write('\n')
#调用正运动学方程，求解7关节到0关节传递矩阵
T0 = fk.T_7to0(Theta[i,:],alpha,a,d)
x[m] = T0[0,3] #提取末端位置
y[m] = T0[1,3]
z[m] = T0[2,3]
m = m+1
#存储位置数据
#末端位置用12个参数表示，分别为姿态角旋转矩阵9参数，末端位置3参数
#写入方式为齐次矩阵前三行从左到右，从上到下
with open('data/wishCircle_line1.txt', 'a') as input_to_write:
for j_i in range(4):
for j_j in range(3):
T_data = T0[j_j,j_i]
#T_data = round(T_data,6)
input_to_write.write(str(T_data))
input_to_write.write(' ')
a = np.zeros(7)
alpha = [90, 90, 90, 90, 90, 90, 0]
alpha = np.array(alpha) * (np.pi / 180)
d = np.array([323.5, 0, 316, 0, 284.5, 0, 201]) * 0.001
thetaMax = np.array([180, 90, 180, 90, 180, 90, 180]) * (np.pi / 180)
thetaMin = np.array([-180, -90, -180, -90, -180, -90, -180]) * (np.pi / 180)
theta = np.zeros([500, 7])
pos = np.zeros([500, 12])
for i in range(500):
for j in range(7):
theta[i, j] = thetaMin[j] + (thetaMax[j] -
thetaMin[j]) * np.random.random()
T = fk.T_7to0(theta[i, :], alpha, a, d)
#末端位置用12个参数表示，分别为姿态角旋转矩阵9参数，末端位置3参数
#写入方式为齐次矩阵前三行从左到右，从上到下
k = 0
for j_i in range(4):
for j_j in range(3):
pos[i, k] = T[j_j, j_i]
k = k + 1
#存储关节角位置
print theta
#FileOpen.write(theta,'data/randtheta500_data1.txt')
#存储位置信息
print pos
import numpy as np
import sys
sys.path.append('../')
import ForwardKinematics
np.set_printoptions(suppress=True)
test_joints_array = np.array([
[0.0, 0.0, 0.0, 0.0],
[np.pi / 4, np.pi / 4, np.pi / 4, np.pi / 4],
[np.pi / 8, np.pi / 2, np.pi / 3, np.pi / 3],
[np.pi / 3, np.pi / 5, np.pi / 6, np.pi / 7],
[-np.pi / 4, np.pi / 4, -np.pi / 4, np.pi / 6],
[-np.pi / 9, -np.pi / 8, -np.pi / 5, np.pi / 3],
[-np.pi / 10, np.pi / 6, np.pi / 6, np.pi / 2],
[np.pi / 4, np.pi / 7, -np.pi / 5, -np.pi / 7],
[np.pi / 5, -np.pi / 4, -np.pi / 8, np.pi / 8]
])
for i in range(test_joints_array.shape[0]):
result1 = ForwardKinematics.automatic_forward_kinematics(test_joints_array[i])
result2 = ForwardKinematics.manual_forward_kinematics(test_joints_array[i])
if np.allclose(result1, result2) is False:
print("Automatic FK is different from Manual FK in the following case:")
print("Joint Angles: {}. Automatic FK: {}. Manual FK: {}.".format(
test_joints_array[i],
ForwardKinematics.automatic_forward_kinematics(test_joints_array[i]),
ForwardKinematics.manual_forward_kinematics(test_joints_array[i])))
def SkyentificInverseK(input):
global thetas
np.set_printoptions(precision=2)
# draw desired end effector location
x, y, z, a, b, g = input.split(" ")
print("x: {}, y: {}, z: {}".format(x, y, z))
# desiredToolPos = np.array([int(x), int(y), int(z)])
Xik = np.array([
float(x),
float(y),
float(z),
float(a) * np.pi / 180,
float(b) * np.pi / 180,
float(g) * np.pi / 180
])
Jik = np.zeros(6)
# inverse kinematics
# input: Xik - pos value for the calculation of the inverse kinematics
# output: Jik - joints value for the calculation of the inversed kinematics
# Denavit-Hartenberg matrix
theta = np.array([0.0, -90.0, 0.0, 0.0, 0.0,
0.0]) # theta=[0 -90+0 0 0 0 0]
alfa = np.array([-90.0, 0.0, -90.0, 90.0, -90.0,
0.0]) # alfa=[-90 0 -90 90 -90 0]
r = np.array([r1, r2, r3, 0.0, 0.0, 0.0]) # r=[47 110 26 0 0 0]
d = np.array([d1, 0.0, d3, d4, 0.0, d6]) # d=[133 0 7 117.5 0 28]
# from deg to rad
mh.MatrixScale(theta, np.pi / 180.0) # theta=theta*pi/180
mh.MatrixScale(alfa, np.pi / 180.0) # alfa=alfa*pi/180
# work frame
Xwf = np.zeros(6)
# tool frame
Xtf = np.zeros(6)
# work frame transformation matrix
Twf = mh.pos2tran(Xwf)
print("Twf: {}".format(Twf))
# tool frame transformation matrix
Ttf = mh.pos2tran(Xtf)
print("Ttf: {}".format(Ttf))
# total transformation matrix
Twt = mh.pos2tran(Xik)
print("Twt: {}".format(Twt))
# find T06
inTwf = mh.invtran(Twf) # inTwf=mh.invtran(Twf)
inTtf = mh.invtran(Ttf) # inTtf=mh.invtran(Ttf)
print("inTwf: {}".format(inTwf))
print("inTft: {}".format(inTtf))
# np.matmul(Twt, inTtf, Tw6) # Tw6=Twt*inTtf
Tw6 = Twt @ inTtf
print("Tw6: {}".format(Tw6))
# np.matmul(inTwf, Tw6, T06) # T06=inTwf*Tw6
T06 = inTwf @ Tw6
print("T06: {}".format(T06))
# positon of the spherical wrist
Xsw = np.array([
T06[0][3] - d[5] * T06[0][2], T06[1][3] - d[5] * T06[1][2],
T06[2][3] - d[5] * T06[2][2]
])
print("Wrist position: {}".format(Xsw))
# Calculate joint angles
# first joint
Jik[0] = np.arctan2(Xsw[1], Xsw[0]) - np.arctan2(
d[2], np.sqrt(Xsw[0] * Xsw[0] + Xsw[1] * Xsw[1] - d[2] * d[2])
) # Jik(1)=np.arctan2(Xsw(2),Xsw(1))-np.arctan2(d(3),np.sqrt(Xsw(1)^2+Xsw(2)^2-d(3)^2))
# second joint
Jik[1] = np.pi / 2.0
-np.arccos(
(r[1] * r[1] + (Xsw[2] - d[0]) * (Xsw[2] - d[0]) +
(np.sqrt(Xsw[0] * Xsw[0] + Xsw[1] * Xsw[1] - d[2] * d[2]) - r[0]) *
(np.sqrt(Xsw[0] * Xsw[0] + Xsw[1] * Xsw[1] - d[2] * d[2]) - r[0]) -
(r[2] * r[2] + d[3] * d[3])) /
(2.0 * r[1] * np.sqrt(
(Xsw[2] - d[0]) * (Xsw[2] - d[0]) +
(np.sqrt(Xsw[0] * Xsw[0] + Xsw[1] * Xsw[1] - d[2] * d[2]) - r[0]) *
(np.sqrt(Xsw[0] * Xsw[0] + Xsw[1] * Xsw[1] - d[2] * d[2]) - r[0])))
)
-np.arctan(
(Xsw[2] - d[0]) /
(np.sqrt(Xsw[0] * Xsw[0] + Xsw[1] * Xsw[1] - d[2] * d[2]) - r[0])
) # Jik(2)=pi/2-np.arccos((r(2)^2+(Xsw(3)-d(1))^2+(np.sqrt(Xsw(1)^2+Xsw(2)^2-d(3)^2)-r(1))^2-(r(3)^2+d(4)^2))/(2*r(2)*np.sqrt((Xsw(3)-d(1))^2+(np.sqrt(Xsw(1)^2+Xsw(2)^2-d(3)^2)-r(1))^2)))-np.arctan((Xsw(3)-d(1))/(np.sqrt(Xsw(1)^2+Xsw(2)^2-d(3)^2)-r(1)))
# third joint
Jik[2] = np.pi
-np.arccos(
(r[1] * r[1] + r[2] * r[2] + d[3] * d[3] - (Xsw[2] - d[0]) *
(Xsw[2] - d[0]) -
(np.sqrt(Xsw[0] * Xsw[0] + Xsw[1] * Xsw[1] - d[2] * d[2]) - r[0]) *
(np.sqrt(Xsw[0] * Xsw[0] + Xsw[1] * Xsw[1] - d[2] * d[2]) - r[0])) /
(2 * r[1] * np.sqrt(r[2] * r[2] + d[3] * d[3])))
-np.arctan(
d[3] / r[2]
) # Jik(3)=pi-np.arccos((r(2)^2+r(3)^2+d(4)^2-(Xsw(3)-d(1))^2-(np.sqrt(Xsw(1)^2+Xsw(2)^2-d(3)^2)-r(1))^2)/(2*r(2)*np.sqrt(r(3)^2+d(4)^2)))-np.arctan(d(4)/r(3))
# last three joints
T01 = np.zeros((4, 4))
T12 = np.zeros((4, 4))
T23 = np.zeros((4, 4))
T02 = np.zeros((4, 4))
T03 = np.zeros((4, 4))
inT03 = np.zeros((4, 4))
T36 = np.zeros((4, 4))
T01 = mh.DH1line(theta[0] + Jik[0], alfa[0], r[0],
d[0]) # T01=DH1line(theta(1)+Jik(1),alfa(1),r(1),d(1))
T12 = mh.DH1line(theta[1] + Jik[1], alfa[1], r[1],
d[1]) # T12=DH1line(theta(2)+Jik(2),alfa(2),r(2),d(2))
T23 = mh.DH1line(theta[2] + Jik[2], alfa[2], r[2],
d[2]) # T23=DH1line(theta(3)+Jik(3),alfa(3),r(3),d(3))
# np.matmul(T01, T12, T02)
# np.matmul(T02, T23, T03)
# np.matmul(inT03, T06, T36)
T02 = T01 @ T12
T03 = T02 @ T23
T36 = inT03 @ T06
inT03 = mh.invtran(T03) # inT03=mh.invtran(T03)
# forth joint
Jik[3] = np.arctan2(-T36[1][2],
-T36[0][2]) # Jik(4)=np.arctan2(-T36(2,3),-T36(1,3))
# fifth joint
Jik[4] = np.arctan2(
np.sqrt(T36[0][2] * T36[0][2] + T36[1][2] * T36[1][2]), T36[2]
[2]) # Jik(5)=np.arctan2(np.sqrt(T36(1,3)^2+T36(2,3)^2),T36(3,3))
# sixth joints
Jik[5] = np.arctan2(-T36[2][1],
T36[2][0]) # Jik(6)=np.arctan2(-T36(3,2),T36(3,1))
# rad to deg
mh.MatrixScale(Jik, 180.0 / np.pi) # Jik=Jik/pi*180
thetas = Jik
print("THETAS: {}".format(thetas))
currentJointPositions, baseTransforms = ForwardK.updateMatrices(
thetas)[0::2]
print("JOINT POSITIONS: {}".format(currentJointPositions))
drawArm(currentJointPositions, baseTransforms)
def findEndThetas():
global thetas
np.set_printoptions(precision=2)
# draw desired end effector location
xEnd = desiredToolPos[0]
yEnd = desiredToolPos[1]
zEnd = desiredToolPos[2]
alpha = desiredToolOri[0]
beta = desiredToolOri[1]
gamma = desiredToolOri[2]
# alpha, beta, gamma input as degrees
xEnd = float(xEnd)
yEnd = float(yEnd)
zEnd = float(zEnd)
alpha = float(alpha)
beta = float(beta)
gamma = float(gamma)
# convert to radians
alpha = alpha * np.pi / 180
beta = beta * np.pi / 180
gamma = gamma * np.pi / 180
lengths = np.array([135.7, 300.32, 293])
# calculate position of spherical wrist
transform0End = np.array(
[[
np.cos(beta) * np.cos(gamma), -np.cos(beta) * np.sin(gamma),
np.sin(beta), xEnd
],
[
np.cos(alpha) * np.sin(gamma) +
np.cos(gamma) * np.sin(alpha) * np.sin(beta),
np.cos(alpha) * np.cos(gamma) -
np.sin(alpha) * np.sin(beta) * np.sin(gamma),
-np.cos(beta) * np.sin(alpha), yEnd
],
[
-np.cos(alpha) * np.cos(gamma) * np.sin(beta) +
np.sin(alpha) * np.sin(gamma),
np.cos(alpha) * np.sin(beta) * np.sin(gamma) +
np.cos(gamma) * np.sin(alpha),
np.cos(alpha) * np.cos(beta), zEnd
], [0, 0, 0, 1]])
transformEndSw = np.array([[1, 0, 0, -d6_1 - 60], [0, 1, 0, 0],
[0, 0, 1, 0], [0, 0, 0, 1]])
transform0Sw = transform0End @ transformEndSw
sphericalPos = np.vstack(transform0Sw[:-1, 3])
sphericalPos = np.array(sphericalPos).ravel()
print(f'Spherical pos: {sphericalPos}')
xSw = sphericalPos[0]
ySw = sphericalPos[1]
zSw = sphericalPos[2]
hSw = zSw - lengths[0]
if np.sqrt(xSw**2 + ySw**2 + hSw**2) > sum(lengths[1:]):
print("Desired position and orientation not in workspace.")
else:
RSw = np.sqrt(xSw**2 + ySw**2)
rSw = np.sqrt(hSw**2 + RSw**2)
alpha2 = np.arcsin(hSw / rSw)
# First three joint angles responsible for placing end effector
# Using law of cosines:
theta1 = np.arctan2(ySw, xSw)
theta2 = -np.arccos((lengths[1]**2 - lengths[2]**2 + rSw**2) /
(2 * lengths[1] * rSw)) + np.pi / 2 - alpha2
theta3 = -np.arccos((lengths[2]**2 + lengths[1]**2 - rSw**2) /
(2 * lengths[1] * lengths[2])) + np.pi / 2
# remove list att from th 1 2 and 3
theta1 = float(theta1)
theta2 = float(theta2)
theta3 = float(theta3)
# Final three joint angles specify rotation only
# rot_mat_0_6_1 = np.array([[np.cos(beta)*np.cos(gamma), -np.cos(beta)*np.sin(gamma), np.sin(beta)],
# [np.cos(alpha)*np.sin(gamma) + np.cos(gamma)*np.sin(alpha)*np.sin(beta), np.cos(alpha)*np.cos(gamma) - np.sin(alpha)*np.sin(beta)*np.sin(gamma), -np.cos(beta)*np.sin(alpha)],
# [-np.cos(alpha)*np.cos(gamma)*np.sin(beta) + np.sin(alpha)*np.sin(gamma), np.cos(alpha)*np.sin(beta)*np.sin(gamma) + np.cos(gamma)*np.sin(alpha), np.cos(alpha)*np.cos(beta)]])
rot_mat_0_6 = transform0End[:3][:3]
# print(rot_mat_0_6_1)
# print(rot_mat_0_6)
rot_mat_0_3 = np.array(
[[
np.cos(theta1) * np.cos(theta2) * np.cos(theta3) -
np.cos(theta1) * np.sin(theta2) * np.sin(theta3),
-np.sin(theta1),
np.cos(theta1) * np.cos(theta2) * np.sin(theta3) +
np.cos(theta1) * np.cos(theta3) * np.sin(theta2)
],
[
np.cos(theta2) * np.cos(theta3) * np.sin(theta1) -
np.sin(theta1) * np.sin(theta2) * np.sin(theta3),
np.cos(theta1),
np.cos(theta2) * np.sin(theta1) * np.sin(theta3) +
np.cos(theta3) * np.sin(theta1) * np.sin(theta2)
],
[
-np.cos(theta2) * np.sin(theta3) -
np.cos(theta3) * np.sin(theta2), 0,
np.cos(theta2) * np.cos(theta3) -
np.sin(theta2) * np.sin(theta3)
]])
inv_rot_mat_0_3 = np.linalg.inv(rot_mat_0_3)
# rot_mat_3_6_1 = inv_rot_mat_0_3 @ rot_mat_0_6_1
# print(rot_mat_3_6_1)
rot_mat_3_6 = inv_rot_mat_0_3 @ rot_mat_0_6
# print(rot_mat_3_6)
theta5 = np.arccos(rot_mat_3_6[0, 0])
theta6_1 = np.arcsin(rot_mat_3_6[0, 1] / np.sin(theta5))
theta6_2 = np.pi - np.arcsin(rot_mat_3_6[0, 1] / np.sin(theta5))
theta4_1 = np.arcsin(rot_mat_3_6[1, 0] / np.sin(theta5))
theta4_2 = np.pi - np.arcsin(rot_mat_3_6[1, 0] / np.sin(theta5))
# calculate tool positions for both possible angles to see which angle produces a closer result
transform01 = np.array([[np.cos(theta1), -np.sin(theta1), 0, xOff[0]],
[np.sin(theta1),
np.cos(theta1), 0, yOff[0]],
[0, 0, 1, zOff[0]], [0, 0, 0, 1]])
transform12 = np.array([[np.cos(theta2), 0,
np.sin(theta2), xOff[1]], [0, 1, 0, yOff[1]],
[-np.sin(theta2), 0,
np.cos(theta2), zOff[1]], [0, 0, 0, 1]])
transform23 = np.array([[np.cos(theta3), 0,
np.sin(theta3), xOff[2]], [0, 1, 0, yOff[2]],
[-np.sin(theta3), 0,
np.cos(theta3), zOff[2]], [0, 0, 0, 1]])
transform34_1 = np.array(
[[1, 0, 0, xOff[3]],
[0, np.cos(theta4_1), -np.sin(theta4_1), yOff[3]],
[0, np.sin(theta4_1),
np.cos(theta4_1), zOff[3]], [0, 0, 0, 1]])
transform45 = np.array([[np.cos(theta5), 0,
np.sin(theta5), xOff[4]], [0, 1, 0, yOff[4]],
[-np.sin(theta5), 0,
np.cos(theta5), zOff[4]], [0, 0, 0, 1]])
transform56_1 = np.array(
[[1, 0, 0, xOff[5]],
[0, np.cos(theta6_1), -np.sin(theta6_1), yOff[5]],
[0, np.sin(theta6_1),
np.cos(theta6_1), zOff[5]], [0, 0, 0, 1]])
transform34_2 = np.array(
[[1, 0, 0, xOff[3]],
[0, np.cos(theta4_2), -np.sin(theta4_2), yOff[3]],
[0, np.sin(theta4_2),
np.cos(theta4_2), zOff[3]], [0, 0, 0, 1]])
transform56_2 = np.array(
[[1, 0, 0, xOff[5]],
[0, np.cos(theta6_2), -np.sin(theta6_2), yOff[5]],
[0, np.sin(theta6_2),
np.cos(theta6_2), zOff[5]], [0, 0, 0, 1]])
# Working position of tool in end effector coordinates
transform6Tool = np.array([[1, 0, 0, endEffector[0]],
[0, 1, 0, endEffector[1]],
[0, 0, 1, endEffector[2]], [0, 0, 0, 1]])
transform = np.array([
transform01, transform12, transform23, transform34_1, transform45,
transform56_1, transform34_2, transform56_2, transform6Tool
])
transform02 = transform[0] @ transform[1]
transform03 = transform[0] @ transform[1] @ transform[2]
transform04_1 = transform[0] @ transform[1] @ transform[2] @ transform[
3]
transform05_1 = transform[0] @ transform[1] @ transform[2] @ transform[
3] @ transform[4]
transform06_1 = transform[0] @ transform[1] @ transform[2] @ transform[
3] @ transform[4] @ transform[5]
toolPos06_1 = transform06_1 @ transform6Tool
transform04_2 = transform[0] @ transform[1] @ transform[2] @ transform[
6]
transform05_2 = transform[0] @ transform[1] @ transform[2] @ transform[
6] @ transform[4]
transform06_2 = transform[0] @ transform[1] @ transform[2] @ transform[
6] @ transform[4] @ transform[7]
toolPos06_2 = transform06_2 @ transform6Tool
baseTransforms1 = np.array([
transform01, transform02, transform03, transform04_1,
transform05_1, transform06_1, toolPos06_1
])
baseTransforms2 = np.array([
transform01, transform02, transform03, transform04_2,
transform05_2, transform06_2, toolPos06_2
])
toolpos1 = toolPos06_1[:-1, 3]
toolpos2 = toolPos06_2[:-1, 3]
distFrom1 = np.sqrt((toolpos1[0] - xEnd)**2 + (toolpos1[1] - yEnd)**2 +
(toolpos1[2] - zEnd)**2)
distFrom2 = np.sqrt((toolpos2[0] - xEnd)**2 + (toolpos2[1] - yEnd)**2 +
(toolpos2[2] - zEnd)**2)
theta4 = 0
theta6 = 0
# choose theta 4 and 6 pair that results in least distance between desired and calculated end effector position
if distFrom1 < distFrom2:
theta4 = theta4_1
theta6 = theta6_1
else:
theta4 = theta4_2
theta6 = theta6_2
# print(f'\nxSw: {xSw}, : {ySw}, zSw: {zSw}')
# print(f'\nAngles in radians:\ntheta1: {theta1}\ntheta2: {theta2}\ntheta3: {theta3}\ntheta4: {theta4}\ntheta5: {theta5}\ntheta6: {theta6}')
# print(f'\nAngles in degrees:\ntheta1: {theta1*180/np.pi}\ntheta2: {theta2*180/np.pi}\ntheta3: {theta3*180/np.pi}\ntheta4: {theta4*180/np.pi}\ntheta5: {theta5*180/np.pi}\ntheta6: {theta6*180/np.pi}')
thetas = np.array([theta1, theta2, theta3, theta4, theta5, theta6])
thetas = thetas * 180 / np.pi
#recalculate forward kinematics to compare
jointPos, jointRot, _ = ForwardK.updateMatrices(thetas)
# print(f"jointPos[5]: {jointPos[5]}")
# print(f"jointPos[6]: {jointPos[6]}")
# print(f"jointRot[5]: {jointRot[5]}") # todo: otolpos and toolrot don't match with original values
# print(f"jointRot[6]: {jointRot[6]}")
# print(f"\ntransform0End: {transform0End}")
print(f"\nxError: {xEnd - jointPos[6][0]}")
print(f"yError: {yEnd - jointPos[6][1]}")
print(f"zError: {zEnd - jointPos[6][2]}")
# currentJointPositions, baseTransforms = ForwardK.updateMatrices(thetas)[0::2]
# # print("JOINT POSITIONS: {}".format(currentJointPositions))
# drawArm(currentJointPositions, baseTransforms)
return thetas
#Theta2 = np.zeros([N,l])
x1 = np.zeros(N)
y1 = np.zeros(N)
z1 = np.zeros(N)
x2 = np.zeros(N)
y2 = np.zeros(N)
z2 = np.zeros(N)
for i in range(N):
for j in range(l):
Theta1[i, j] = eval(data1[i, j])
#Theta2[i,j] = eval(data2[i,j])
for i in range(N):
T1 = fk.T_7to0(Theta1[i, :], alpha, a, d)
#T2 = fk.T_7to0(Theta2[i,:],alpha,a,d)
x1[k] = T1[0, 3]
y1[k] = T1[1, 3]
z1[k] = T1[2, 3]
x2[k] = data3[i, 9]
y2[k] = data3[i, 10]
z2[k] = data3[i, 11]
k = k + 1
'''
with open('data/PrePos_line3.txt', 'a') as file_to_write:
for col in range(4):
for row in range(3):
file_to_write.write(str(T1[row,col]))
file_to_write.write(' ')
file_to_write.write('\n')
def straightLeg_Link(Length, dir, time):
step = 15
tx1 = ForwardKinematics.Tempxyz()[0]
ty1 = ForwardKinematics.Tempxyz()[1]
tz1 = ForwardKinematics.Tempxyz()[2]
tx2 = ForwardKinematics.Tempxyz()[3]
ty2 = ForwardKinematics.Tempxyz()[4]
tz2 = ForwardKinematics.Tempxyz()[5]
tx3 = ForwardKinematics.Tempxyz()[6]
ty3 = ForwardKinematics.Tempxyz()[7]
tz3 = ForwardKinematics.Tempxyz()[8]
tx4 = ForwardKinematics.Tempxyz()[9]
ty4 = ForwardKinematics.Tempxyz()[10]
tz4 = ForwardKinematics.Tempxyz()[11]
tx5 = ForwardKinematics.Tempxyz()[12]
ty5 = ForwardKinematics.Tempxyz()[13]
tz5 = ForwardKinematics.Tempxyz()[14]
tx6 = ForwardKinematics.Tempxyz()[15]
ty6 = ForwardKinematics.Tempxyz()[16]
tz6 = ForwardKinematics.Tempxyz()[17]
for i in range(0, 31):
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
straight_Leg1_Link(Length, dir, tx1, ty1, tz1, i, step, date)
straight_Leg2_Link(Length, dir, tx2, ty2, tz2, i, step, date)
straight_Leg3_Link(Length, dir, tx3, ty3, tz3, i, step, date)
straight_Leg4_Link(Length, dir, tx4, ty4, tz4, i, step, date)
straight_Leg5_Link(Length, dir, tx5, ty5, tz5, i, step, date)
straight_Leg6_Link(Length, dir, tx6, ty6, tz6, i, step, date)
ser.write(date)
