def setUp(self):
arm = DebraArm.DebraArm(l1=l1, l2=l2)
self.ws = Workspace(0.2,2.0, -1.0,1.0, 0.0,0.2, 1)
ws_front = self.ws
ws_side = self.ws
ws_back = self.ws
self.arm_mng = ArmManager.ArmManager(arm, ws_front, ws_side, ws_back, DELTA_T)
def setUp(self):
self.arm = DebraArm.DebraArm(l1=l1, l2=l2)
self.ws_front = Workspace(-1.0,1.0, 0.2,2.0, 0.0,0.2, 1)
self.ws_side = Workspace(0.2,2.0, -1.0,1.0, 0.0,0.2, 1)
self.ws_back = Workspace(-1.0,1.0, -2.0,-0.2, 0.0,0.2, -1)
self.arm_mng = \
ArmManager.ArmManager(self.arm, self.ws_front, self.ws_side, self.ws_back, DELTA_T)
def main():
# Initial robot state
origin_x, origin_y = 0.0, 0.0
arm = DebraArm.DebraArm(l1=L1, l2=L2, flip_x=1)
arm.inverse_kinematics(RobotSpacePoint(0.99 * (L1 + L2), 0, 0, 0))
start = arm.get_tool()
end = RobotSpacePoint(0.0, -1.2, 0.1, GRIPPER_HEADING - pi / 2)
ws_front = Workspace(-1.0, 1.0, abs(L1 - L2), abs(L1 + L2), 0.0, 0.2, 1)
ws_side = Workspace(abs(L1 - L2), abs(L1 + L2), -1.0, 1.0, 0.0, 0.2, 1)
ws_back = Workspace(-1.0, 1.0, -abs(L1 + L2), -abs(L1 - L2), 0.0, 0.2, -1)
arm_manager = ArmManager.ArmManager(arm, ws_front, ws_side, ws_back,
DELTA_T)
# Draw robot
origin, p1, p2, p3, z = arm.get_detailed_pos(L3)
draw_arm(origin, p1, p2, p3, RANGE_MIN, RANGE_MAX)
pygame.display.update()
pth1, pth2, pz, pth3 = \
arm_manager.goto(start, RobotSpacePoint(0,0,0,0),
end, RobotSpacePoint(0,0,0,0),
'line')
graph_trajectory_joint(pth1, pth2, pth3)
draw_trajectory(arm, pth1, pth2, pz, pth3, DELTA_T)
origin, p1, p2, p3, z = arm.get_detailed_pos(L3)
draw_arm(origin, p1, p2, p3, RANGE_MIN, RANGE_MAX)
pygame.display.update()
def test_check_arm_class(self):
arm = Joint.Joint('wrong')
ws = Workspace(0.2,2.0, -1.0,1.0, 0.0,0.2, 1)
ws_front = ws
ws_side = ws
ws_back = ws
with self.assertRaises(ValueError):
arm_mng = ArmManager.ArmManager(arm, ws_front, ws_side, ws_back, DELTA_T)
def setUp(self):
self.arm = DebraArm.DebraArm(l1=l1, l2=l2)
self.ws = Workspace(0.2,2.0, -1.0,1.0, 0.0,0.2, 1)
ws_front = self.ws
ws_side = self.ws
ws_back = self.ws
self.arm_mng = ArmManager.ArmManager(self.arm, ws_front, ws_side, ws_back, DELTA_T)
self.start_pos = RobotSpacePoint(1.0, -0.5, 0.0, 0.0)
self.start_vel = RobotSpacePoint(0.0, 0.0, 0.0, 0.0)
self.target_pos = RobotSpacePoint(1.0, 0.5, 0.0, 0.0)
self.target_vel = RobotSpacePoint(0.0, 0.0, 0.0, 0.0)
def test_init(self):
arm = DebraArm.DebraArm(l1=l1, l2=l2)
ws = Workspace(0.2,2.0, -1.0,1.0, 0.0,0.2, 1)
ws_front = ws
ws_side = ws
ws_back = ws
arm_mng = ArmManager.ArmManager(arm, ws_front, ws_side, ws_back, DELTA_T)
self.assertAlmostEqual(arm_mng.arm, arm)
self.assertAlmostEqual(arm_mng.workspace, ws)
self.assertAlmostEqual(arm_mng.dt, DELTA_T)
def setUp(self):
self.arm = DebraArm.DebraArm(l1=l1, l2=l2)
self.ws = Workspace(0.2,2.0, -1.0,1.0, 0.0,0.2, 1)
ws_front = self.ws
ws_side = self.ws
ws_back = self.ws
self.arm_mng = ArmManager.ArmManager(self.arm, ws_front, ws_side, ws_back, DELTA_T)
self.start_pos = RobotSpacePoint(1.0, -0.5, 0.0, 0.0)
self.start_vel = RobotSpacePoint(0.0, 0.0, 0.0, 0.0)
self.target_pos = RobotSpacePoint(1.0, 0.5, 0.0, 0.0)
self.target_vel = RobotSpacePoint(0.0, 0.0, 0.0, 0.0)
self.start_joints_pos = self.arm.inverse_kinematics(self.start_pos)
self.arm.compute_jacobian_inv()
self.start_joints_vel = self.arm.get_joints_vel(self.start_vel)
self.target_joints_pos = self.arm.inverse_kinematics(self.target_pos)
self.arm.compute_jacobian_inv()
self.target_joints_vel = self.arm.get_joints_vel(self.target_vel)
def main():
"draw loop"
# Initial robot state
origin = Vector3D(-0.5, 0.0, 0.0)
arm = DebraArm.DebraArm(l1=L1, l2=L2, origin=origin, flip_x=-1)
arm.inverse_kinematics(
RobotSpacePoint(-0.99 * (L1 + L2) + origin.x, 0 + origin.y,
0 + origin.z, 0))
tool = arm.get_tool()
joints = arm.get_joints()
ws_front = Workspace(-1.5 + origin.x, 1.5 + origin.x,
abs(L1 - L2) + origin.y,
abs(L1 + L2) + origin.y, 0.0 + origin.z,
0.2 + origin.z, 1)
ws_side = Workspace(-abs(L1 + L2) + origin.x, -abs(L1 - L2) + origin.x,
-1.5 + origin.y, 1.5 + origin.y, 0.0 + origin.z,
0.2 + origin.z, 1)
ws_back = Workspace(-1.5 + origin.x, 1.5 + +origin.x,
-abs(L1 + L2) + origin.y, -abs(L1 - L2) + origin.y,
0.0 + origin.z, 0.2 + origin.y, -1)
arm_manager = ArmManager.ArmManager(arm, ws_front, ws_side, ws_back,
DELTA_T)
# Draw robot
origin, p1, p2, p3, z = arm.get_detailed_pos(L3)
draw_arm(origin, p1, p2, p3, RANGE_MIN, RANGE_MAX)
draw_workspaces(ws_front, ws_side, ws_back)
pygame.display.update()
paused = False
while True:
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
sys.exit()
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_p:
paused = not paused
if event.type == pygame.MOUSEBUTTONUP:
x, y = get_cursor_pos()
tool_prev = arm_manager.arm.get_tool()
tool = RobotSpacePoint(x, y, z, GRIPPER_HEADING)
start_time = time.time()
pth1, pth2, pz, pth3 = arm_manager.goto(
tool_prev, RobotSpacePoint(0, 0, 0, 0), tool,
RobotSpacePoint(0, 0, 0, 0), 'line')
elapsed_time = time.time() - start_time
print('elapsed time: ', elapsed_time)
graph_trajectory_joint(pth1, pth2, pth3)
draw_trajectory(arm_manager.arm, pth1, pth2, pz, pth3, DELTA_T)
if not paused:
SCREEN.fill(BLACK)
origin, p1, p2, p3, z = arm_manager.arm.get_detailed_pos(L3)
draw_arm(origin, p1, p2, p3, RANGE_MIN, RANGE_MAX)
draw_workspaces(ws_front, ws_side, ws_back)
pygame.display.update()