pick.ipynb

{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "EgiF12Hf1Dhs"
      },
      "source": [
        "This notebook provides examples to go along with the [textbook](http://manipulation.csail.mit.edu/pick.html).  I recommend having both windows open, side-by-side!"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "eeMrMI0-1Dhu"
      },
      "outputs": [],
      "source": [
        "# Imports\n",
        "import numpy as np\n",
        "import altair as alt\n",
        "import pydot\n",
        "from IPython.display import display, SVG, clear_output\n",
        "import plotly.express as px\n",
        "\n",
        "from pydrake.all import (\n",
        "    AddMultibodyPlantSceneGraph, AngleAxis, BasicVector, \n",
        "    DiagramBuilder, FindResourceOrThrow, Integrator, JacobianWrtVariable, \n",
        "    LeafSystem, MeshcatVisualizerCpp, MultibodyPlant, MultibodyPositionToGeometryPose, Parser,\n",
        "    PiecewisePose, PiecewisePolynomial, Quaternion, RigidTransform, \n",
        "    RollPitchYaw, RotationMatrix, SceneGraph, Simulator, TrajectorySource\n",
        ")\n",
        "from pydrake.examples.manipulation_station import ManipulationStation\n",
        "\n",
        "from manipulation import running_as_notebook\n",
        "from manipulation.meshcat_cpp_utils import (\n",
        "    StartMeshcat, MeshcatJointSliders)\n",
        "from manipulation.scenarios import AddMultibodyTriad, SetColor\n",
        "\n",
        "# TODO(russt): Move this to drake (adding the element name support to the base class).\n",
        "import pandas as pd\n",
        "\n",
        "def dataframe(trajectory, times, names):\n",
        "  assert trajectory.rows() == len(names)\n",
        "  values = trajectory.vector_values(times)\n",
        "  data = {'t': times }\n",
        "  for i in range(len(names)):\n",
        "    data[names[i]] = values[i,:]\n",
        "  return pd.DataFrame(data)\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [],
      "source": [
        "# Start the visualizer.\n",
        "meshcat = StartMeshcat()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "7q0A14bAilIX"
      },
      "source": [
        "# Inspecting the kinematic tree\n",
        "\n",
        "Here is a simple example that demonstrates how to inspect the kinematic tree stored in a `MultibodyPlant`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "ILYLouFTjv6e"
      },
      "outputs": [],
      "source": [
        "def kinematic_tree_example():\n",
        "    plant = MultibodyPlant(time_step=0.0)\n",
        "    parser = Parser(plant)\n",
        "    parser.AddModelFromFile(FindResourceOrThrow(\n",
        "        \"drake/manipulation/models/allegro_hand_description/sdf/allegro_hand_description_right.sdf\"))\n",
        "    parser.AddModelFromFile(FindResourceOrThrow(\n",
        "        \"drake/examples/manipulation_station/models/061_foam_brick.sdf\"))\n",
        "    plant.Finalize()\n",
        "\n",
        "    # TODO(russt): Add floating base connections\n",
        "    # TODO(russt): Consider a more interactive javascript rendering?\n",
        "    display(SVG(pydot.graph_from_dot_data(plant.GetTopologyGraphvizString())[0].create_svg()))\n",
        "\n",
        "kinematic_tree_example()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "wSa5QcU0Piak"
      },
      "source": [
        "# Forward kinematics of the gripper frame\n",
        "\n",
        "Here is a simple example that let's you visualize the frames on the iiwa and the gripper.  If you click on the \"Open Controls\" menu in the MeshCat visualizer, and dig into the menu `meshcat->drake->Source` then you will see elements for each of the models in the `SceneGraph`: one for the iiwa, another for the WSG, and others for the clutter bins.  You can enable/disable their visualization.  Give it a spin!"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "5SjOClhTltPk"
      },
      "outputs": [],
      "source": [
        "def gripper_forward_kinematics_example():\n",
        "    builder = DiagramBuilder()\n",
        "\n",
        "    # TODO: Replace this with a simple model directive of iiwa+wsg (no clutter bins)\n",
        "    station = builder.AddSystem(ManipulationStation())\n",
        "    station.SetupClutterClearingStation()\n",
        "    station.Finalize()\n",
        "\n",
        "    plant = station.get_multibody_plant()\n",
        "    scene_graph = station.get_scene_graph()\n",
        "    # Draw the frames\n",
        "    for body_name in [\"iiwa_link_1\", \"iiwa_link_2\", \"iiwa_link_3\", \"iiwa_link_4\", \"iiwa_link_5\", \"iiwa_link_6\", \"iiwa_link_7\", \"body\"]:\n",
        "        AddMultibodyTriad(plant.GetFrameByName(body_name), scene_graph)\n",
        "\n",
        "    meshcat.Delete()\n",
        "    visualizer = MeshcatVisualizerCpp.AddToBuilder(\n",
        "        builder, station.GetOutputPort(\"query_object\"), meshcat)\n",
        "\n",
        "    diagram = builder.Build()\n",
        "    context = diagram.CreateDefaultContext()\n",
        "\n",
        "    gripper = plant.GetBodyByName(\"body\")\n",
        "    def pose_callback(context):\n",
        "        pose = plant.EvalBodyPoseInWorld(context, gripper)   ## This is the important line\n",
        "        clear_output(wait=True)\n",
        "        print(\"gripper position (m): \" + np.array2string(\n",
        "            pose.translation(), formatter={\n",
        "                'float': lambda x: \"{:3.2f}\".format(x)}))\n",
        "        print(\"gripper roll-pitch-yaw (rad):\" + np.array2string(\n",
        "            RollPitchYaw(pose.rotation()).vector(),\n",
        "                         formatter={'float': lambda x: \"{:3.2f}\".format(x)}))\n",
        "\n",
        "    sliders = MeshcatJointSliders(meshcat, plant, context)\n",
        "    sliders.Run(visualizer, context, pose_callback)\n",
        "    meshcat.DeleteAddedControls()\n",
        "\n",
        "gripper_forward_kinematics_example()\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "8ZUhwdTOtOjp"
      },
      "source": [
        "# Don't assume $\\dot{q} \\equiv v$\n",
        "\n",
        "Let's just add a single object into the scene.  We won't weld it to the world frame, so it is a \"free body\" or has a \"floating base\"."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "MKezK0PjtUP4"
      },
      "outputs": [],
      "source": [
        "def num_positions_velocities_example():\n",
        "    plant = MultibodyPlant(time_step = 0.0)\n",
        "    Parser(plant).AddModelFromFile(FindResourceOrThrow(\n",
        "        \"drake/examples/manipulation_station/models/061_foam_brick.sdf\"))\n",
        "    plant.Finalize()\n",
        "\n",
        "    context = plant.CreateDefaultContext()\n",
        "    print(context)\n",
        "\n",
        "    print(f\"plant.num_positions() = {plant.num_positions()}\")\n",
        "    print(f\"plant.num_velocities() = {plant.num_velocities()}\")\n",
        "\n",
        "num_positions_velocities_example()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "tlE6mkO0ajJu"
      },
      "source": [
        "Looking at the `Context` you can see that this system has 13 total state variables.  7 of them are positions, $q$; this is due to our pose representation using unit quaternions.  But only 6 of them are velocities, $v$; this is because a six-element spatial velocity provides a better (unconstrained) representation of the rate of change of the unit quaternion.  But clearly, if the length of the vectors don't even match, we do *not* have $\\dot{q} = v$."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "hSeD3PBotJUU"
      },
      "source": [
        "# Kinematic Jacobians for pick and place\n",
        "\n",
        "Let's set up the same iiwa + wsg example, with sliders (but without the frames), that we used above.  But this time I'll display the value of the Jacobian $J^G(q)$."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "6F_KZoc1QDkY"
      },
      "outputs": [],
      "source": [
        "def pick_and_place_jacobians_example():\n",
        "    builder = DiagramBuilder()\n",
        "\n",
        "    # TODO: Replace this with a simple model directive of iiwa+wsg (no clutter bins)\n",
        "    station = builder.AddSystem(ManipulationStation())\n",
        "    station.SetupClutterClearingStation()\n",
        "    station.Finalize()\n",
        "\n",
        "    meshcat.Delete()\n",
        "    visualizer = MeshcatVisualizerCpp.AddToBuilder(\n",
        "        builder, station.GetOutputPort(\"query_object\"), meshcat)\n",
        "\n",
        "    diagram = builder.Build()\n",
        "    context = diagram.CreateDefaultContext()\n",
        "\n",
        "    plant = station.get_multibody_plant()\n",
        "    G = plant.GetBodyByName(\"body\").body_frame()\n",
        "    W = plant.world_frame()\n",
        "    def pose_callback(context):\n",
        "        J_G = plant.CalcJacobianSpatialVelocity(context, JacobianWrtVariable.kQDot, G, [0,0,0], W, W)   ## This is the important line\n",
        "\n",
        "        clear_output(wait=True)\n",
        "        print(\"J_G:\")\n",
        "        print(np.array2string(J_G, formatter={\n",
        "              'float': lambda x: \"{:5.1f}\".format(x)}))\n",
        "        print(\n",
        "            f\"smallest singular value(J_G): {np.min(np.linalg.svd(J_G, compute_uv=False))}\")\n",
        "\n",
        "    # If you want to set the initial positions manually, use this:\n",
        "    #plant.SetPositions(plant.GetMyContextFromRoot(context),\n",
        "    #                   plant.GetModelInstanceByName(\"iiwa\"),\n",
        "    #                   [0, 0, 0, 0, 0, 0, 0])\n",
        "\n",
        "    sliders = MeshcatJointSliders(meshcat, plant, context)\n",
        "    sliders.Run(visualizer, context, pose_callback)\n",
        "    meshcat.DeleteAddedControls()\n",
        "\n",
        "pick_and_place_jacobians_example()\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "UZym7Xo7slPU"
      },
      "source": [
        "# Our first end-effector \"controller\"\n",
        "\n",
        "Let's use the pseudo-inverse of the Jacobian to drive the robot around.  To do that, we'll write a very simple system that looks at the current value of $q$, computes $[J^G]^+$, and uses it to command a constant spatial velocity, $V^G$.\n",
        "\n",
        "We'll only run this controller for a short duration.  Constant spatial velocities aren't something that makes sense for a longer simulation!\n",
        "\n",
        "Make sure you try changing $V^G$, and understand how the command relates to the motion of the robot."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "WBTFzB5ZuXvP"
      },
      "outputs": [],
      "source": [
        "# We can write a new System by deriving from the LeafSystem class.\n",
        "# There is a little bit of boiler plate, but hopefully this example makes sense.\n",
        "class PseudoInverseController(LeafSystem):\n",
        "    def __init__(self, plant):\n",
        "        LeafSystem.__init__(self)\n",
        "        self._plant = plant\n",
        "        self._plant_context = plant.CreateDefaultContext()\n",
        "        self._iiwa = plant.GetModelInstanceByName(\"iiwa\")\n",
        "        self._G = plant.GetBodyByName(\"body\").body_frame()\n",
        "        self._W = plant.world_frame()\n",
        "\n",
        "        self.DeclareVectorInputPort(\"iiwa_position\", BasicVector(7))\n",
        "        self.DeclareVectorOutputPort(\"iiwa_velocity\", BasicVector(7), \n",
        "                                     self.CalcOutput)\n",
        "\n",
        "    def CalcOutput(self, context, output):\n",
        "        q = self.get_input_port().Eval(context)\n",
        "        self._plant.SetPositions(self._plant_context, self._iiwa, q)\n",
        "        J_G = self._plant.CalcJacobianSpatialVelocity(\n",
        "            self._plant_context, JacobianWrtVariable.kQDot, \n",
        "            self._G, [0,0,0], self._W, self._W)\n",
        "        J_G = J_G[:,0:7] # Ignore gripper terms\n",
        "        \n",
        "        V_G_desired = np.array([0,    # rotation about x\n",
        "                                -.1,  # rotation about y\n",
        "                                0,    # rotation about z\n",
        "                                0,    # x\n",
        "                                -.05, # y\n",
        "                                -.1]) # z\n",
        "        v = np.linalg.pinv(J_G).dot(V_G_desired)\n",
        "        output.SetFromVector(v)\n",
        "\n",
        "        \n",
        "def jacobian_controller_example():\n",
        "    builder = DiagramBuilder()\n",
        "\n",
        "    station = builder.AddSystem(ManipulationStation())\n",
        "    station.SetupClutterClearingStation()\n",
        "    station.Finalize()\n",
        "\n",
        "    controller = builder.AddSystem(PseudoInverseController(\n",
        "        station.get_multibody_plant()))\n",
        "    integrator = builder.AddSystem(Integrator(7))\n",
        "\n",
        "    builder.Connect(controller.get_output_port(), \n",
        "                    integrator.get_input_port())\n",
        "    builder.Connect(integrator.get_output_port(),\n",
        "                    station.GetInputPort(\"iiwa_position\"))\n",
        "    builder.Connect(station.GetOutputPort(\"iiwa_position_measured\"),\n",
        "                    controller.get_input_port())\n",
        "\n",
        "    meshcat.Delete()\n",
        "    visualizer = MeshcatVisualizerCpp.AddToBuilder(\n",
        "        builder, station.GetOutputPort(\"query_object\"), meshcat)\n",
        "\n",
        "    diagram = builder.Build()\n",
        "    simulator = Simulator(diagram)\n",
        "    station_context = station.GetMyContextFromRoot(simulator.get_mutable_context())\n",
        "    station.GetInputPort(\"iiwa_feedforward_torque\").FixValue(station_context, np.zeros((7,1)))\n",
        "    station.GetInputPort(\"wsg_position\").FixValue(station_context, [0.1])\n",
        "    integrator.set_integral_value(\n",
        "        integrator.GetMyContextFromRoot(simulator.get_mutable_context()), \n",
        "            station.GetIiwaPosition(station_context))\n",
        "\n",
        "    simulator.set_target_realtime_rate(1.0)\n",
        "    simulator.AdvanceTo(0.01);\n",
        "\n",
        "    return simulator\n",
        "\n",
        "simulator = jacobian_controller_example()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "6v-EGfoI3y6V"
      },
      "outputs": [],
      "source": [
        "# Wait for the model to load in the visualizer, then run this cell to see the interesting part...\n",
        "simulator.AdvanceTo(5.0 if running_as_notebook else 0.1);"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "DfwIMIJCSkx6"
      },
      "source": [
        "# Compute grasp and pregrasp poses\n",
        "\n",
        "Here is a simple example with a floating Schunk gripper and the foam brick.  It defines the grasp pose as described in the notes, and renders it to the 3D visualizer.\n",
        "\n",
        "**Check yourself**: Try changing the grasp pose to our pregrasp pose.  Do you like the numbers that I picked in the text?"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "QuFmomDOxoAR"
      },
      "outputs": [],
      "source": [
        "def grasp_poses_example():\n",
        "    builder = DiagramBuilder()\n",
        "\n",
        "    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step = 0.0)\n",
        "    parser = Parser(plant, scene_graph)\n",
        "    grasp = parser.AddModelFromFile(FindResourceOrThrow(\n",
        "        \"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf\"), \"grasp\")\n",
        "    pregrasp = parser.AddModelFromFile(FindResourceOrThrow(\n",
        "        \"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf\"), \"pregrasp\")\n",
        "    brick = parser.AddModelFromFile(FindResourceOrThrow(\n",
        "        \"drake/examples/manipulation_station/models/061_foam_brick.sdf\"))\n",
        "    plant.Finalize()\n",
        "\n",
        "    B_O = plant.GetBodyByName(\"base_link\", brick)\n",
        "    B_Ggrasp = plant.GetBodyByName(\"body\", grasp)\n",
        "    B_Gpregrasp = plant.GetBodyByName(\"body\", pregrasp)\n",
        "\n",
        "    # Set the pregrasp to be green and slightly transparent.\n",
        "    inspector = scene_graph.model_inspector()\n",
        "    for body_index in plant.GetBodyIndices(pregrasp):\n",
        "        SetColor(\n",
        "            scene_graph, [0, .6, 0, .5], plant.get_source_id(), \n",
        "            inspector.GetGeometries(plant.GetBodyFrameIdOrThrow(body_index)))\n",
        "\n",
        "    meshcat.Delete()\n",
        "    meshcat.SetProperty(\"/Background\", \"visible\", False)\n",
        "    visualizer = MeshcatVisualizerCpp.AddToBuilder(\n",
        "        builder, scene_graph, meshcat)\n",
        "\n",
        "    diagram = builder.Build()\n",
        "    context = diagram.CreateDefaultContext()\n",
        "    plant_context = plant.GetMyContextFromRoot(context)\n",
        "\n",
        "    # TODO(russt): Set a random pose of the object.\n",
        "\n",
        "    # Get the current object, O, pose\n",
        "    X_WO = plant.EvalBodyPoseInWorld(plant_context, B_O)\n",
        "\n",
        "    p_GgraspO = [0, 0.12, 0]\n",
        "    R_GgraspO = RotationMatrix.MakeXRotation(np.pi/2.0).multiply(\n",
        "        RotationMatrix.MakeZRotation(np.pi/2.0))\n",
        "    X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO)\n",
        "    X_OGgrasp = X_GgraspO.inverse()\n",
        "    X_WGgrasp = X_WO.multiply(X_OGgrasp)\n",
        "\n",
        "    # pregrasp is negative y in the gripper frame (see the figure!).\n",
        "    X_GgraspGpregrasp = RigidTransform([0, -0.08, 0])\n",
        "    X_WGpregrasp = X_WGgrasp @ X_GgraspGpregrasp\n",
        "\n",
        "    plant.SetFreeBodyPose(plant_context, B_Ggrasp, X_WGgrasp)\n",
        "    # Open the fingers, too.\n",
        "    plant.GetJointByName(\"left_finger_sliding_joint\", grasp).set_translation(plant_context, -0.054)\n",
        "    plant.GetJointByName(\"right_finger_sliding_joint\", grasp).set_translation(plant_context, 0.054)\n",
        "\n",
        "    plant.SetFreeBodyPose(plant_context, B_Gpregrasp, X_WGpregrasp)\n",
        "    # Open the fingers, too.\n",
        "    plant.GetJointByName(\"left_finger_sliding_joint\", pregrasp).set_translation(plant_context, -0.054)\n",
        "    plant.GetJointByName(\"right_finger_sliding_joint\", pregrasp).set_translation(plant_context, 0.054)    \n",
        "\n",
        "    diagram.Publish(context)\n",
        "\n",
        "grasp_poses_example()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "d5p2tbJWrWZ5"
      },
      "source": [
        "# A pick and place \"sketch\" with keyframes\n",
        "\n",
        "Let's first define the canonical frames of our problem.  We've got a handful of them, and will pass them in and out of several functions, so I'll use a dictionary to store them.\n",
        "\n",
        "*Edit*: I've added one more frame into the mix here, to get some clearance when moving between the bins.  Leaving it out gave my a nice [robot outtake](http://manipulation.csail.mit.edu/data/pick_fail.html), but we can do better."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "zYuRDF6mSBx5"
      },
      "outputs": [],
      "source": [
        "def make_gripper_frames(X_G, X_O):\n",
        "  \"\"\"\n",
        "  Takes a partial specification with X_G[\"initial\"] and X_O[\"initial\"] and X_0[\"goal\"], and \n",
        "  returns a X_G and times with all of the pick and place frames populated.\n",
        "  \"\"\"\n",
        "  # Define (again) the gripper pose relative to the object when in grasp.\n",
        "  p_GgraspO = [0, 0.12, 0]\n",
        "  R_GgraspO = RotationMatrix.MakeXRotation(np.pi/2.0).multiply(\n",
        "      RotationMatrix.MakeZRotation(np.pi/2.0))\n",
        "  X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO)\n",
        "  X_OGgrasp = X_GgraspO.inverse()\n",
        "  # pregrasp is negative y in the gripper frame (see the figure!).\n",
        "  X_GgraspGpregrasp = RigidTransform([0, -0.08, 0])\n",
        "\n",
        "  X_G[\"pick\"] = X_O[\"initial\"].multiply(X_OGgrasp)\n",
        "  X_G[\"prepick\"] = X_G[\"pick\"].multiply(X_GgraspGpregrasp)\n",
        "  X_G[\"place\"] = X_O[\"goal\"].multiply(X_OGgrasp)\n",
        "  X_G[\"preplace\"] = X_G[\"place\"].multiply(X_GgraspGpregrasp)\n",
        "\n",
        "  # I'll interpolate a halfway orientation by converting to axis angle and halving the angle.\n",
        "  X_GprepickGpreplace = X_G[\"prepick\"].inverse().multiply(X_G[\"preplace\"])\n",
        "  angle_axis = X_GprepickGpreplace.rotation().ToAngleAxis()\n",
        "  X_GprepickGclearance = RigidTransform(AngleAxis(angle=angle_axis.angle()/2.0, axis=angle_axis.axis()), \n",
        "                                        X_GprepickGpreplace.translation()/2.0 + np.array([0, -0.3, 0]))\n",
        "  X_G[\"clearance\"] = X_G[\"prepick\"].multiply(X_GprepickGclearance)\n",
        "\n",
        "  # Now let's set the timing\n",
        "  times = {\"initial\": 0}\n",
        "  X_GinitialGprepick = X_G[\"initial\"].inverse().multiply(X_G[\"prepick\"])\n",
        "  times[\"prepick\"] = times[\"initial\"] + 10.0*np.linalg.norm(X_GinitialGprepick.translation())\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(X_GprepickGclearance.translation())\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",
        "\n",
        "X_G = {\"initial\": RigidTransform(RotationMatrix.MakeXRotation(-np.pi/2.0), [0, -0.25, 0.25])}\n",
        "X_O = {\"initial\": RigidTransform(RotationMatrix.MakeZRotation(np.pi/2.0), [-.2, -.75, 0.025]),\n",
        "       \"goal\": RigidTransform(RotationMatrix.MakeZRotation(np.pi),[.75, 0, 0.025])}\n",
        "X_G, times = make_gripper_frames(X_G, X_O)\n",
        "print(f\"Sanity check: The entire maneuver will take {times['postplace']} seconds to execute.\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "JnmfEw0zgzsn"
      },
      "source": [
        "Here's a simple visualization of those frames.  Use the controls menu in meshcat to view the different objects on the different \"paths\" to understand which objects belong to which frames.  Everything look reasonable?\n",
        "\n",
        "Try changing the initial and final pose of the gripper / object."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "oXh50Cf1hU6N"
      },
      "outputs": [],
      "source": [
        "def visualize_gripper_frames(X_G, X_O):\n",
        "    builder = DiagramBuilder()\n",
        "\n",
        "    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step = 0.0)\n",
        "    parser = Parser(plant, scene_graph)\n",
        "    gripper = FindResourceOrThrow(\n",
        "        \"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf\")\n",
        "    brick = FindResourceOrThrow(\n",
        "        \"drake/examples/manipulation_station/models/061_foam_brick.sdf\")\n",
        "    for key, pose in X_G.items():\n",
        "      g = parser.AddModelFromFile(gripper, f\"gripper_{key}\")\n",
        "      plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(\"body\", g), pose)\n",
        "    for key, pose in X_O.items():\n",
        "      o = parser.AddModelFromFile(brick, f\"object_{key}\")\n",
        "      plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(\"base_link\", o), pose)\n",
        "\n",
        "    plant.Finalize()\n",
        "\n",
        "    meshcat.Delete()\n",
        "    visualizer = MeshcatVisualizerCpp.AddToBuilder(\n",
        "        builder, scene_graph, meshcat)\n",
        "\n",
        "    diagram = builder.Build()\n",
        "    context = diagram.CreateDefaultContext()\n",
        "    diagram.Publish(context)\n",
        "\n",
        "visualize_gripper_frames(X_G, X_O)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "HmcG3b_xSeQR"
      },
      "source": [
        "# Pick and place trajectories\n",
        "\n",
        "We've defined the keyframes, but now we will fill in timing details (what should be happening at each instant).\n",
        "\n",
        "Interpolating between the positions is quite simple; we can encode these details as a `PiecewisePolynomial` trajectory in $\\Re^3$ over time.  Interpolating over orientations requires more care.  We'll use `PiecewiseQuaternionSlerp` to do the job.\n",
        "\n",
        "See the documentation of [PiecewisePose](https://drake.mit.edu/doxygen_cxx/classdrake_1_1trajectories_1_1_piecewise_pose.html#ae2f8a49b642aecb5c19ef3625be5e61d) for more details."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "oiwljuszWh_O"
      },
      "outputs": [],
      "source": [
        "def make_gripper_trajectory(X_G, times):\n",
        "    \"\"\"\n",
        "    Constructs a gripper position trajectory from the plan \"sketch\".\n",
        "    \"\"\"\n",
        "\n",
        "    sample_times = []\n",
        "    poses = []\n",
        "    for name in [\"initial\", \"prepick\", \"pick_start\", \"pick_end\", \"postpick\",\n",
        "                 \"clearance\", \"preplace\", \"place_start\", \"place_end\",\n",
        "                 \"postplace\"]:\n",
        "        sample_times.append(times[name])\n",
        "        poses.append(X_G[name])\n",
        "\n",
        "    return PiecewisePose.MakeLinear(sample_times, poses)\n",
        "\n",
        "traj = make_gripper_trajectory(X_G, times)\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "2PnYrnSmEMYC"
      },
      "source": [
        "Now let's plot our trajectories.  `Altair` is a plotting library built on Pandas and Vega-lite (to support interactive graphics), which works well in Colab.  I aim to start using it more.  \n",
        "\n",
        "<sup><sub>TODO(russt): Add mouseover interaction that publishes to meshcat, so that a cursor on this plot renders the corresponding pose.(Unfortunately, I don't believe I can link a callback to python with altair).</sub></sup>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "Ppd-5B3V3cNC"
      },
      "outputs": [],
      "source": [
        "traj_p_G = traj.get_position_trajectory()\n",
        "data = dataframe(traj_p_G, traj_p_G.get_segment_times(), ['x','y','z'])\n",
        "alt.vconcat(\n",
        "  alt.Chart(data).mark_line().encode(x='t', y='x').properties(height=80),\n",
        "  alt.Chart(data).mark_line().encode(x='t', y='y').properties(height=80),\n",
        "  alt.Chart(data).mark_line().encode(x='t', y='z').properties(height=80),\n",
        ")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "yIVvA6ZoFLvt"
      },
      "source": [
        "Unfortunately, Altair doesn't do 3D plotting (neither does mpld3), so here is the Plotly Express version."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "2vgn7mxcDvlK"
      },
      "outputs": [],
      "source": [
        "data = dataframe(traj_p_G, np.linspace(traj_p_G.start_time(), traj_p_G.end_time(), 50), ['x','y','z'])\n",
        "fig = px.line_3d(data, x='x', y='y', z='z')\n",
        "fig.update_traces(mode='lines+markers',\n",
        "                  marker=dict(size=3, color=\"rgb(153, 51, 51)\"),\n",
        "                  line=dict(color=\"rgb(153, 51, 51)\"))\n",
        "if running_as_notebook:\n",
        "    fig.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "zNNRr4Fq7frG"
      },
      "source": [
        "We can plot the orientation trajectory, too.  Here are the components of the quaternion over time."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "4aKEnaLJ50pF"
      },
      "outputs": [],
      "source": [
        "traj_R_G = traj.get_orientation_trajectory()\n",
        "data = dataframe(traj_R_G, traj_R_G.get_segment_times(), ['x','y','z','w'])\n",
        "alt.vconcat(\n",
        "  alt.Chart(data).mark_line().encode(x='t', y='x').properties(height=80),\n",
        "  alt.Chart(data).mark_line().encode(x='t', y='y').properties(height=80),\n",
        "  alt.Chart(data).mark_line().encode(x='t', y='z').properties(height=80),\n",
        "  alt.Chart(data).mark_line().encode(x='t', y='w').properties(height=80)\n",
        ")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "dWCyowMC8LUN"
      },
      "source": [
        "Better, I think, to just plot the entire result.  Before we do, let's quickly define the finger trajectory."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "65OocrXD8TSA"
      },
      "outputs": [],
      "source": [
        "opened = np.array([0.107]);\n",
        "closed = np.array([0.0]);\n",
        "\n",
        "def make_wsg_command_trajectory(times):\n",
        "    traj_wsg_command = PiecewisePolynomial.FirstOrderHold(\n",
        "        [times[\"initial\"], times[\"pick_start\"]], np.hstack([[opened], [opened]]))\n",
        "    traj_wsg_command.AppendFirstOrderSegment(times[\"pick_end\"], closed)\n",
        "    traj_wsg_command.AppendFirstOrderSegment(times[\"place_start\"], closed)\n",
        "    traj_wsg_command.AppendFirstOrderSegment(times[\"place_end\"], opened)\n",
        "    traj_wsg_command.AppendFirstOrderSegment(times[\"postplace\"], opened)\n",
        "    return traj_wsg_command\n",
        "\n",
        "traj_wsg_command = make_wsg_command_trajectory(times)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "aEc95stq9tyx"
      },
      "outputs": [],
      "source": [
        "data = dataframe(traj_wsg_command, traj_wsg_command.get_segment_times(), ['wsg'])\n",
        "alt.Chart(data).mark_line().encode(x='t', y='wsg').properties(height=80)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "wrhTvm5eIJFi"
      },
      "source": [
        "Now let's really visualize the trajectory.  The best way is to animate it in 3D!  Please forgive the messiness of this particular bit of code; I'm trying to provide some intermediate steps that aren't typically used by the main drake developers.\n",
        "\n",
        "TODO(russt): make this less \"bespoke\"."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "PZ7S7oai_ORF"
      },
      "outputs": [],
      "source": [
        "class GripperTrajectoriesToPosition(LeafSystem):\n",
        "    def __init__(self, plant, traj_p_G, traj_R_G, traj_wsg_command):\n",
        "        LeafSystem.__init__(self)\n",
        "        self.plant = plant\n",
        "        self.gripper_body = plant.GetBodyByName(\"body\")\n",
        "        self.left_finger_joint = plant.GetJointByName(\"left_finger_sliding_joint\")\n",
        "        self.right_finger_joint = plant.GetJointByName(\"right_finger_sliding_joint\")\n",
        "        self.traj_p_G = traj_p_G\n",
        "        self.traj_R_G = traj_R_G\n",
        "        self.traj_wsg_command = traj_wsg_command\n",
        "        self.plant_context = plant.CreateDefaultContext()\n",
        "\n",
        "        self.DeclareVectorOutputPort(\"position\", \n",
        "                                     BasicVector(plant.num_positions()),\n",
        "                                     self.CalcPositionOutput)\n",
        "\n",
        "    def CalcPositionOutput(self, context, output):\n",
        "        t = context.get_time()\n",
        "        X_G = RigidTransform(Quaternion(self.traj_R_G.value(t)), self.traj_p_G.value(t))\n",
        "        self.plant.SetFreeBodyPose(self.plant_context, self.gripper_body, X_G)\n",
        "        wsg = self.traj_wsg_command.value(t)\n",
        "        self.left_finger_joint.set_translation(self.plant_context, -wsg/2.0)\n",
        "        self.right_finger_joint.set_translation(self.plant_context, wsg/2.0)\n",
        "        output.SetFromVector(self.plant.GetPositions(self.plant_context))\n",
        "\n",
        "def visualize_pick_and_place_trajectory(traj_p_G, traj_R_G, traj_wsg_command, X_O):\n",
        "    builder = DiagramBuilder()\n",
        "\n",
        "    # Note: Don't use AddMultibodyPlantSceneGraph because we are only using\n",
        "    # MultibodyPlant for parsing, then wiring directly to SceneGraph.\n",
        "    scene_graph = builder.AddSystem(SceneGraph())\n",
        "    plant = MultibodyPlant(time_step=0.0)\n",
        "    plant.RegisterAsSourceForSceneGraph(scene_graph)\n",
        "    parser = Parser(plant, scene_graph)\n",
        "    gripper = parser.AddModelFromFile(FindResourceOrThrow(\n",
        "        \"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf\"), \"gripper\")\n",
        "    brick = FindResourceOrThrow(\n",
        "        \"drake/examples/manipulation_station/models/061_foam_brick.sdf\")\n",
        "    for key, pose in X_O.items():\n",
        "      o = parser.AddModelFromFile(brick, f\"object_{key}\")\n",
        "      plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(\"base_link\", o), pose)\n",
        "    plant.Finalize()\n",
        "\n",
        "    to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))\n",
        "    builder.Connect(to_pose.get_output_port(), scene_graph.get_source_pose_port(plant.get_source_id()))\n",
        "\n",
        "    traj_to_position = builder.AddSystem(GripperTrajectoriesToPosition(plant, traj_p_G, traj_R_G, traj_wsg_command))\n",
        "    builder.Connect(traj_to_position.get_output_port(), to_pose.get_input_port())\n",
        "\n",
        "    meshcat.Delete()\n",
        "    visualizer = MeshcatVisualizerCpp.AddToBuilder(\n",
        "        builder, scene_graph, meshcat)\n",
        "\n",
        "    diagram = builder.Build()\n",
        "\n",
        "    simulator = Simulator(diagram)\n",
        "    # TODO(russt): get recording working again with new Meshcat (and remove realtime rate)\n",
        "    #meshcat.start_recording()\n",
        "    if running_as_notebook: \n",
        "        simulator.set_target_realtime_rate(1.0)\n",
        "    simulator.AdvanceTo(traj_p_G.end_time() if running_as_notebook else 0.1)\n",
        "    #meshcat.publish_recording()\n",
        "\n",
        "visualize_pick_and_place_trajectory(traj_p_G, traj_R_G, traj_wsg_command, X_O)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "ue9ofS7GHpXr"
      },
      "source": [
        "# Putting it all together\n",
        "\n",
        "In the visualization above, we animated the plan, but did not actually command the robot!  Let's put our grasping trajectories back together with our Jacobian-based controller.\n",
        "\n",
        "The first thing you might have noticed is that the Jacobian controller takes spatial velocities as input.  But so far we have trajectories for positions and orientations.  Let's differentiate them to get the velocities."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "MfaCoTnNXfjj"
      },
      "outputs": [],
      "source": [
        "traj_v_G = traj_p_G.MakeDerivative()\n",
        "\n",
        "data = dataframe(traj_v_G, np.linspace(traj_v_G.start_time(), traj_v_G.end_time()), ['x','y','z'])\n",
        "alt.vconcat(\n",
        "  alt.Chart(data).mark_line().encode(x='t', y='x').properties(height=80),\n",
        "  alt.Chart(data).mark_line().encode(x='t', y='y').properties(height=80),\n",
        "  alt.Chart(data).mark_line().encode(x='t', y='z').properties(height=80),\n",
        ")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "BrDNL8cvMo_o"
      },
      "source": [
        "Now we need a slightly modified version of our Jacobian controller, which can take the spatial velocity commands from an input port."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "nVKlTQKGKUU0"
      },
      "outputs": [],
      "source": [
        "# We can write a new System by deriving from the LeafSystem class.\n",
        "# There is a little bit of boiler plate, but hopefully this example makes sense.\n",
        "class PseudoInverseController(LeafSystem):\n",
        "    def __init__(self, plant):\n",
        "        LeafSystem.__init__(self)\n",
        "        self._plant = plant\n",
        "        self._plant_context = plant.CreateDefaultContext()\n",
        "        self._iiwa = plant.GetModelInstanceByName(\"iiwa\")\n",
        "        self._G = plant.GetBodyByName(\"body\").body_frame()\n",
        "        self._W = plant.world_frame()\n",
        "\n",
        "        self.V_G_port = self.DeclareVectorInputPort(\"V_WG\", BasicVector(6))\n",
        "        self.q_port = self.DeclareVectorInputPort(\"iiwa_position\", BasicVector(7))\n",
        "        self.DeclareVectorOutputPort(\"iiwa_velocity\", BasicVector(7), \n",
        "                                     self.CalcOutput)\n",
        "        # TODO(russt): Add missing binding\n",
        "        #joint_indices = plant.GetJointIndices(self._iiwa)\n",
        "        #self.position_start = plant.get_joint(joint_indices[0]).position_start()\n",
        "        #self.position_end = plant.get_joint(joint_indices[-1]).position_start()\n",
        "        self.iiwa_start = plant.GetJointByName(\"iiwa_joint_1\").velocity_start()\n",
        "        self.iiwa_end = plant.GetJointByName(\"iiwa_joint_7\").velocity_start()\n",
        "\n",
        "    def CalcOutput(self, context, output):\n",
        "        V_G = self.V_G_port.Eval(context)\n",
        "        q = self.q_port.Eval(context)\n",
        "        self._plant.SetPositions(self._plant_context, self._iiwa, q)\n",
        "        J_G = self._plant.CalcJacobianSpatialVelocity(\n",
        "            self._plant_context, JacobianWrtVariable.kV, \n",
        "            self._G, [0,0,0], self._W, self._W)\n",
        "        J_G = J_G[:,self.iiwa_start:self.iiwa_end+1] # Only iiwa terms.\n",
        "        v = np.linalg.pinv(J_G).dot(V_G)\n",
        "        output.SetFromVector(v)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "uM1pk1zzN9D3"
      },
      "source": [
        "Oh, boy.  Big moment.  I'm excited... are you?  Let's do this thing!"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "hDrXl1oSNt6u"
      },
      "outputs": [],
      "source": [
        "X_O = {\"initial\": RigidTransform(RotationMatrix.MakeZRotation(np.pi/2.0), [-.2, -.65, 0.09]),\n",
        "       \"goal\": RigidTransform(RotationMatrix.MakeZRotation(np.pi),[.5, 0, 0.09])}\n",
        "\n",
        "builder = DiagramBuilder()\n",
        "\n",
        "station = builder.AddSystem(ManipulationStation())\n",
        "station.SetupClutterClearingStation()\n",
        "station.AddManipulandFromFile(\n",
        "    \"drake/examples/manipulation_station/models/061_foam_brick.sdf\",\n",
        "    X_O[\"initial\"])\n",
        "station.Finalize()\n",
        "\n",
        "# Find the initial pose of the gripper (as set in the default Context)\n",
        "temp_context = station.CreateDefaultContext()\n",
        "plant = station.get_multibody_plant()\n",
        "temp_plant_context = plant.GetMyContextFromRoot(temp_context)\n",
        "X_G = {\"initial\": plant.EvalBodyPoseInWorld(temp_plant_context, plant.GetBodyByName(\"body\"))}\n",
        "X_G, times = make_gripper_frames(X_G, X_O)\n",
        "print(f\"Sanity check: The entire maneuver will take {times['postplace']} seconds to execute.\")\n",
        "\n",
        "# Make the trajectories\n",
        "traj = make_gripper_trajectory(X_G, times)\n",
        "traj_V_G = traj.MakeDerivative()\n",
        "\n",
        "V_G_source = builder.AddSystem(TrajectorySource(traj_V_G))\n",
        "V_G_source.set_name(\"v_WG\")\n",
        "controller = builder.AddSystem(PseudoInverseController(plant))\n",
        "controller.set_name(\"PseudoInverseController\")\n",
        "builder.Connect(V_G_source.get_output_port(), controller.GetInputPort(\"V_WG\"))\n",
        "\n",
        "integrator = builder.AddSystem(Integrator(7))\n",
        "integrator.set_name(\"integrator\")\n",
        "builder.Connect(controller.get_output_port(), \n",
        "                integrator.get_input_port())\n",
        "builder.Connect(integrator.get_output_port(),\n",
        "                station.GetInputPort(\"iiwa_position\"))\n",
        "builder.Connect(station.GetOutputPort(\"iiwa_position_measured\"),\n",
        "                controller.GetInputPort(\"iiwa_position\"))\n",
        "\n",
        "traj_wsg_command = make_wsg_command_trajectory(times)\n",
        "wsg_source = builder.AddSystem(TrajectorySource(traj_wsg_command))\n",
        "wsg_source.set_name(\"wsg_command\")\n",
        "builder.Connect(wsg_source.get_output_port(), station.GetInputPort(\"wsg_position\"))\n",
        "\n",
        "meshcat.Delete()\n",
        "visualizer = MeshcatVisualizerCpp.AddToBuilder(\n",
        "    builder, station.GetOutputPort(\"query_object\"), meshcat)\n",
        "\n",
        "diagram = builder.Build()\n",
        "diagram.set_name(\"pick_and_place\")\n",
        "\n",
        "simulator = Simulator(diagram)\n",
        "station_context = station.GetMyContextFromRoot(simulator.get_mutable_context())\n",
        "# TODO(russt): Add this missing python binding\n",
        "#integrator.set_integral_value(\n",
        "#    integrator.GetMyContextFromRoot(simulator.get_mutable_context()), \n",
        "#        station.GetIiwaPosition(station_context))\n",
        "integrator.GetMyContextFromRoot(simulator.get_mutable_context()).get_mutable_continuous_state_vector().SetFromVector(station.GetIiwaPosition(station_context))\n",
        "\n",
        "if running_as_notebook:\n",
        "    simulator.set_target_realtime_rate(1.0)\n",
        "#meshcat.start_recording()\n",
        "simulator.AdvanceTo(traj_p_G.end_time() if running_as_notebook else 0.1)\n",
        "#meshcat.stop_recording()\n",
        "#meshcat.publish_recording()\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "AeKMhIacWZ00"
      },
      "source": [
        "That's quite some diagram we've assembled.  Do you understand all of the parts?"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "ZWXemy-bUAab"
      },
      "outputs": [],
      "source": [
        "SVG(pydot.graph_from_dot_data(diagram.GetGraphvizString(max_depth=1))[0].create_svg())"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {},
        "colab_type": "code",
        "id": "JX1Oq7gfWsBD"
      },
      "outputs": [],
      "source": []
    }
  ],
  "metadata": {
    "colab": {
      "collapsed_sections": [],
      "name": "Robotic Manipulation - Basic Pick and Place.ipynb",
      "provenance": []
    },
    "interpreter": {
      "hash": "31f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6"
    },
    "kernelspec": {
      "display_name": "Python 3.6.9 64-bit",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.6.9"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 2
}