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