demo.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "cell_id": "00000-1748bd05-6504-4c3d-bc7d-805cc959bf77",
    "deepnote_cell_type": "markdown"
   },
   "source": [
    "# Ping Pong Paddle Maneuvers\n",
    "\n",
    "_Final Project for 6.843, Fall 2021_\n",
    "\n",
    "_Viraj Parimi, Cameron Pittman_"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "cell_id": "00001-87498833-d809-4000-b8ec-9980f6a0c83d",
    "deepnote_cell_type": "code",
    "deepnote_to_be_reexecuted": false,
    "execution_millis": 1003,
    "execution_start": 1638766139610,
    "source_hash": "9f2a0cfe"
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from manipulation.meshcat_cpp_utils import (\n",
    "    StartMeshcat\n",
    ")\n",
    "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
    "\n",
    "from pydrake.all import (\n",
    "    AddTriad,\n",
    "    BasicVector,\n",
    "    DiagramBuilder,\n",
    "    Integrator,\n",
    "    JacobianWrtVariable,\n",
    "    LeafSystem,\n",
    "    MeshcatVisualizerCpp,\n",
    "    MeshcatVisualizerParams,\n",
    "    RollPitchYaw,\n",
    "    Simulator,\n",
    "    MathematicalProgram, SnoptSolver\n",
    ")\n",
    "\n",
    "from iiwa_paddle_station import MakeIiwaPaddleStation\n",
    "\n",
    "proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=[])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "cell_id": "00002-262ac397-4786-407d-a585-01276613c6e7",
    "deepnote_cell_type": "code",
    "deepnote_output_heights": [
     null,
     21.1875
    ],
    "deepnote_to_be_reexecuted": false,
    "execution_millis": 318,
    "execution_start": 1638765569710,
    "is_output_hidden": false,
    "source_hash": "67118576",
    "tags": []
   },
   "outputs": [
    {
     "data": {
      "text/html": [
       "Meshcat is now available at <a href=\"http://localhost:7001\">http://localhost:7001</a>"
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "meshcat = StartMeshcat()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "cell_id": "00003-5fc20b8b-b9d7-4b5a-b40b-d3961e2fc522",
    "deepnote_cell_type": "code",
    "deepnote_to_be_reexecuted": false,
    "execution_millis": 16,
    "execution_start": 1638765572610,
    "source_hash": "f867aa45",
    "tags": []
   },
   "outputs": [],
   "source": [
    "class PseudoInverseController(LeafSystem):\n",
    "    \"\"\"\n",
    "    Based on the PseudoInverseController from pset3/robot_painter\n",
    "    \"\"\"\n",
    "\n",
    "    def __init__(self, plant):\n",
    "        LeafSystem.__init__(self)\n",
    "        self.plant = plant\n",
    "        self.plant_context = plant.CreateDefaultContext()\n",
    "        self.iiwa = plant.GetModelInstanceByName(\"iiwa7\")\n",
    "        self.P = plant.GetBodyByName(\"base_link\").body_frame()\n",
    "        self.W = plant.world_frame()\n",
    "\n",
    "        self.q_port = self.DeclareVectorInputPort(\"iiwa_pos_measured\", BasicVector(7))\n",
    "        self.w_P_port = self.DeclareVectorInputPort(\n",
    "            \"paddle_desired_angular_velocity\", BasicVector(3)\n",
    "        )\n",
    "        self.v_P_port = self.DeclareVectorInputPort(\n",
    "            \"paddle_desired_velocity\", BasicVector(3)\n",
    "        )\n",
    "\n",
    "        self.DeclareVectorOutputPort(\"iiwa_velocity\", BasicVector(7), self.CalcOutput)\n",
    "\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",
    "        q = self.q_port.Eval(context)\n",
    "        w_P = np.zeros(3)\n",
    "        # w_P = self.w_P_port.Eval(context)\n",
    "        v_P = self.v_P_port.Eval(context)\n",
    "        self.plant.SetPositions(self.plant_context, self.iiwa, q)\n",
    "        J_P = self.plant.CalcJacobianSpatialVelocity(\n",
    "            self.plant_context,\n",
    "            JacobianWrtVariable.kV,\n",
    "            self.P,\n",
    "            [0, 0, 0],\n",
    "            self.W,\n",
    "            self.W,\n",
    "        )\n",
    "        J_P = J_P[:, self.iiwa_start : self.iiwa_end + 1]  # Only iiwa terms.\n",
    "        V_P = np.hstack([w_P, v_P])\n",
    "        v = np.linalg.pinv(J_P).dot(V_P)  # important\n",
    "        output.SetFromVector(v)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "cell_id": "00004-f20b0d2f-81a5-49e5-85d4-11ff522fc4ea",
    "deepnote_cell_type": "code",
    "deepnote_to_be_reexecuted": false,
    "execution_millis": 18,
    "execution_start": 1638765975244,
    "source_hash": "3219f658",
    "tags": []
   },
   "outputs": [],
   "source": [
    "class DifferentialIKSystem(LeafSystem):\n",
    "    \"\"\"Wrapper system for Differential IK. \n",
    "        @param plant MultibodyPlant of the simulated plant. \n",
    "        @param diffik_fun function object that handles diffik. Must have the signature \n",
    "               diffik_fun(J_G, V_G_desired, q_now, v_now, X_now)\n",
    "    \"\"\" \n",
    "    def __init__(self, plant, diffik_fun):\n",
    "        LeafSystem.__init__(self)\n",
    "        self._plant = plant\n",
    "        self._plant_context = plant.CreateDefaultContext()\n",
    "        self._iiwa = plant.GetModelInstanceByName(\"iiwa7\")\n",
    "        self._G = plant.GetBodyByName(\"base_link\").body_frame()\n",
    "        self._W = plant.world_frame()\n",
    "        self._diffik_fun = diffik_fun\n",
    "\n",
    "        self.DeclareVectorInputPort(\"spatial_velocity\", BasicVector(6))\n",
    "        self.DeclareVectorInputPort(\"iiwa_position_measured\", BasicVector(7))\n",
    "        self.DeclareVectorInputPort(\"iiwa_velocity_measured\", BasicVector(7))\n",
    "        self.DeclareVectorOutputPort(\"iiwa_velocity_command\", BasicVector(7), \n",
    "                                     self.CalcOutput)\n",
    "        \n",
    "    def CalcOutput(self, context, output):\n",
    "        V_G_desired = self.get_input_port(0).Eval(context)\n",
    "        q_now = self.get_input_port(1).Eval(context)\n",
    "        v_now = self.get_input_port(2).Eval(context)\n",
    "\n",
    "        self._plant.SetPositions(self._plant_context, self._iiwa, q_now)\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",
    "        X_now = self._plant.CalcRelativeTransform(self._plant_context, \n",
    "                                                     self._W, self._G)\n",
    "        p_now = X_now.translation()\n",
    "        rpy_now = RollPitchYaw(X_now.rotation()).vector()\n",
    "        \n",
    "        v = self._diffik_fun(J_G, V_G_desired, q_now, v_now, p_now, rpy_now) \n",
    "        output.SetFromVector(v)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "cell_id": "00005-a769d588-13fb-411c-97ec-7d71b0346937",
    "deepnote_cell_type": "code",
    "deepnote_to_be_reexecuted": false,
    "execution_millis": 1,
    "execution_start": 1638766470923,
    "source_hash": "d41a94f4",
    "tags": []
   },
   "outputs": [],
   "source": [
    "def deg_to_rad(deg):\n",
    "  return (deg / 360) * 2 * np.pi\n",
    "\n",
    "def DiffIKQP(J_G, V_G_desired, q_now, v_now, p_now, rpy_now):\n",
    "\n",
    "  prog = MathematicalProgram()\n",
    "  v = prog.NewContinuousVariables(7, 'v')\n",
    "  v_max = 3.0 # do not modify\n",
    "  h = 4e-3 # do not modify\n",
    "\n",
    "  obj = J_G.dot(v) - V_G_desired\n",
    "  prog.AddCost(obj.dot(obj))\n",
    "  prog.AddBoundingBoxConstraint(-v_max * np.ones(7), v_max * np.ones(7), v)\n",
    "\n",
    "  solver = SnoptSolver()\n",
    "  result = solver.Solve(prog)\n",
    "\n",
    "  if not (result.is_success()):\n",
    "    raise ValueError(\"Could not find the optimal solution.\")\n",
    "\n",
    "  v_solution = result.GetSolution(v)\n",
    "\n",
    "  return v_solution"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "cell_id": "00004-48a85349-6194-47d8-940a-e40df0841a78",
    "deepnote_cell_type": "code",
    "deepnote_to_be_reexecuted": false,
    "execution_millis": 46,
    "execution_start": 1638767032445,
    "source_hash": "d4f1ca45",
    "tags": []
   },
   "outputs": [],
   "source": [
    "def ball_p_from_state(pose):\n",
    "    \"\"\"\n",
    "    Get x, y, z from ball_state\n",
    "\n",
    "    Params:\n",
    "        BasicVector(13)\n",
    "    Returns:\n",
    "        array[3]\n",
    "    \"\"\"\n",
    "    return pose[4:7]\n",
    "\n",
    "\n",
    "def ball_v_from_state(pose):\n",
    "    \"\"\"\n",
    "    Get translational velocity from ball_state\n",
    "\n",
    "    Params:\n",
    "        BasicVector(13)\n",
    "    Returns:\n",
    "        array[3]\n",
    "    \"\"\"\n",
    "    return pose[-3:]\n",
    "\n",
    "\n",
    "# TODO: could we use a signed distance function instead?\n",
    "def get_mode(p_ball, v_ball, p_paddle, atol = 1e-03):\n",
    "    \"\"\"\n",
    "    Determine current paddle mode\n",
    "\n",
    "    Output is one of: \"rising\", \"falling\", \"contact\"\n",
    "    \"\"\"\n",
    "    b_z = p_ball[2]\n",
    "    vz = v_ball[2]\n",
    "    p_z = p_paddle[2]\n",
    "\n",
    "    # TODO: there's a better way to do this\n",
    "    if np.isclose(b_z, p_z, atol=atol):\n",
    "        return \"contact\"\n",
    "    else:\n",
    "        return \"rising\" if vz >= 0 else \"falling\"\n",
    "\n",
    "\n",
    "delta_height = 0.2\n",
    "CATCH_HEIGHT = 0.55\n",
    "g = -9.81\n",
    "# g = -3.721 # mars\n",
    "e_z = np.array([0, 0, 1], dtype = float)\n",
    "\n",
    "# hardcoded initial paddle position\n",
    "p_p_0 = np.array([0.07659419, -0.75730758, 0.63302061])\n",
    "\n",
    "# initial x, y, z of ball\n",
    "p_b_0 = p_p_0 + np.array([-0.2, -0.1, 1])\n",
    "\n",
    "# initial vx, vy, vz of ball\n",
    "v_b_0 = np.array([0.1, 0, 0])\n",
    "\n",
    "class Catcher5(LeafSystem):\n",
    "    \"\"\"\n",
    "    Catch the ball\n",
    "    \"\"\"\n",
    "\n",
    "    def __init__(self, plant):\n",
    "        LeafSystem.__init__(self)\n",
    "        self.plant = plant\n",
    "        self.plant_context = plant.CreateDefaultContext()\n",
    "        self.iiwa = plant.GetModelInstanceByName(\"iiwa7\")\n",
    "        self.P = plant.GetBodyByName(\"base_link\")\n",
    "        self.ball = plant.GetBodyByName(\"ball\")\n",
    "        self.W = plant.world_frame()\n",
    "        \n",
    "        self.t_catch = None\n",
    "        # assume falling at first\n",
    "        self.made_contact = False\n",
    "\n",
    "        self.DeclareVectorInputPort(\"ball_pose\", BasicVector(3))\n",
    "        self.DeclareVectorInputPort(\"ball_velocity\", BasicVector(3))\n",
    "        self.DeclareVectorInputPort(\"iiwa_pos_measured\", BasicVector(7))\n",
    "        self.DeclareVectorInputPort(\"iiwa_velocity_estimated\", BasicVector(7))\n",
    "        self.DeclareVectorInputPort(\"ball_state\", BasicVector(13))\n",
    "        self.DeclareVectorOutputPort(\n",
    "            \"paddle_velocity_desired\", BasicVector(6), self.CalcOutput\n",
    "        )\n",
    "\n",
    "    def compute_time_of_impact(self, p_bz, v_bz, catch_height):\n",
    "        g_pos = abs(g)\n",
    "        return (1. / g_pos) * (v_bz + np.sqrt(v_bz**2 + 2 * g_pos * (p_bz - catch_height)))\n",
    "\n",
    "    def CalcOutput(self, context, output):\n",
    "        q = self.GetInputPort(\"iiwa_pos_measured\").Eval(context)\n",
    "        v = self.GetInputPort(\"iiwa_velocity_estimated\").Eval(context)\n",
    "        # self.plant.SetPositions(self.plant_context, self.iiwa, q)\n",
    "        self.plant.SetPositionsAndVelocities(self.plant_context, self.iiwa, np.hstack([q, v]))\n",
    "        v = self.GetInputPort(\"iiwa_velocity_estimated\").Eval(context)\n",
    "\n",
    "        p_Ball = self.GetInputPort(\"ball_pose\").Eval(context)\n",
    "        v_Ball = np.array(self.GetInputPort(\"ball_velocity\").Eval(context))\n",
    "\n",
    "        p_Paddle = self.plant.EvalBodyPoseInWorld(\n",
    "            self.plant_context, self.P\n",
    "        ).translation()\n",
    "\n",
    "        v_Paddle = self.plant.EvalBodySpatialVelocityInWorld(\n",
    "            self.plant_context, self.P\n",
    "        ).translational()\n",
    "\n",
    "        v_p_desired = np.zeros(3)\n",
    "        w_p_desired = np.zeros(3)\n",
    "\n",
    "        current_time = context.get_time()\n",
    "\n",
    "        mode = get_mode(p_Ball, v_Ball, p_Paddle, atol=1e-01)\n",
    "        if not self.made_contact and mode == \"contact\":\n",
    "            self.made_contact = True\n",
    "\n",
    "        max_angle = 30\n",
    "        paddle_radius = 0.175 # according to the paddle.sdf\n",
    "        ball_radius = 0.035 # according to ball.sdf\n",
    "        gain = np.array([-35, 35, 0])\n",
    "\n",
    "        # useful when trying to match the positions of the ball and paddle\n",
    "        dist_ball_and_paddle = p_Ball - p_Paddle - np.array([0, 0, ball_radius])\n",
    "\n",
    "        [x_offset, y_offset, z_offset] = (dist_ball_and_paddle) / paddle_radius\n",
    "        \n",
    "        w_p_x = np.tan(y_offset * max_angle)\n",
    "        w_p_y = np.tan(x_offset * max_angle) * -1\n",
    "        w_p_z = 0\n",
    "\n",
    "        w_p_desired = np.array(list(map(deg_to_rad, [w_p_x, w_p_y, w_p_z]))) * gain\n",
    "\n",
    "        if not self.made_contact:\n",
    "            # try to beat the ball to the catch point\n",
    "            if self.t_catch is None:\n",
    "                self.t_catch = self.compute_time_of_impact(p_Ball[2], v_Ball[2], CATCH_HEIGHT)\n",
    "\n",
    "            # tell the iiwa to get to the location of the catch a split second early\n",
    "            t_pre_catch = self.t_catch * 0.75\n",
    "\n",
    "            # assume ball is only acting under gravity\n",
    "            a_b = np.array([0, 0, g])\n",
    "\n",
    "            p_catch = p_b_0 + v_b_0 * self.t_catch + (a_b / 2) * self.t_catch**2\n",
    "            v_b_catch = v_b_0 + a_b * self.t_catch\n",
    "\n",
    "            v_p_0 = (p_catch - p_p_0 - 0.5 * v_b_catch * t_pre_catch) / (0.5 * t_pre_catch)\n",
    "            a_p = (v_b_catch - v_p_0) / t_pre_catch\n",
    "\n",
    "            v_p_desired = v_p_0 + a_p * current_time\n",
    "        else:\n",
    "            kp = 10\n",
    "            kd = 0.1\n",
    "            # ball should be moving slowly. change to PD behavior after the intial contact\n",
    "            v_p_desired = kp * dist_ball_and_paddle + kd * (v_Ball - v_Paddle)\n",
    "\n",
    "        V_P = np.concatenate([w_p_desired, v_p_desired])\n",
    "        output.SetFromVector(V_P)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {
    "cell_id": "00005-8d1d34ef-0abe-46c0-9219-e1e72b425c64",
    "deepnote_cell_type": "code",
    "deepnote_output_heights": [
     596.8125
    ],
    "deepnote_to_be_reexecuted": false,
    "execution_millis": 32967,
    "execution_start": 1638767033762,
    "source_hash": "cad5d679"
   },
   "outputs": [],
   "source": [
    "class Demo:\n",
    "    def __init__(self, time_step=0.001):\n",
    "        \"\"\"\n",
    "        Robotic Kuka iiwa with paddle end effector\n",
    "\n",
    "        Args:\n",
    "            time_step (float, optional): time step for internal manipulation station controller\n",
    "        \"\"\"\n",
    "        self.time = 0\n",
    "        diagram, plant, scene_graph = MakeIiwaPaddleStation(time_step=time_step)\n",
    "\n",
    "        self.builder = DiagramBuilder()\n",
    "        self.station = self.builder.AddSystem(diagram)\n",
    "        self.position_log = []\n",
    "        self.velocity_log = []\n",
    "\n",
    "        self.visualizer = MeshcatVisualizerCpp.AddToBuilder(\n",
    "            self.builder,\n",
    "            diagram.GetOutputPort(\"geometry_query\"),\n",
    "            meshcat,\n",
    "            MeshcatVisualizerParams(delete_prefix_initialization_event=False)\n",
    "        )\n",
    "\n",
    "        # change gravity\n",
    "        plant.mutable_gravity_field().set_gravity_vector([0, 0, g])\n",
    "\n",
    "        self.plant = plant\n",
    "\n",
    "        diff_ik = self.builder.AddSystem(DifferentialIKSystem(self.plant, DiffIKQP))\n",
    "        diff_ik.set_name(\"diff_ik_sys\")\n",
    "        catcher = self.builder.AddSystem(Catcher5(self.plant))\n",
    "        catcher.set_name(\"catcher\")\n",
    "        integrator = self.builder.AddSystem(Integrator(7))\n",
    "\n",
    "\n",
    "        self.builder.Connect(diff_ik.get_output_port(),\n",
    "                        integrator.get_input_port())\n",
    "        self.builder.Connect(integrator.get_output_port(),\n",
    "                        self.station.GetInputPort(\"iiwa_position\"))\n",
    "        self.builder.Connect(self.station.GetOutputPort(\"iiwa_position_measured\"),\n",
    "                        diff_ik.get_input_port(1))\n",
    "        self.builder.Connect(self.station.GetOutputPort(\"iiwa_velocity_estimated\"),\n",
    "                        diff_ik.get_input_port(2))\n",
    "        self.builder.Connect(catcher.get_output_port(0),\n",
    "                        diff_ik.get_input_port(0))\n",
    "\n",
    "        self.builder.Connect(self.station.GetOutputPort(\"iiwa_position_measured\"), catcher.GetInputPort(\"iiwa_pos_measured\"))\n",
    "        self.builder.Connect(self.station.GetOutputPort(\"iiwa_velocity_estimated\"), catcher.GetInputPort(\"iiwa_velocity_estimated\"))\n",
    "\n",
    "        self.builder.Connect(\n",
    "            self.station.GetOutputPort(\"ball_state\"),\n",
    "            catcher.GetInputPort(\"ball_state\")\n",
    "        )\n",
    "\n",
    "        self.builder.ExportInput(catcher.GetInputPort(\"ball_pose\"), \"v_ball_pose\")\n",
    "        self.builder.ExportInput(catcher.GetInputPort(\"ball_velocity\"), \"v_ball_velocity\")\n",
    "\n",
    "        self.diagram = self.builder.Build()\n",
    "        self.simulator = Simulator(self.diagram)\n",
    "        self.simulator.set_target_realtime_rate(1.0)\n",
    "\n",
    "        self.context = self.simulator.get_context()\n",
    "        self.station_context = self.station.GetMyContextFromRoot(self.context)\n",
    "        self.plant_context = self.plant.GetMyContextFromRoot(self.context)\n",
    "\n",
    "        iiwa_q0 = [-1.57, 0, 0, -np.pi/2, 0, 0, 0]\n",
    "        self.plant.SetPositions(self.plant_context, self.plant.GetModelInstanceByName(\"iiwa7\"), np.array(iiwa_q0))\n",
    "\n",
    "        # [<4 quaternion>, <3 translational pos>, <3 angular vel>, <3 translational vel>]\n",
    "        ball_q0 = np.concatenate([ np.ones(4), p_b_0, np.zeros(3), v_b_0 ])\n",
    "        self.plant.SetPositionsAndVelocities(self.plant_context, self.plant.GetModelInstanceByName(\"ball\"), np.array(ball_q0))\n",
    "\n",
    "        self.station.GetInputPort(\"iiwa_feedforward_torque\").FixValue(self.station_context, np.zeros((7,1)))\n",
    "        iiwa_model_instance = self.plant.GetModelInstanceByName(\"iiwa7\")\n",
    "        iiwa_q = self.plant.GetPositions(self.plant_context, iiwa_model_instance)\n",
    "        integrator.GetMyContextFromRoot(self.context).get_mutable_continuous_state_vector().SetFromVector(iiwa_q)\n",
    "\n",
    "\n",
    "    # borrowed from github.com/matt-bev/robot-juggler\n",
    "    def step(self, simulate=True, duration=0.1, final=True, verbose=False, display_pose=False):\n",
    "        \"\"\"\n",
    "        step the closed loop system\n",
    "\n",
    "        Args:\n",
    "            simulate (bool, optional): whether or not to visualize the command. Defaults to True.\n",
    "            duration (float, optional): duration to complete command in simulation. Defaults to 0.1.\n",
    "            final (bool, optional): whether or not this is the final command in the sequence; relevant for recording. Defaults to True.\n",
    "            verbose (bool, optional): whether or not to print measured position change. Defaults to False.\n",
    "            display_pose (bool, optional): whether or not to show the pose of the paddle in simulation. Defaults to False.\n",
    "        \"\"\"\n",
    "        ball_pose = self.plant.EvalBodyPoseInWorld(self.plant_context, self.plant.GetBodyByName(\"ball\")).translation()\n",
    "        ball_velocity = self.plant.EvalBodySpatialVelocityInWorld(self.plant_context, self.plant.GetBodyByName(\"ball\")).translational()\n",
    "        self.diagram.GetInputPort(\"v_ball_pose\").FixValue(self.context, ball_pose)\n",
    "        self.diagram.GetInputPort(\"v_ball_velocity\").FixValue(self.context, ball_velocity)\n",
    "\n",
    "        if display_pose:\n",
    "            transform = self.plant.EvalBodyPoseInWorld(self.plant_context, self.plant.GetBodyByName(\"base_link\")).GetAsMatrix4()\n",
    "            AddTriad(self.visualizer.vis, name=f\"paddle_{round(self.time, 1)}\", prefix='', length=0.15, radius=.006)\n",
    "            self.visualizer.vis[''][f\"paddle_{round(self.time, 1)}\"].set_transform(transform)\n",
    "\n",
    "        if simulate:\n",
    "\n",
    "            self.visualizer.StartRecording()\n",
    "            self.simulator.AdvanceTo(self.time + duration)\n",
    "            self.visualizer.StopRecording()\n",
    "\n",
    "            self.position_log.append(self.station.GetOutputPort(\"iiwa_position_measured\").Eval(self.station_context))\n",
    "            self.velocity_log.append(self.station.GetOutputPort(\"iiwa_velocity_estimated\").Eval(self.station_context))\n",
    "\n",
    "            if verbose:\n",
    "                print(\"Time: {}\\nMeasured Position: {}\\n\\n\".format(round(self.time, 3), np.around(self.station.GetOutputPort(\"iiwa_position_measured\").Eval(self.station_context), 3)))\n",
    "\n",
    "            if final:\n",
    "                self.visualizer.PublishRecording()\n",
    "                return self.time + duration\n",
    "\n",
    "        self.time += duration\n",
    "\n",
    "\n",
    "time_step = .001\n",
    "demo = Demo(time_step=time_step)\n",
    "\n",
    "\n",
    "seconds = 2\n",
    "for i in range(int(seconds*20)):\n",
    "    t = demo.step(duration=0.05, final=i==seconds*20-1, verbose=False, display_pose=False)\n",
    "    if t is not None and t != seconds:\n",
    "        break"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "cell_id": "00006-578df41a-ad48-4af9-9d50-4114108cc6f1",
    "deepnote_cell_type": "code",
    "deepnote_to_be_reexecuted": true,
    "execution_millis": 2,
    "execution_start": 1638675620277,
    "source_hash": "27fb5bf4",
    "tags": []
   },
   "outputs": [],
   "source": [
    "juggler.visualizer.PublishRecording()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "created_in_deepnote_cell": true,
    "deepnote_cell_type": "markdown",
    "tags": []
   },
   "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=b95b60bf-1e32-44f8-a07c-9240db62d20a' 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>\n",
    "Created in <span style='font-weight:600;margin-left:4px;'>Deepnote</span></a>"
   ]
  }
 ],
 "metadata": {
  "deepnote": {},
  "deepnote_execution_queue": [],
  "deepnote_notebook_id": "1540b228-ab8b-46dc-97fd-6e8d2ad9648e",
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}