コード例 #1
0
    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)
コード例 #2
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)
コード例 #3
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)
コード例 #4
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)
コード例 #5
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)
コード例 #6
0
 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)
コード例 #7
0
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()
コード例 #8
0
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()
コード例 #9
0
    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)
コード例 #10
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)
コード例 #11
0
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()
コード例 #12
0
    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)
コード例 #13
0
 def setUp(self):
     self.arm = DebraArm.DebraArm(l1=l1, l2=l2)
     self.arm.inverse_kinematics(RobotSpacePoint(0.99 * (l1 + l2), 0, 0, 0))
コード例 #14
0
 def setUp(self):
     self.arm = DebraArm.DebraArm(l1=l1, l2=l2)
コード例 #15
0
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()