class arm1(arm):
def __init__(self):
self.ac = ArmController()
seg = list()
seg.append(s.segment(0, .012, 0, 0))
seg.append(s.segment(0, 0, .077, (np.pi / 2)))
seg.append(s.segment((np.pi / 2) - acos(.128 / .13), .130, 0, 0))
seg.append(s.segment(acos(.128 / .13) - (np.pi / 2), .124, 0, 0))
seg.append(s.segment(0, .126, 0, -(np.pi / 2)))
self.segments = seg
super(arm1, self).__init__(seg)
def move(self, u1, u2, u4):
print('moving to ' + str(u1) + ',' + str(u2) + ',' + str(u4))
super(arm1, self).move([0, u1, -u2, 0, -u4])
self.ac.set_joints([u1, u2, 0, u4])
def pose(self):
T = self.matrix()
x = T[0, 3]
y = T[1, 3]
z = T[2, 3]
print("x = ", x)
print("y = ", y)
print("z = ", z)
phi = atan2(-T[2, 0], sqrt(T[0, 0] * T[0, 0] + T[1, 0] * T[1, 0]))
theta = -atan2(T[1, 0] / cos(phi), T[0, 0] / cos(phi))
psi = atan2(T[2, 1] / cos(phi), T[2, 2] / cos(phi))
print("roll =", psi)
print("pitch =", phi)
print("yaw =", theta)
print(self.ac.get_pose())
return (x, y, z, psi, phi, theta)
def main():
# get X, Y, Z, roll, pitch, and yaw at end of arm
posX = input("Arm's X value? ")
posY = input("Arm's Y value? ")
posZ = input("Arm's Z value? ")
psi = input("Arm's Roll? ")
phi = input("Arm's Pitch? ")
theta = input("Arm's Yaw? ")
# Matrix = T
matrix = getTransMatrix(posX, posY, posZ, psi, phi, theta)
# We now need to use the values we got after multiplying all of the matrices together to solve for cos1, sin1, cos2, sin2, and cos3, sin3.
cos1 = matrix[1][1]
sin1 = -matrix[0][1]
cos23 = matrix[2][2]
sin23 = -matrix[2][0]
# Next we do jointAngle1 = arctan2(sin1, cos1)
# jointAngle2 = arctan2(sin2, cos2)
# jointAngle3 = 0 (default)
# jointAngle4 = arctan2(sin3, cos3)
jointAngle1 = np.arctan2(sin1, cos1)
jointAngle2 = math.acos((matrix[2][3] - 0.077) / -0.126) + 0.713
sin2 = sin(jointAngle2)
cos2 = cos(jointAngle2)
jointAngle3 = np.arctan2(sin23, cos23) - np.arctan2(sin2, cos2)
# Print joint angles to reach user's desired position
print("Joint angles required to reach the desired position:")
print("Joint Angle 1:")
print(str(jointAngle1))
print("Joint Angle 2:")
print(str(jointAngle2))
print("Joint Angle 3:")
print(str(jointAngle3))
# Move the arm to the desired position
ac = ArmController()
ac.set_joints([jointAngle1, jointAngle2, 0, jointAngle3])
from arm_controller import ArmController ac = ArmController() ac.set_joints([.2, -.5, .3, -.4]) ac.set_joints([-.3, -.6, .7, .9]) ac.set_joints([0, 0, 0, 0]) print(ac.get_pose())