scrabble_project.ipynb

{
  "cells": [
    {
      "cell_type": "markdown",
      "source": "# Scrabble-Playing Robot",
      "metadata": {
        "formattedRanges": [],
        "cell_id": "a38f492e144148df8a76e99c34fc4c39",
        "deepnote_cell_type": "text-cell-h1"
      },
      "block_group": "31a29e27267c429e869da1e4f27d0dfe"
    },
    {
      "cell_type": "markdown",
      "source": "## Imports",
      "metadata": {
        "formattedRanges": [],
        "cell_id": "fb15bdf1299049f587f3e8d232eac86b",
        "deepnote_cell_type": "text-cell-h2"
      },
      "block_group": "9c15ea16c0b74ad7a8f98510d20361b6"
    },
    {
      "cell_type": "code",
      "source": "import logging\nfrom copy import copy\nfrom enum import Enum\nimport matplotlib.pyplot as plt\nimport pydot\nfrom IPython.display import HTML, SVG, display, Image\n\nimport numpy as np\nfrom pydrake.all import (\n    AbstractValue,\n    Demultiplexer,\n    DiagramBuilder,\n    InputPortIndex,\n    JointIndex,\n    LeafSystem,\n    Parser,\n    PiecewisePolynomial,\n    PiecewisePose,\n    PointCloud,\n    PortSwitch,\n    RigidTransform,\n    Simulator,\n    StateInterpolatorWithDiscreteDerivative,\n    StartMeshcat,\n    UniformlyRandomRotationMatrix,\n    QuaternionFloatingJoint,\n    Role,\n    AngleAxis,\n    RigidTransform,\n    ModelVisualizer,\n    MeshcatVisualizer\n)\nfrom pydrake.math import (\n    RotationMatrix\n)\n\nfrom manipulation import running_as_notebook\nfrom manipulation.meshcat_utils import AddMeshcatTriad\nfrom manipulation.pick import MakeGripperPoseTrajectory\nfrom manipulation.scenarios import AddIiwaDifferentialIK, ycb\nfrom manipulation.station import (\n    MakeHardwareStation,\n    load_scenario,\n)\nimport importlib\n\n\nclass NoDiffIKWarnings(logging.Filter):\n    def filter(self, record):\n        return not record.getMessage().startswith(\"Differential IK\")\n\n\nlogging.getLogger(\"drake\").addFilter(NoDiffIKWarnings())",
      "metadata": {
        "id": "eeMrMI0-1Dhu",
        "colab": {},
        "colab_type": "code",
        "source_hash": null,
        "execution_start": 1702338087032,
        "execution_millis": 1820,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "d2bee9831abe44da8446dccfb4dfacda",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "6a2c37c5b54548b386f8f3bdea919cff"
    },
    {
      "cell_type": "code",
      "source": "import constants\nfrom constants import *\nimportlib.reload(constants)\n\nimport letter_tree\nimport board\nimport solver\nimport scorer\nimport holder\nimport bag\n\nimportlib.reload(letter_tree)\nimportlib.reload(board)\nimportlib.reload(solver)\nimportlib.reload(scorer)\nimportlib.reload(holder)\nimportlib.reload(bag)\n\nfrom board import Board\nfrom solver import SolveState\nfrom holder import Holder\nfrom letter_tree import basic_english\nfrom bag import *",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338088857,
        "execution_millis": 6914,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "ac49a98924494aeea7892861cf080e97",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "f7c347c1c96a44498418da81856033f2"
    },
    {
      "cell_type": "code",
      "source": "meshcat = StartMeshcat()",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338095781,
        "execution_millis": 74,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "a4998f1f92864909a408f820043cad0e",
        "deepnote_cell_type": "code"
      },
      "outputs": [
        {
          "name": "stderr",
          "text": "INFO:drake:Meshcat listening for connections at https://4b17a40c-416d-4160-9e25-b767157b933d.deepnoteproject.com/7000/\n",
          "output_type": "stream"
        },
        {
          "data": {
            "text/plain": "<IPython.core.display.HTML object>",
            "text/html": "Meshcat URL: <a href='https://4b17a40c-416d-4160-9e25-b767157b933d.deepnoteproject.com/7000/' target='_blank'>https://4b17a40c-416d-4160-9e25-b767157b933d.deepnoteproject.com/7000/</a>"
          },
          "metadata": {},
          "output_type": "display_data"
        }
      ],
      "execution_count": null,
      "block_group": "95d9b62da2f24289b5859ec6851bff16"
    },
    {
      "cell_type": "markdown",
      "source": "## Game Constants",
      "metadata": {
        "formattedRanges": [],
        "cell_id": "d8459578786a427495ba37e4a700c5e0",
        "deepnote_cell_type": "text-cell-h2"
      },
      "block_group": "8d5c6bc5f1f043c9a103275d7de3719f"
    },
    {
      "cell_type": "code",
      "source": "BOARD_SIZE = 5\nPIECES_IN_HOLDER = 3\nWORD_DICTIONARY = basic_english()\nz = 0.0026 # height of tile\ndelta = 0.02 # distance between adjacent board positions\n\n# location to teleport preloaded pieces from - just a placeholder location\nbag_loc = [0.2, 0.2, 0]\n\n# location to pickup randomly generated tile when refilling the holder\ngeneration_loc = [-0.2, -0.2, 0.0]\n\ntile_to_count = tile_to_count_tiny\n# tile_to_count = tile_to_count_small",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338095850,
        "execution_millis": 289,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "e7deb5dd30304d72ad13a81eea5b60a2",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "54374801bdbd42869f7a6d33db5f017e"
    },
    {
      "cell_type": "code",
      "source": "def fill_bag():\n    pos_to_id = dict()\n    return Bag(tile_to_count)\n\ndef get_tile_body_name(tile):\n    return f\"{tile}1\"\n\ndef get_tile_instance_name(tile, tile_id):\n    return f\"{tile}{tile_id}\"\n\ndef get_tile_joint_name(tile, tile_id):\n    return f\"{tile}{tile_id}_joint\"",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338096144,
        "execution_millis": 51,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "a1c0844634a142cea2b99ee4ca6d1923",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "c46124ad1ea84adca21c9ab900b6e947"
    },
    {
      "cell_type": "markdown",
      "source": "## Transformations",
      "metadata": {
        "formattedRanges": [],
        "cell_id": "2bba6607d87e4b96a7c7759658a9a22e",
        "deepnote_cell_type": "text-cell-h2"
      },
      "block_group": "cca900ac5b264c698b15f587e656af57"
    },
    {
      "cell_type": "code",
      "source": "def board_to_world(board_state, pos, zi=0.0):\n    \"\"\"\n    Gets the pose of a board piece with respect to the world.\n\n    Parameters:\n    - board_state (Board): The Board\n    - pos (tuple): The (row, col) coordinates of the piece in the internal board state\n    - zi (int): The height of the piece\n\n    Returns:\n    - RigidBodyTransform: The transform of the board piece with respect to the world.\n    \"\"\"\n    TAG = \"board_to_world\"\n    shifted = (np.array(board_state.get_center_pos()) - np.array(pos)) * delta\n    X_WP = RigidTransform(RotationMatrix(), [shifted[0], shifted[1], zi])\n    # print(f\"{TAG}: pos: {pos} and shifted: {shifted}\")\n    return X_WP\n\n\ndef holder_to_world(idx, zi=0.0):\n    \"\"\"\n    Gets the pose of the holder piece with respect to the world.\n\n    Parameters:\n    - idx (int): The position of the piece in the internal holder state\n    - zi (int): The height of the piece\n\n    Returns:\n    - RigidBodyTransform: The transform of the holder piece with respect to the world.\n    \"\"\"\n    TAG = \"holder_to_world\"\n    x = -10 * delta\n    y = idx * delta\n    X_WP = RigidTransform(RotationMatrix(), [x, y, zi])\n    # print(f\"{TAG} pos: ({x},{y}) and idx: {idx}\")\n    return X_WP\n\n\ndef bag_to_world(i, j, zi=0.0):\n    \"\"\"\n    Gets the pose of a bag piece with respect to the world.\n\n    Parameters:\n    - i (int): The row coordinate of the piece in the bag\n    - j (int): The col coordinate of the piece in the bag\n    - zi (int): The height of the piece\n\n    Returns:\n    - RigidBodyTransform: The transform of the bag piece with respect to the world.\n    \"\"\"\n    x = -0.25 + delta * j\n    y = -0.2 - delta * i\n    X_WP = RigidTransform(RotationMatrix(), [x, y, zi])\n    return X_WP\n\n\ndef Get_X_SP(X_WS, X_WP):\n    \"\"\"\n    Gets the pose of a piece with respect to the suction tip link.\n\n    Parameters:\n    - X_WS (RigidBodyTransform): The transform of the tip link with respect to the world\n    - X_WP (RigidBodyTransform): The transform of the piece with respect to the world\n\n    Returns:\n    - RigidBodyTransform: The transform of the piece with respect to the tip link.\n    \"\"\"\n    return RigidTransform.multiply(X_WS.inverse(), X_WP)",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338096159,
        "execution_millis": 35,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "b0e9578f2eab46d4be6c88225848a249",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "d86e2cf1700e4c4b89afeee8780f1689"
    },
    {
      "cell_type": "markdown",
      "source": "## Create Scene with Initial Board and Holder States",
      "metadata": {
        "formattedRanges": [],
        "cell_id": "bf350085f92343c7977c147d98c03a8d",
        "deepnote_cell_type": "text-cell-h2"
      },
      "block_group": "788a2683f5344e7d9182d2354c4f34b9"
    },
    {
      "cell_type": "code",
      "source": "def create_scene():\n    '''\n    Writes the scenario data for the initial scene of the board. This includes:\n    1. iiwa robot with a camera welded to the seventh joint\n    2. Table to play the game\n    3. Scrabble Board\n    4. Holder for the the letters in play\n    \n    Additionally, all the letters are loaded in with the locations defined by `locations`\n\n    Returns the scenario data (text) for the Scrabble game\n    '''\n    scenario_data = f\"\"\"\ndirectives: \n- add_model:\n    name: iiwa\n    file: package://drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf\n    default_joint_positions:\n        iiwa_joint_1: [0]       # Base joint - rotate the base to face the board\n        iiwa_joint_2: [0.1]     # Slight lift of the second joint to lower the end effector\n        iiwa_joint_3: [0]       # Keep the third joint straight for now\n        iiwa_joint_4: [-1.2]     # Slight bend of the fourth joint to bring the arm closer to the board\n        iiwa_joint_5: [0]       # Keep the fifth joint straight for now\n        iiwa_joint_6: [1.6]    # Bend the sixth joint to position the end effector downwards\n        iiwa_joint_7: [0] \n- add_weld:\n    parent: world\n    child: iiwa::iiwa_link_0\n    X_PC:\n        translation: [-0.6, 0, 0]\n- add_model:\n    name: suction_base\n    file: package://manipulation/suction/suction-base.urdf\n- add_weld:\n    parent: iiwa::iiwa_link_7\n    child: suction_base::baseLink\n    X_PC:\n        translation: [0, 0, 0.01]\n        rotation: !Rpy {{ deg: [0, 0, 0] }}\n- add_model:\n    name: suction_head\n    file: file:///work/models/suction_head.urdf\n- add_weld:\n    parent: iiwa::iiwa_link_7\n    child: suction_head::headLink\n    X_PC:\n        translation: [0, 0, 0.10]\n        rotation: !Rpy {{ deg: [0, 0, 0] }}\n- add_model:\n    name: work_table\n    file: file:///work/models/extra_heavy_duty_table_surface_only_collision.sdf\n- add_weld:\n    parent: world\n    child: work_table::link\n    X_PC:\n        translation: [0, 0, -0.7645]\n- add_model:\n    name: board\n    file: file:///work/models/scrabble_board.sdf\n    default_free_body_pose:\n        scrabble_board: # Change here!\n            translation: [0,0,0]\n            rotation: !Rpy {{ deg: [0, 0, 0] }} \n- add_weld:\n    parent: world\n    child: scrabble_board\n    X_PC:\n        translation: [0, 0, 0]\n        rotation: !Rpy {{ deg: [0, 0, 0] }}\n\"\"\"\n\n    for tile, tile_id in bag_state.get_all_tiles():\n        scenario_data += f\"\"\"\n- add_model:\n    name: {tile}{str(tile_id)}\n    file: file:///work/models/{tile}1.sdf\n    default_free_body_pose:\n        {tile}1: # Change here!\n            translation: {bag_loc}\n            rotation: !Rpy {{ deg: [0, 0, 0] }}  \n\"\"\"\n\n    scenario_data += f\"\"\"\nmodel_drivers:\n    iiwa+suction_base+suction_head: !InverseDynamicsDriver {{}}\n\"\"\"\n    return scenario_data",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338096175,
        "execution_millis": 57,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "15600a01491045deb9d71ff85b032b2b",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "00e8679e515d494392295c063488fd49"
    },
    {
      "cell_type": "code",
      "source": "def teleport_piece(X, tile, tile_id, plant, plant_context):\n    \"\"\"\n    Teleport tile to a given pose.\n    \"\"\"\n    TAG = \"teleport_piece\"\n    body_name = get_tile_body_name(tile)\n    instance_name = get_tile_instance_name(tile, tile_id)\n    plant.SetFreeBodyPose(\n        plant_context,\n        plant.GetBodyByName(body_name, plant.GetModelInstanceByName(instance_name)),\n        X)\n    # print(f\"{TAG}: Teleporting tile '{instance_name}' to {X_WP.translation()}.\")",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338096229,
        "execution_millis": 22,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "d4ec086e31ff4701a8cb488cb0e13019",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "c4ae8233079247f59c9e8da5bc760370"
    },
    {
      "cell_type": "code",
      "source": "def set_board_state(board_state: Board, station, context, plant, plant_context, X_WS):\n    \"\"\"\n    Used at the beginning of the game to set the board.\n    Given the current board, place the scrabble pieces on the board.\n    \"\"\"\n    TAG = \"set_board_state\"\n\n    for pos in board_state.all_positions():\n\n        if board_state.is_empty(pos):\n            continue\n\n        tile = board_state.get_tile(pos)\n        tile_id = pos_to_id[pos]\n\n        # transform of the piece with respect to the world\n        transform = board_to_world(board_state, pos, z)\n        X_WP = RigidTransform(transform.rotation().MakeZRotation(-np.pi/2), transform.translation())\n        X_SP = Get_X_SP(X_WS, X_WP)\n        joint_name = get_tile_joint_name(tile, tile_id)\n        instance_name = get_tile_instance_name(tile, tile_id)\n        model_joint = plant.GetJointByName(joint_name)\n        model_joint.SetDefaultPose(RigidTransform([0, 0, 0.1]))\n        model_joint.Unlock(plant_context)\n        teleport_piece(X_SP, tile, tile_id, plant, plant_context)\n        # print(f\"{TAG}: Set pose of {instance_name} to {X_SP.translation()}.\")",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338096240,
        "execution_millis": 18,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "9522b8ba8dc1487e88849cb4ef0870cc",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "7fc30ddbf4904a7b932b63d7c954a0b0"
    },
    {
      "cell_type": "code",
      "source": "def set_holder_state(holder_state, station, context, plant, plant_context, X_WS):\n    \"\"\"\n    Used at the beginning of the game to set up the holder.\n    \"\"\"\n    TAG = \"set_holder_state\"\n\n    for i in holder_state.get_filled_positions():\n        transform = holder_to_world(i, z)\n        X_WP = RigidTransform(transform.rotation().MakeZRotation(-np.pi/2), transform.translation())\n        X_SP = Get_X_SP(X_WS, X_WP)\n        tile, tile_id = holder_state.get_tile(i)\n        joint_name = get_tile_joint_name(tile, tile_id)\n        instance_name = get_tile_instance_name(tile, tile_id)\n        model_joint = plant.GetJointByName(joint_name)\n        model_joint.SetDefaultPose(RigidTransform([0, 0, 0.1]))\n        model_joint.Unlock(plant_context)\n        teleport_piece(X_SP, tile, tile_id, plant, plant_context)\n        # print(f\"{TAG}: Set pose of {instance_name} to {X_SP.translation()}.\")",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338096245,
        "execution_millis": 22,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "d9087d3fe01341bc93bb2ac69c63b134",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "3a2c5374030f4e51aceb14ea19c0dd12"
    },
    {
      "cell_type": "code",
      "source": "def set_bag_state(bag_state, station, context, plant, plant_context, X_WS):\n    \"\"\"\n    Teleport bag pieces to correct locations\n    \"\"\"\n    TAG = \"set_bag_state\"\n    l = bag_state.get_all_tiles()\n\n    for tile, tile_id in bag_state.get_all_tiles():\n\n        bag_pos = bag_state.tile_to_pos[(tile, tile_id)]\n        i, j = bag_pos\n        transform = bag_to_world(i, j, z)\n        X_WP = RigidTransform(transform.rotation().MakeZRotation(-np.pi/2), transform.translation())\n        X_SP = Get_X_SP(X_WS, X_WP)\n        joint_name = get_tile_joint_name(tile, tile_id)\n        instance_name = get_tile_instance_name(tile, tile_id)\n        model_joint = plant.GetJointByName(joint_name)\n        model_joint.SetDefaultPose(RigidTransform([0, 0, 0.1]))\n        model_joint.Unlock(plant_context)\n        teleport_piece(X_SP, tile, tile_id, plant, plant_context)\n        # print(f\"{TAG}: Set pose of {instance_name} to {X_SP.translation()}.\")",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338096308,
        "execution_millis": 16,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "c55feba35e1a4636a1994818cf43ca63",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "dd1d2e646cad4463a6b2eed19997327a"
    },
    {
      "cell_type": "markdown",
      "source": "## PlannerState Enum",
      "metadata": {
        "formattedRanges": [],
        "cell_id": "df939bf5d6ff4dfe949369750b1d284b",
        "deepnote_cell_type": "text-cell-h2"
      },
      "block_group": "d8a3c2489bb943bb88f12153a1be7555"
    },
    {
      "cell_type": "code",
      "source": "class PlannerState(Enum):\n    \"\"\"\n    Represents different states of the scrabble game.\n    \"\"\"\n    CREATE_WORD = 1\n    PLAN_MOVE_TO_PICKUP_IN_HOLDER = 2\n    MOVE_TO_PICKUP_IN_HOLDER = 3\n    PICKUP_IN_HOLDER = 4\n    MOVE_TO_DROP_OFF_ON_BOARD = 5\n    DROP_OFF_ON_BOARD = 6\n\n    GENERATE_PIECE = 7\n    MOVE_TO_PICKUP_IN_GENERATION = 8\n    PICKUP_IN_GENERATION = 9\n    MOVE_TO_DROP_OFF_IN_HOLDER = 10\n    DROP_OFF_IN_HOLDER = 11\n\n    END_GAME = 12",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338096309,
        "execution_millis": 16,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "f5f44a9cffa94d07bbedcc505de560dc",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "7bd88daa1c804a79b0033c83140d4714"
    },
    {
      "cell_type": "markdown",
      "source": "## Initialize Game Variables",
      "metadata": {
        "formattedRanges": [],
        "cell_id": "73f0d39eb4c14d42b4358420ceb6365b",
        "deepnote_cell_type": "text-cell-h2"
      },
      "block_group": "dac3f661b74943c19b5a08029ac4bc76"
    },
    {
      "cell_type": "code",
      "source": "rng = np.random.default_rng(135)  # this is for python\n\npos_to_id = dict() # maps positions of tiles on board to their ids\nbag_state: Bag = fill_bag()",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338096356,
        "execution_millis": 58,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "11d3e3c145be449d9c1dde24e38c400a",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "dc06e84ddfc74e43a39468f561e52f70"
    },
    {
      "cell_type": "code",
      "source": "def MakeGripperFrames(X_G, t0=0):\n    \"\"\"\n    Takes a partial specification with X_G[\"initial\"], X_G[\"pick\"], and\n    X_G[\"place\"], and returns a X_G and times with all of the pick and place\n    frames populated.\n    \"\"\"\n    # pregrasp is negative y in the gripper frame (see the figure!).\n    X_GgraspGpregrasp = RigidTransform([0, 0, -0.2])\n\n    X_G[\"prepick\"] = X_G[\"pick\"] @ X_GgraspGpregrasp\n    X_G[\"preplace\"] = X_G[\"place\"] @ X_GgraspGpregrasp\n\n    # I'll interpolate a halfway orientation by converting to axis angle and\n    # halving the angle.\n    X_GinitialGprepick = X_G[\"initial\"].inverse() @ X_G[\"prepick\"]\n    angle_axis = X_GinitialGprepick.rotation().ToAngleAxis()\n    X_GinitialGprepare = RigidTransform(\n        AngleAxis(angle=angle_axis.angle() / 2.0, axis=angle_axis.axis()),\n        X_GinitialGprepick.translation() / 2.0,\n    )\n    X_G[\"prepare\"] = X_G[\"initial\"] @ X_GinitialGprepare\n    p_G = np.array(X_G[\"prepare\"].translation())\n    p_G[2] = 0.5\n    # To avoid hitting the cameras, make sure the point satisfies x - y < .5\n    if p_G[0] - p_G[1] < 0.5:\n        scale = 0.5 / (p_G[0] - p_G[1])\n        p_G[:1] /= scale\n    X_G[\"prepare\"].set_translation(p_G)\n\n    X_GprepickGpreplace = X_G[\"prepick\"].inverse() @ X_G[\"preplace\"]\n    angle_axis = X_GprepickGpreplace.rotation().ToAngleAxis()\n    X_GprepickGclearance = RigidTransform(\n        AngleAxis(angle=angle_axis.angle() / 2.0, axis=angle_axis.axis()),\n        X_GprepickGpreplace.translation() / 2.0,\n    )\n    X_G[\"clearance\"] = X_G[\"prepick\"] @ X_GprepickGclearance\n    p_G = np.array(X_G[\"clearance\"].translation())\n    p_G[2] = 0.5\n    # To avoid hitting the cameras, make sure the point satisfies x - y < .5\n    if p_G[0] - p_G[1] < 0.5:\n        scale = 0.5 / (p_G[0] - p_G[1])\n        p_G[:1] /= scale\n    X_G[\"clearance\"].set_translation(p_G)\n\n    # Now let's set the timing\n    times = {\"initial\": t0}\n    prepare_time = 10.0 * np.linalg.norm(X_GinitialGprepare.translation())\n    times[\"prepare\"] = times[\"initial\"] + prepare_time\n    times[\"prepick\"] = times[\"prepare\"] + prepare_time\n    # Allow some time for the gripper to close.\n    times[\"pick_start\"] = times[\"prepick\"] + 2.0\n    times[\"pick_end\"] = times[\"pick_start\"] + 2.0\n    X_G[\"pick_start\"] = X_G[\"pick\"]\n    X_G[\"pick_end\"] = X_G[\"pick\"]\n    times[\"postpick\"] = times[\"pick_end\"] + 2.0\n    X_G[\"postpick\"] = X_G[\"prepick\"]\n    time_to_from_clearance = 10.0 * np.linalg.norm(\n        X_GprepickGclearance.translation()\n    )\n    times[\"clearance\"] = times[\"postpick\"] + time_to_from_clearance\n    times[\"preplace\"] = times[\"clearance\"] + time_to_from_clearance\n    times[\"place_start\"] = times[\"preplace\"] + 2.0\n    times[\"place_end\"] = times[\"place_start\"] + 2.0\n    X_G[\"place_start\"] = X_G[\"place\"]\n    X_G[\"place_end\"] = X_G[\"place\"]\n    times[\"postplace\"] = times[\"place_end\"] + 2.0\n    X_G[\"postplace\"] = X_G[\"preplace\"]\n\n    return X_G, times\n",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338096357,
        "execution_millis": 56,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "17472bc3c9ee493d94a9e8abd6481a1e",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "b382953e7a06406d8a823faab48f3bd0"
    },
    {
      "cell_type": "markdown",
      "source": "## Planner Class",
      "metadata": {
        "formattedRanges": [],
        "cell_id": "4d538e3cdd7643d09a340454132ca1a7",
        "deepnote_cell_type": "text-cell-h2"
      },
      "block_group": "b3225da30d7c42aeb92366154688a365"
    },
    {
      "cell_type": "code",
      "source": "class Planner(LeafSystem):\n\n    def __init__(self, plant, bag_state: Bag, board_state: Board, holder_state: Holder):\n        LeafSystem.__init__(self)\n\n        self.bag_state = bag_state\n        self.board_state = board_state\n        self.holder_state = holder_state\n        self.overall_score = 0\n        self.word = ''\n        self.letters_to_add = []\n        self.word_score = 0\n\n        self.position_in_holder = 0\n        self.drop_off_pos_board = None\n        self.drop_off_pos_holder = None\n\n        self.attached_tile = None\n        self.tile_to_attach = None\n        \n        self._gripper_body_index = plant.GetBodyByName(\"tipLink\").index()\n        self.DeclareAbstractInputPort(\n            \"body_poses\", AbstractValue.Make([RigidTransform()])\n        )\n\n        self._mode_index = self.DeclareAbstractState(\n            AbstractValue.Make(PlannerState.CREATE_WORD)\n        )\n        self._traj_X_G_index = self.DeclareAbstractState(\n            AbstractValue.Make(PiecewisePose())\n        )\n        self._times_index = self.DeclareAbstractState(\n            AbstractValue.Make({\"initial\": 0.0})\n        )\n        self._attempts_index = self.DeclareDiscreteState(1)\n\n        self.DeclareAbstractOutputPort(\n            \"X_WG\",\n            lambda: AbstractValue.Make(RigidTransform()),\n            self.CalcGripperPose,\n        )\n        port = self.DeclareVectorOutputPort(\"suction_on\", 1, self.DoCalcOutput)\n        port.disable_caching_by_default()\n\n        # For GoHome mode.\n        num_positions = 7\n        self._iiwa_position_index = self.DeclareVectorInputPort(\n            \"iiwa_position\", num_positions\n        ).get_index()\n        self.DeclareAbstractOutputPort(\n            \"control_mode\",\n            lambda: AbstractValue.Make(InputPortIndex(0)),\n            self.CalcControlMode,\n        )\n        self.DeclareAbstractOutputPort(\n            \"reset_diff_ik\",\n            lambda: AbstractValue.Make(False),\n            self.CalcDiffIKReset,\n        )\n        self._q0_index = self.DeclareDiscreteState(num_positions)  # for q0\n        self._traj_q_index = self.DeclareAbstractState(\n            AbstractValue.Make(PiecewisePolynomial())\n        )\n        self.DeclareVectorOutputPort(\n            \"iiwa_position_command\", num_positions, self.CalcIiwaPosition\n        )\n        self.DeclareInitializationDiscreteUpdateEvent(self.Initialize)\n\n        self.DeclarePeriodicUnrestrictedUpdateEvent(0.1, 0.0, self.Update)\n\n\n    def GamePlay(self):\n        \"\"\"\n        Call Scrabble board algorithm with the holder pieces and board state to find valid words, \n        and from those words find the highest scoring word.\n\n        Returns:\n        - tuple: The word to play (str), letters and positions (list), and the score of the word (int).\n        \"\"\"\n        TAG = \"game_play\"\n        solver = SolveState(WORD_DICTIONARY, self.board_state, self.holder_state.get_just_tiles())\n        highest_score_word = scorer.get_highest_score_word(solver)\n\n        if highest_score_word is not None:\n            word, positions, word_score = highest_score_word\n            print(f\"{TAG}: Found word '{word}' with positions {positions} and score {word_score}\")\n        else:\n            print(f\"{TAG}: TODO\") # could not find a word so need to replace rack\n            return False\n\n        letters_to_add = [(letter, pos) for letter, pos in zip(word, positions) if self.board_state.is_empty(pos)]\n        return word, letters_to_add, word_score\n\n\n    def MakeTrajectory(self, X_G, context, state):\n        \"\"\"\n        Composes a trajectory for the iiwa given a dictionary of RigidBodyTransforms.\n        \"\"\"\n        X_G, times = MakeGripperFrames(X_G, t0=context.get_time())\n        state.get_mutable_abstract_state(int(self._times_index)).set_value(\n            times\n        )\n        if False:  # Useful for debugging\n            for key in X_G:\n                AddMeshcatTriad(meshcat, key, X_PT=X_G[key])\n        traj_X_G = MakeGripperPoseTrajectory(X_G, times)\n        state.get_mutable_abstract_state(int(self._traj_X_G_index)).set_value(traj_X_G)\n\n\n    def Update(self, context, state):\n        \"\"\"\n        Periodically called update function that runs the game loop.\n        \"\"\"\n\n        mode = context.get_abstract_state(int(self._mode_index)).get_value()\n        current_time = context.get_time()\n        times = context.get_abstract_state(int(self._times_index)).get_value()\n        suction_space = 0\n        suction_pickup = [0,0,0.023]\n        suction_dropoff = [0,0,0.024]\n        R = RotationMatrix().MakeYRotation(np.pi)\n\n        print(f\"mode: {mode}\")\n        print(f\"current time: {current_time}\")\n        print(f\"holder: {self.holder_state}\")\n        print(f\"board: \\n{self.board_state}\")\n        print(f\"times: {times}\")\n\n        # find best word with scrabble solver\n        if mode == PlannerState.CREATE_WORD:\n            result = self.GamePlay()\n\n            if not result:\n                mode = PlannerState.END_GAME\n            else:\n                self.word, self.letters_to_add, self.word_score = result\n                mode = PlannerState.PLAN_MOVE_TO_PICKUP_IN_HOLDER\n        \n        elif mode == PlannerState.PLAN_MOVE_TO_PICKUP_IN_HOLDER:\n            tile, board_position = self.letters_to_add.pop()\n            self.loc_in_holder, tile_id = self.holder_state.get_tile_position(tile)\n            self.drop_off_pos_board = board_position\n        \n            initial_pose = self.get_input_port(0).Eval(context)[\n                int(self._gripper_body_index)\n            ]\n            \n            X_G = {\n                \"initial\": initial_pose, \n                \"pick\": RigidTransform(R, holder_to_world(self.loc_in_holder, suction_space).translation() + suction_pickup),\n                \"place\": RigidTransform(R, board_to_world(self.board_state, self.drop_off_pos_board, suction_space).translation() + suction_dropoff)\n            }\n            self.MakeTrajectory(X_G, context, state)\n            self.tile_to_attach = (tile, tile_id)\n            mode = PlannerState.MOVE_TO_PICKUP_IN_HOLDER\n\n        # moves to pickup location in holder\n        elif mode == PlannerState.MOVE_TO_PICKUP_IN_HOLDER:\n            if context.get_time() > times[\"pick_start\"]:\n                mode = PlannerState.PICKUP_IN_HOLDER\n        \n        # picks up piece and moves to drop off on board\n        elif mode == PlannerState.PICKUP_IN_HOLDER:\n            self.attached_tile = self.tile_to_attach\n            self.tile_to_attach = None\n            self.holder_state.delete_tile(self.loc_in_holder)\n            mode = PlannerState.MOVE_TO_DROP_OFF_ON_BOARD\n        \n        # picks up piece and moves to drop off on board\n        elif mode == PlannerState.MOVE_TO_DROP_OFF_ON_BOARD:\n            if context.get_time() > times[\"place_start\"]:\n                mode = PlannerState.DROP_OFF_ON_BOARD\n\n        # drops piece, moves from board drop off to neutral position\n        elif mode == PlannerState.DROP_OFF_ON_BOARD:\n            if context.get_time() > times[\"postplace\"]:\n                if len(self.letters_to_add) == 0 and not self.bag_state.is_empty():\n                    mode = PlannerState.GENERATE_PIECE\n                    self.overall_score += self.word_score\n                elif len(self.letters_to_add) > 0:\n                    mode = PlannerState.PLAN_MOVE_TO_PICKUP_IN_HOLDER\n                else: \n                    mode = PlannerState.END_GAME\n            self.board_state.set_tile(self.drop_off_pos_board, self.attached_tile[0]) # added\n        \n        # generates piece and teleports to generation location\n        elif mode == PlannerState.GENERATE_PIECE:\n            \n            tile, tile_id = self.bag_state.draw_tile()\n            body_name = get_tile_body_name(tile)\n            instance_name = get_tile_instance_name(tile, tile_id)\n            self.tile_to_attach = (tile, tile_id)\n\n            initial_pose = self.get_input_port(0).Eval(context)[\n                int(self._gripper_body_index)\n            ]\n\n            pos_i, pos_j = self.bag_state.tile_to_pos[(tile, tile_id)]\n            generation_loc = bag_to_world(pos_i, pos_j, suction_space)\n\n            empty_positions = self.holder_state.get_empty_positions()\n            pos_idx = empty_positions[0]\n            self.drop_off_pos_holder = pos_idx\n            \n            X_G = {\n                \"initial\": initial_pose, \n                \"pick\": RigidTransform(R, generation_loc.translation() + suction_pickup),\n                \"place\": RigidTransform(R, holder_to_world(pos_idx, suction_space).translation() + suction_dropoff)\n            }\n\n            self.MakeTrajectory(X_G, context, state)\n            mode = PlannerState.MOVE_TO_PICKUP_IN_GENERATION\n\n        # picks up piece, and moves to dropoff in holder\n        elif mode == PlannerState.MOVE_TO_PICKUP_IN_GENERATION:\n            if context.get_time() >= times[\"pick_start\"]:\n                mode = PlannerState.PICKUP_IN_GENERATION\n        \n        elif mode == PlannerState.PICKUP_IN_GENERATION:\n            self.attached_tile = self.tile_to_attach\n            self.tile_to_attach = None\n            mode = PlannerState.MOVE_TO_DROP_OFF_IN_HOLDER\n\n        elif mode == PlannerState.MOVE_TO_DROP_OFF_IN_HOLDER:\n            if context.get_time() >= times[\"place_start\"]:\n                mode = PlannerState.DROP_OFF_IN_HOLDER\n\n        # drops piece, moves from dropoff in holder to neutral position\n        elif mode == PlannerState.DROP_OFF_IN_HOLDER:\n            if context.get_time() > times[\"postplace\"]:\n                tile, tile_id = self.attached_tile\n                self.holder_state.set_tile(tile, tile_id, self.drop_off_pos_holder)\n                self.attached_tile = None\n                if self.holder_state.is_full():\n                    mode = PlannerState.CREATE_WORD\n                elif not self.holder_state.is_full() and not self.bag_state.is_empty():\n                    mode = PlannerState.GENERATE_PIECE\n                else: \n                    print(\"No more plays left to play\")\n                    mode = PlannerState.END_GAME\n        else:\n            print(f\"Game over! Your final score is {self.overall_score}. Please press `esc` to stop the simulation.\")\n        \n        state.get_mutable_abstract_state(int(self._mode_index)).set_value(mode)\n\n\n    def start_time(self, context):\n        return (\n            context.get_abstract_state(int(self._traj_X_G_index))\n            .get_value()\n            .start_time()\n        )\n\n\n    def end_time(self, context):\n        return (\n            context.get_abstract_state(int(self._traj_X_G_index))\n            .get_value()\n            .end_time()\n        )\n\n\n    def CalcGripperPose(self, context, output):\n        context.get_abstract_state(int(self._mode_index)).get_value()\n\n        traj_X_G = context.get_abstract_state(\n            int(self._traj_X_G_index)\n        ).get_value()\n        if traj_X_G.get_number_of_segments() > 0 and traj_X_G.is_time_in_range(\n            context.get_time()\n        ):\n            # Evaluate the trajectory at the current time, and write it to the\n            # output port.\n            output.set_value(\n                context.get_abstract_state(int(self._traj_X_G_index))\n                .get_value()\n                .GetPose(context.get_time())\n            )\n            return\n        \n        # Command the current position (note: this is not particularly good if the velocity is non-zero)\n        output.set_value(\n            self.get_input_port(0).Eval(context)[int(self._gripper_body_index)]\n        )\n\n\n    def DoCalcOutput(self, context, output):\n        mode = context.get_abstract_state(int(self._mode_index)).get_value()\n\n        if mode == PlannerState.PICKUP_IN_HOLDER \\\n            or mode == PlannerState.MOVE_TO_DROP_OFF_ON_BOARD \\\n            or mode == PlannerState.PICKUP_IN_GENERATION \\\n            or mode == PlannerState.MOVE_TO_DROP_OFF_IN_HOLDER:\n            output.SetAtIndex(0, 1)\n\n        else:\n            output.SetAtIndex(0, 0)\n\n\n    def CalcControlMode(self, context, output):\n        mode = context.get_abstract_state(int(self._mode_index)).get_value()\n\n        output.set_value(InputPortIndex(1))  # Diff IK\n\n\n    def CalcDiffIKReset(self, context, output):\n        mode = context.get_abstract_state(int(self._mode_index)).get_value()\n\n        output.set_value(False)\n\n\n    def Initialize(self, context, discrete_state):\n        discrete_state.set_value(\n            int(self._q0_index),\n            self.get_input_port(int(self._iiwa_position_index)).Eval(context),\n        )\n\n\n    def CalcIiwaPosition(self, context, output):\n        traj_q = context.get_mutable_abstract_state(\n            int(self._traj_q_index)\n        ).get_value()\n\n        output.SetFromVector(traj_q.value(context.get_time()))",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338096388,
        "execution_millis": 52,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "da65cbd1209d4bc0a5a0f86e821114aa",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "2df46c8f34a940f2b6610ad1a67c006e"
    },
    {
      "cell_type": "markdown",
      "source": "## Run game",
      "metadata": {
        "formattedRanges": [],
        "cell_id": "5fdf11b8e63d4981b608c27aab148d50",
        "deepnote_cell_type": "text-cell-h2"
      },
      "block_group": "f043eb5125ad4346baca20d943302a05"
    },
    {
      "cell_type": "code",
      "source": "def set_up_sample_game(bag_state):\n    \"\"\"\n    Sets up a sample game of:\n    - A 7x7 game board with 'a' initialized in the center\n    - A holder of size 3 initialized with the letters 't' and 'h'\n    - A bag with the letters 'n' and 'o'\n    \"\"\"\n\n    # initialize empty board\n    board_size = 7\n    board_state = Board(board_size)\n\n    # add start tiles to board\n    board_tiles = [\"a\"]\n    board_positions = [(3, 3)]\n\n    for start_tile, start_pos in zip(board_tiles, board_positions):\n        board_state.set_tile(start_pos, start_tile)\n        idxb = bag_state.remove_tile(start_tile)\n        pos_to_id[start_pos] = idxb\n\n    # add holder tiles\n    holder_tiles = [\"t\", \"h\"]\n    assert len(holder_tiles) <= PIECES_IN_HOLDER\n\n    holder_state = Holder(PIECES_IN_HOLDER)\n    for i, tile in enumerate(holder_tiles):\n        tile_id = bag_state.remove_tile(tile)\n        holder_state.set_tile(tile, tile_id, i)\n\n    return bag_state, board_state, holder_state",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338096436,
        "execution_millis": 14,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "95e2f7091c614508b350a524818ccf19",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "cbd4c9448f3f497bae28cb1027efa391"
    },
    {
      "cell_type": "code",
      "source": "def set_up_sample_game2(bag_state):\n\n    # initialize empty board\n    board_size = 7\n    board_state = Board(board_size)\n\n    # add start tiles to board\n    board_tiles = [\"r\"]\n    board_positions = [(3, 3)]\n\n    for start_tile, start_pos in zip(board_tiles, board_positions):\n        board_state.set_tile(start_pos, start_tile)\n        idxb = bag_state.remove_tile(start_tile)\n        pos_to_id[start_pos] = idxb\n\n    # add holder tiles\n    holder_tiles = [\"w\", \"o\", \"d\", \"s\", \"i\"]\n\n    holder_state = Holder(5)\n    for i, tile in enumerate(holder_tiles):\n        tile_id = bag_state.remove_tile(tile)\n        holder_state.set_tile(tile, tile_id, i)\n\n    return bag_state, board_state, holder_state",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338096450,
        "execution_millis": 15,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "53c424cb6a8743cebad245832b7efc49",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "cfd8468103c748c6ae36518a31a118b1"
    },
    {
      "cell_type": "code",
      "source": "def add_suction_joints(parser):\n    plant = parser.plant()\n    suction_body = plant.GetBodyByName(\"tipLink\")\n    constraints = {}\n    for tile, tile_id in bag_state.get_all_tiles():\n        body_name = get_tile_body_name(tile)\n        instance_name = get_tile_instance_name(tile, tile_id)\n\n        model_instance_id = plant.GetModelInstanceByName(instance_name)\n        model_body = plant.GetBodyByName(body_name, model_instance_id)\n        model_joint = plant.AddJoint(\n            QuaternionFloatingJoint(\n                get_tile_joint_name(tile, tile_id),\n                suction_body.body_frame(),\n                model_body.body_frame(),\n            )\n        )\n        X_SuctionTile = RigidTransform([0, 0, 0.1])\n        model_joint.SetDefaultPose(X_SuctionTile)",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338096472,
        "execution_millis": 21,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "ca431a7f97df4c9d8bc2f7eb4a780975",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "c8c4e752deaa4f15b8fed125324419f4"
    },
    {
      "cell_type": "code",
      "source": "def run_game():\n\n    bag_state = fill_bag()\n    meshcat.Delete()\n    builder = DiagramBuilder()\n\n    scenario_data = create_scene()\n    scenario = load_scenario(data=scenario_data)\n\n    station = builder.AddSystem(\n        MakeHardwareStation(\n            scenario, meshcat, parser_prefinalize_callback=add_suction_joints\n        )\n    )\n\n    bag_state, board_state, holder_state = set_up_sample_game(bag_state)\n    plant = station.GetSubsystemByName(\"plant\")\n    planner = builder.AddSystem(Planner(plant, bag_state, board_state, holder_state))\n\n    builder.Connect(\n        station.GetOutputPort(\"body_poses\"),\n        planner.GetInputPort(\"body_poses\"),\n    )\n    demux = builder.AddSystem(Demultiplexer([7, 7]))\n    builder.Connect(station.GetOutputPort(\"iiwa+suction_base+suction_head.state_estimated\"), demux.get_input_port())\n    builder.Connect(\n        demux.get_output_port(0),\n        planner.GetInputPort(\"iiwa_position\"),\n    )\n\n    robot = station.GetSubsystemByName(\n        \"iiwa+suction_base+suction_head.controller\"\n    ).get_multibody_plant_for_control()\n\n    desired_state_from_position = builder.AddSystem(\n        StateInterpolatorWithDiscreteDerivative(\n            7,\n            plant.time_step(),\n            suppress_initial_transient=True,\n        )\n    )\n    desired_state_from_position.set_name(\n        \"iiwa.desired_state_from_position\"\n    )\n    builder.Connect(\n        desired_state_from_position.get_output_port(),\n        station.GetInputPort(\"iiwa+suction_base+suction_head.desired_state\")\n    )\n\n    # Set up differential inverse kinematics.\n    diff_ik = AddIiwaDifferentialIK(builder, robot, frame=robot.GetFrameByName(\"tipLink\"))\n    builder.Connect(planner.GetOutputPort(\"X_WG\"), diff_ik.get_input_port(0))\n    builder.Connect(\n        station.GetOutputPort(\"iiwa+suction_base+suction_head.state_estimated\"),\n        diff_ik.GetInputPort(\"robot_state\"),\n    )\n    builder.Connect(\n        planner.GetOutputPort(\"reset_diff_ik\"),\n        diff_ik.GetInputPort(\"use_robot_state\"),\n    )\n\n    # The DiffIK and the direct position-control modes go through a PortSwitch\n    switch = builder.AddSystem(PortSwitch(7))\n    builder.Connect(\n        diff_ik.get_output_port(), switch.DeclareInputPort(\"diff_ik\")\n    )\n    builder.Connect(\n        planner.GetOutputPort(\"iiwa_position_command\"),\n        switch.DeclareInputPort(\"position\"),\n    )\n    builder.Connect(\n        switch.get_output_port(), \n        desired_state_from_position.get_input_port()\n    )\n    builder.Connect(\n        planner.GetOutputPort(\"control_mode\"),\n        switch.get_port_selector_input_port(),\n    )\n\n    scene_graph = station.GetSubsystemByName(\"scene_graph\")\n    diagram = builder.Build()\n\n    simulator = Simulator(diagram)\n    context = simulator.get_mutable_context()\n    station_context = station.GetMyMutableContextFromRoot(context)\n    plant_context = station.GetMutableSubsystemContext(plant, station_context)\n\n    X_WS = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"tipLink\"))\n    set_board_state(board_state, station, station_context, plant, plant_context, X_WS)\n    set_holder_state(holder_state, station, station_context, plant, plant_context, X_WS)\n    set_bag_state(bag_state, station, station_context, plant, plant_context, X_WS)\n\n    z = 0.2\n    for body_index in plant.GetFloatingBaseBodies():\n        tf = RigidTransform(\n            UniformlyRandomRotationMatrix(generator),\n            [rng.uniform(0.35, 0.65), rng.uniform(-0.12, 0.28), z],\n        )\n        plant.SetFreeBodyPose(plant_context, plant.get_body(body_index), tf)\n        z += 0.1\n\n    # FROM SUCTION CODE \n    scene_graph_context = station.GetMutableSubsystemContext(scene_graph, station_context)\n    planner_context = planner.GetMyMutableContextFromRoot(context)\n    \n    tip_geom_id = scene_graph.model_inspector().GetGeometries(\n        plant.GetBodyFrameIdOrThrow(plant.GetBodyByName(\"tipLink\").index()),\n        Role.kProximity,\n    )[0]\n\n    suction_max_distance = 0.01\n    last_suction_state = False\n    active_suction_joints = []\n\n    meshcat.Flush()  # Wait for the large object meshes to get to meshcat.\n    \n    meshcat.StartRecording()\n    \n    if running_as_notebook:  # Then we're not just running as a test on CI.\n        # simulator.set_target_realtime_rate(1)\n        meshcat.AddButton(\"Stop Simulation\", \"Escape\")\n        print(\"Press Escape to stop the simulation\")\n        while meshcat.GetButtonClicks(\"Stop Simulation\") < 1:\n            suction_state = planner.GetOutputPort(\"suction_on\").Eval(\n                    planner_context\n                )\n            print(f\"suction state = {suction_state}\")\n            if suction_state and not last_suction_state:\n                print(\"turning suction on\")\n                query_object = scene_graph.get_query_output_port().Eval(\n                    scene_graph_context\n                )\n                inspector = query_object.inspector()\n                for (pair) in query_object.ComputeSignedDistancePairwiseClosestPoints(suction_max_distance):\n                    # Do a little gymnastics to figure out which joint to lock\n                    suctioned_geom_id = None\n                    if pair.id_A == tip_geom_id:\n                        suctioned_geom_id = pair.id_B\n                    elif pair.id_B == tip_geom_id:\n                        suctioned_geom_id.geom_id = pair.id_A\n                    else:\n                        continue\n                    suctioned_frame_id = inspector.GetFrameId(\n                        suctioned_geom_id\n                    )\n                    suctioned_body = plant.GetBodyFromFrameId(\n                        suctioned_frame_id\n                    )\n\n                    for i in range(plant.num_joints()):\n                        joint = plant.get_joint(JointIndex(i))\n                        if joint.child_body() == suctioned_body:\n                            joint.Lock(plant_context)\n                            active_suction_joints.append(joint)\n                            print(f\"suctioned the {suctioned_body.name()}\")\n                if not active_suction_joints:\n                    print(\"too far away to suction anything\")\n\n            elif not suction_state and last_suction_state:\n                print(\"turning suction off\")\n                for joint in active_suction_joints:\n                    joint.Unlock(plant_context)\n                active_suction_joints = []\n            \n            last_suction_state = suction_state\n            simulator.AdvanceTo(context.get_time() + 0.1)\n\n        meshcat.DeleteButton(\"Stop Simulation\")\n    else:\n        simulator.set_target_realtime_rate(0)\n        simulator.AdvanceTo(context.get_time() + 2.0)\n\n    meshcat.PublishRecording()\n\nrun_game()",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702338335602,
        "execution_millis": 4735171,
        "deepnote_to_be_reexecuted": false,
        "cell_id": "a7bf4f4decd74e0ca131e0b185d25fb2",
        "deepnote_cell_type": "code"
      },
      "outputs": [
        {
          "name": "stderr",
          "text": "_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 495.3\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 495.40000000000003\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 495.5\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 495.6\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 495.70000000000005\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 495.8\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 495.90000000000003\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 496.0\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 496.1\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 496.20000000000005\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 496.3\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 496.40000000000003\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 496.5\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 496.6\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 496.70000000000005\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 496.8\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 496.90000000000003\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 497.0\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 497.1\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 497.20000000000005\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 497.3\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 497.40000000000003\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 497.5\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 497.6\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 497.70000000000005\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 497.8\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 497.90000000000003\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 498.0\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 498.1\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 498.20000000000005\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 498.3\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 498.40000000000003\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 498.5\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 498.6\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 498.70000000000005\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 498.8\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 498.90000000000003\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 499.0\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 499.1\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 499.20000000000005\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 499.3\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 499.40000000000003\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 499.5\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 499.6\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 499.70000000000005\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 499.8\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 499.90000000000003\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 500.0\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 500.1\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 500.20000000000005\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 500.3\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 500.40000000000003\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 500.5\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 500.6\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 500.70000000000005\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 500.8\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 500.90000000000003\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 501.0\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 501.1\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 501.20000000000005\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 501.3\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 501.40000000000003\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 501.5\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 501.6\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 501.70000000000005\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 501.8\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 501.90000000000003\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 502.0\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 502.1\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 502.20000000000005\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\nmode: PlannerState.END_GAME\ncurrent time: 502.3\nholder: [None, 'o1', None]\nboard: \n_______\n____a__\n____n__\n__hat__\n_______\n_______\n_______\ntimes: {'initial': 100.4, 'prepare': 101.5380198611403, 'prepick': 102.67603972228059, 'pick_start': 104.67603972228059, 'pick_end': 106.67603972228059, 'postpick': 108.67603972228059, 'clearance': 109.91298151557565, 'preplace': 111.14992330887071, 'place_start': 113.14992330887071, 'place_end': 115.14992330887071, 'postplace': 117.14992330887071}\nGame over! Your final score is 4. Please press `esc` to stop the simulation.\nsuction state = [0.]\n",
          "output_type": "stream"
        },
        {
          "output_type": "error",
          "ename": "KeyboardInterrupt",
          "evalue": "",
          "traceback": [
            "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
            "\u001b[0;31mKeyboardInterrupt\u001b[0m                         Traceback (most recent call last)",
            "Cell \u001b[0;32mIn [21], line 175\u001b[0m\n\u001b[1;32m    171\u001b[0m         simulator\u001b[38;5;241m.\u001b[39mAdvanceTo(context\u001b[38;5;241m.\u001b[39mget_time() \u001b[38;5;241m+\u001b[39m \u001b[38;5;241m2.0\u001b[39m)\n\u001b[1;32m    173\u001b[0m     meshcat\u001b[38;5;241m.\u001b[39mPublishRecording()\n\u001b[0;32m--> 175\u001b[0m \u001b[43mrun_game\u001b[49m\u001b[43m(\u001b[49m\u001b[43m)\u001b[49m\n",
            "Cell \u001b[0;32mIn [21], line 166\u001b[0m, in \u001b[0;36mrun_game\u001b[0;34m()\u001b[0m\n\u001b[1;32m    163\u001b[0m             active_suction_joints \u001b[38;5;241m=\u001b[39m []\n\u001b[1;32m    165\u001b[0m         last_suction_state \u001b[38;5;241m=\u001b[39m suction_state\n\u001b[0;32m--> 166\u001b[0m         \u001b[43msimulator\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mAdvanceTo\u001b[49m\u001b[43m(\u001b[49m\u001b[43mcontext\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mget_time\u001b[49m\u001b[43m(\u001b[49m\u001b[43m)\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m+\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;241;43m0.1\u001b[39;49m\u001b[43m)\u001b[49m\n\u001b[1;32m    168\u001b[0m     meshcat\u001b[38;5;241m.\u001b[39mDeleteButton(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mStop Simulation\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[1;32m    169\u001b[0m \u001b[38;5;28;01melse\u001b[39;00m:\n",
            "Cell \u001b[0;32mIn [15], line 263\u001b[0m, in \u001b[0;36mPlanner.CalcGripperPose\u001b[0;34m(self, context, output)\u001b[0m\n\u001b[1;32m    255\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21mend_time\u001b[39m(\u001b[38;5;28mself\u001b[39m, context):\n\u001b[1;32m    256\u001b[0m     \u001b[38;5;28;01mreturn\u001b[39;00m (\n\u001b[1;32m    257\u001b[0m         context\u001b[38;5;241m.\u001b[39mget_abstract_state(\u001b[38;5;28mint\u001b[39m(\u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39m_traj_X_G_index))\n\u001b[1;32m    258\u001b[0m         \u001b[38;5;241m.\u001b[39mget_value()\n\u001b[1;32m    259\u001b[0m         \u001b[38;5;241m.\u001b[39mend_time()\n\u001b[1;32m    260\u001b[0m     )\n\u001b[0;32m--> 263\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21mCalcGripperPose\u001b[39m(\u001b[38;5;28mself\u001b[39m, context, output):\n\u001b[1;32m    264\u001b[0m     context\u001b[38;5;241m.\u001b[39mget_abstract_state(\u001b[38;5;28mint\u001b[39m(\u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39m_mode_index))\u001b[38;5;241m.\u001b[39mget_value()\n\u001b[1;32m    266\u001b[0m     traj_X_G \u001b[38;5;241m=\u001b[39m context\u001b[38;5;241m.\u001b[39mget_abstract_state(\n\u001b[1;32m    267\u001b[0m         \u001b[38;5;28mint\u001b[39m(\u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39m_traj_X_G_index)\n\u001b[1;32m    268\u001b[0m     )\u001b[38;5;241m.\u001b[39mget_value()\n",
            "\u001b[0;31mKeyboardInterrupt\u001b[0m: "
          ]
        }
      ],
      "execution_count": null,
      "block_group": "e12364bb2a61445c8666bee40491a3bd"
    },
    {
      "cell_type": "code",
      "source": "",
      "metadata": {
        "source_hash": null,
        "execution_start": 1702337310967,
        "execution_millis": 24,
        "deepnote_to_be_reexecuted": true,
        "cell_id": "cf3ff45841e04b0dade7dfcfe4494ca5",
        "deepnote_cell_type": "code"
      },
      "outputs": [],
      "execution_count": null,
      "block_group": "98af9d1686d3408381004ffc9e6dda9c"
    },
    {
      "cell_type": "markdown",
      "source": "<a style='text-decoration:none;line-height:16px;display:flex;color:#5B5B62;padding:10px;justify-content:end;' href='https://deepnote.com?utm_source=created-in-deepnote-cell&projectId=4b17a40c-416d-4160-9e25-b767157b933d' target=\"_blank\">\n<img alt='Created in deepnote.com' style='display:inline;max-height:16px;margin:0px;margin-right:7.5px;' src='data:image/svg+xml;base64,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' > </img>\nCreated in <span style='font-weight:600;margin-left:4px;'>Deepnote</span></a>",
      "metadata": {
        "created_in_deepnote_cell": true,
        "deepnote_cell_type": "markdown"
      }
    }
  ],
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "deepnote": {},
    "kernelspec": {
      "name": "python3",
      "language": "python",
      "display_name": "Python 3"
    },
    "deepnote_notebook_id": "b48bbdc1038e462a8ec494b7b8775336",
    "deepnote_execution_queue": []
  }
}