def test_fwdkin_origin(self):
"""
Checks that forward kinematics works with origin not at 0
"""
arm = DebraArm.DebraArm(l1=l1, l2=l2, origin=Vector3D(1, 1, 1))
th1 = pi / 2
th2 = pi / 2
z = 0.1
th3 = pi / 2
joints = JointSpacePoint(th1, th2, z, th3)
tool = arm.forward_kinematics(joints)
self.assertAlmostEqual(tool.x, -l2 + 1)
self.assertAlmostEqual(tool.y, l1 + 1)
self.assertAlmostEqual(tool.z, 0.1 + 1)
self.assertAlmostEqual(tool.gripper_hdg, -pi)
th1 = -pi / 2
th2 = pi / 2
z = 0.2
th3 = pi / 2
joints = JointSpacePoint(th1, th2, z, th3)
tool = arm.forward_kinematics(joints)
self.assertAlmostEqual(tool.x, l2 + 1)
self.assertAlmostEqual(tool.y, -l1 + 1)
self.assertAlmostEqual(tool.z, 0.2 + 1)
self.assertAlmostEqual(tool.gripper_hdg, 0.0)
def test_invkin(self):
"""
Checks that inverse kinematics works
"""
arm = DebraArm.DebraArm(l1=l1, l2=l2)
x = l2
y = -l1
z = 0.1
grp_hdg = 0
tool = RobotSpacePoint(x, y, z, grp_hdg)
joints = arm.inverse_kinematics(tool)
self.assertAlmostEqual(joints.theta1, -pi / 2)
self.assertAlmostEqual(joints.theta2, pi / 2)
self.assertAlmostEqual(joints.z, 0.1)
self.assertAlmostEqual(joints.theta3, pi / 2)
x = 0
y = l1 + l2
z = 0.2
grp_hdg = 0
tool = RobotSpacePoint(x, y, z, grp_hdg)
joints = arm.inverse_kinematics(tool)
self.assertAlmostEqual(joints.theta1, pi / 2)
self.assertAlmostEqual(joints.theta2, 0.0)
self.assertAlmostEqual(joints.z, 0.2)
self.assertAlmostEqual(joints.theta3, 0)
def test_fwdkin(self):
"""
Checks that forward kinematics works
"""
arm = DebraArm.DebraArm(l1=l1, l2=l2)
th1 = pi / 2
th2 = pi / 2
z = 0.1
th3 = pi / 2
joints = JointSpacePoint(th1, th2, z, th3)
tool = arm.forward_kinematics(joints)
self.assertAlmostEqual(tool.x, -l2)
self.assertAlmostEqual(tool.y, l1)
self.assertAlmostEqual(tool.z, 0.1)
self.assertAlmostEqual(tool.gripper_hdg, -pi)
th1 = -pi / 2
th2 = pi / 2
z = 0.2
th3 = pi / 2
joints = JointSpacePoint(th1, th2, z, th3)
tool = arm.forward_kinematics(joints)
self.assertAlmostEqual(tool.x, l2)
self.assertAlmostEqual(tool.y, -l1)
self.assertAlmostEqual(tool.z, 0.2)
self.assertAlmostEqual(tool.gripper_hdg, 0.0)
def test_invkin_origin(self):
"""
Checks that inverse kinematics works with origin not at 0
"""
arm = DebraArm.DebraArm(l1=l1, l2=l2, origin=Vector3D(1, 1, 1))
x = 1 + l2
y = 1 - l1
z = 1 + 0.1
grp_hdg = 0
tool = RobotSpacePoint(x, y, z, grp_hdg)
joints = arm.inverse_kinematics(tool)
self.assertAlmostEqual(joints.theta1, -pi / 2)
self.assertAlmostEqual(joints.theta2, pi / 2)
self.assertAlmostEqual(joints.z, 0.1)
self.assertAlmostEqual(joints.theta3, pi / 2)
x = 1
y = 1 + l1 + l2
z = 1 + 0.2
grp_hdg = 0
tool = RobotSpacePoint(x, y, z, grp_hdg)
joints = arm.inverse_kinematics(tool)
self.assertAlmostEqual(joints.theta1, pi / 2)
self.assertAlmostEqual(joints.theta2, 0)
self.assertAlmostEqual(joints.z, 0.2)
self.assertAlmostEqual(joints.theta3, 0)
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 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 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 main():
"draw loop"
# 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))
tool = arm.get_tool()
joints = arm.get_joints()
# 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()
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 = tool
tool = RobotSpacePoint(x, y, z, GRIPPER_HEADING)
if MODE == PLAN_JOINT_SPACE:
start_time = time.time()
pth1, pth2, pz, pth3 = arm.get_path(
tool_prev, RobotSpacePoint(0, 0, 0, 0), tool,
RobotSpacePoint(0, 0, 0, 0), DELTA_T)
elapsed_time = time.time() - start_time
print('elapsed time: ', elapsed_time)
else:
start_time = time.time()
pth1, pth2, pz, pth3, px, py, pz, pgrp = \
arm.get_path_xyz(tool_prev, RobotSpacePoint(0,0,0,0),
tool, RobotSpacePoint(0,0,0,0),
DELTA_T, 'all')
elapsed_time = time.time() - start_time
print('elapsed time: ', elapsed_time)
graph_trajectory_xyz(px, py, pz, pgrp)
graph_trajectory_joint(pth1, pth2, pth3)
draw_trajectory(arm, pth1, pth2, pz, pth3, DELTA_T)
if not paused:
SCREEN.fill(BLACK)
origin, p1, p2, p3, z = arm.get_detailed_pos(L3)
draw_arm(origin, p1, p2, p3, RANGE_MIN, RANGE_MAX)
pygame.display.update()
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 main():
"draw loop"
# Initial robot state
origin_x, origin_y = 0.0, 0.0
arm = DebraArm.DebraArm(l1=L1, l2=L2, flip_x=-1, flip_elbow=1)
tool = arm.get_tool()
joints = arm.get_joints()
# Draw robot
origin, p1, p2, p3, z = arm.get_detailed_pos(L3)
draw_arm(origin, p1, p2, p3)
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 not paused:
SCREEN.fill(BLACK)
(x, y) = pygame.mouse.get_pos()
x = (x - WIDTH / 2) / PX_PER_METER
y = -(y - HEIGHT / 2) / PX_PER_METER
print("cursor: x: ", x, ", y: ", y)
try:
tool = RobotSpacePoint(x, y, z, GRIPPER_HEADING)
joints = arm.inverse_kinematics(tool)
except ValueError:
pass
print("arm: ", "x:", arm.tool.x, "y:", arm.tool.y, \
"th1:", arm.joints.theta1, "th2:", arm.joints.theta2)
# Draw robot
origin, p1, p2, p3, z = arm.get_detailed_pos(L3)
draw_arm(origin, p1, p2, p3)
pygame.display.update()
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 setUp(self):
self.arm = DebraArm.DebraArm(l1=l1, l2=l2)
self.arm.inverse_kinematics(RobotSpacePoint(0.99 * (l1 + l2), 0, 0, 0))
def setUp(self):
self.arm = DebraArm.DebraArm(l1=l1, l2=l2)
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()