spatial_projectile_matching_with_SNOPT.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "source": "# Ping Pong Catching\n\n### Spatial Projectile Matching with SNOPT\n\n_Final Project for 6.843, Fall 2021_\n\n_Viraj Parimi, Cameron Pittman_",
   "metadata": {
    "cell_id": "00000-efeb3bfa-121e-41aa-a6c1-9afedb8d820e",
    "deepnote_cell_type": "markdown"
   }
  },
  {
   "cell_type": "code",
   "metadata": {
    "deepnote_to_be_reexecuted": false,
    "execution_millis": 2459,
    "execution_start": 1638987316292,
    "source_hash": "324d63b0",
    "cell_id": "00001-ebe3ee43-6cef-4586-9aa2-cdfc6d8e70b6",
    "deepnote_cell_type": "code"
   },
   "source": "import time\nimport pydot\nimport numpy as np\nfrom manipulation.meshcat_cpp_utils import (\n    StartMeshcat\n)\nfrom pydrake.systems.meshcat_visualizer import MeshcatVisualizer\nfrom meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n\nfrom pydrake.all import (\n    BasicVector, DiagramBuilder, Integrator, JacobianWrtVariable, LeafSystem, MeshcatVisualizerCpp,\n    MeshcatVisualizerParams, Simulator, MathematicalProgram, SnoptSolver, Solve\n)\n\nfrom iiwa_paddle_station import MakeIiwaPaddleStation\nproc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=[])",
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {
    "tags": [],
    "deepnote_to_be_reexecuted": false,
    "source_hash": "67118576",
    "execution_start": 1638987318759,
    "execution_millis": 40,
    "deepnote_output_heights": [
     21.1875
    ],
    "is_output_hidden": false,
    "cell_id": "00002-f3e54463-a61e-44bb-bc58-2d52bd1528d1",
    "deepnote_cell_type": "code"
   },
   "source": "meshcat = StartMeshcat()",
   "outputs": [
    {
     "data": {
      "text/plain": "<IPython.core.display.HTML object>",
      "text/html": "Meshcat is now available at <a href=\"https://b95b60bf-1e32-44f8-a07c-9240db62d20a.deepnoteproject.com\">https://b95b60bf-1e32-44f8-a07c-9240db62d20a.deepnoteproject.com</a>"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "source": "### Define Controller",
   "metadata": {
    "tags": [],
    "cell_id": "00003-47963c2f-93e6-45a8-bcb0-1aabf882baa3",
    "deepnote_cell_type": "markdown"
   }
  },
  {
   "cell_type": "code",
   "metadata": {
    "tags": [],
    "deepnote_to_be_reexecuted": false,
    "source_hash": "3b227cd2",
    "execution_start": 1638987319064,
    "execution_millis": 0,
    "cell_id": "00004-a5a89354-57ba-4b91-8d6b-63d7977b70b4",
    "deepnote_cell_type": "code"
   },
   "source": "class DifferentialIKSystem(LeafSystem):\n    \"\"\"\n        Wrapper system for Differential IK. \n\n        Args:\n            plant (MultibodyPlant): multibody plant object of the simulated plant. \n            diffik_fun (function): function object that handles diffik. Must have the signature \n                                   diffik_fun(J_G, V_G_desired).\n    \"\"\"\n    \n    def __init__(self, plant, diffik_fun):\n\n        LeafSystem.__init__(self)\n\n        self._plant = plant\n        self._W = plant.world_frame()\n        self._diffik_fun = diffik_fun\n        self._plant_context = plant.CreateDefaultContext()\n        self._iiwa = plant.GetModelInstanceByName(\"iiwa7\")\n        self._G = plant.GetBodyByName(\"base_link\").body_frame()\n\n        # Inputs\n        self.DeclareVectorInputPort(\"spatial_velocity\", BasicVector(6))\n        self.DeclareVectorInputPort(\"iiwa_position_measured\", BasicVector(7))\n        self.DeclareVectorInputPort(\"iiwa_velocity_measured\", BasicVector(7))\n\n        # Outputs\n        self.DeclareVectorOutputPort(\"iiwa_velocity_command\", BasicVector(7), self.CalcOutput)\n        \n    def CalcOutput(self, context, output):\n\n        V_G_desired = self.get_input_port(0).Eval(context)\n        \n        q_now = self.get_input_port(1).Eval(context)\n        v_now = self.get_input_port(2).Eval(context)\n        self._plant.SetPositions(self._plant_context, self._iiwa, q_now)\n\n        J_G = self._plant.CalcJacobianSpatialVelocity(\n            self._plant_context, \n            JacobianWrtVariable.kQDot, \n            self._G, \n            [0, 0, 0],\n            self._W,\n            self._W\n        )\n        J_G = J_G[:, 0:7]\n        \n        v = self._diffik_fun(J_G, V_G_desired) \n        output.SetFromVector(v)",
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "source": "### Define the Differential Inverse Kinematics Mathematical Program",
   "metadata": {
    "tags": [],
    "cell_id": "00005-113fc8dc-a735-4aad-9c11-5d9336b1ab80",
    "deepnote_cell_type": "markdown"
   }
  },
  {
   "cell_type": "code",
   "metadata": {
    "tags": [],
    "deepnote_to_be_reexecuted": false,
    "source_hash": "a2a0d78c",
    "execution_start": 1638987334748,
    "execution_millis": 2,
    "cell_id": "00006-93b04e7d-43bc-45f2-bfdf-d4434feba914",
    "deepnote_cell_type": "code"
   },
   "source": "def DiffIKQP(J_G, V_G_desired):\n\n    prog = MathematicalProgram()\n    v = prog.NewContinuousVariables(7, 'v')\n    v_max = 3.0\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    return v_solution",
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "source": "### Constants",
   "metadata": {
    "tags": [],
    "cell_id": "00007-40c3ace6-a988-428f-b974-36b379382923",
    "deepnote_cell_type": "markdown"
   }
  },
  {
   "cell_type": "code",
   "metadata": {
    "tags": [],
    "deepnote_to_be_reexecuted": false,
    "source_hash": "81b94ad3",
    "execution_start": 1638987336770,
    "execution_millis": 1,
    "cell_id": "00008-3bbc8821-048b-4861-ac94-c27123d086a6",
    "deepnote_cell_type": "code"
   },
   "source": "iiwa_q0 = np.array([-1.57, 0, 0, -np.pi/2, 0, 0, 0])\npaddle_state_pos = np.array([0.07659419, -0.75730758, 0.63302061])\n\ng = -9.81\nheight = 1\noffset = 0.5\nCATCH_HEIGHT = 0.55\nsimulation_time = 3\nball_radius = 0.035\nmax_angle = np.pi / 6\npaddle_radius = 0.175\ne_z = np.array([0, 0, 1], dtype = float)",
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "source": "### Define Initial States",
   "metadata": {
    "tags": [],
    "cell_id": "00009-dfb8d521-1e24-4501-a724-477f5b9d5143",
    "deepnote_cell_type": "markdown"
   }
  },
  {
   "cell_type": "code",
   "metadata": {
    "tags": [],
    "deepnote_to_be_reexecuted": false,
    "source_hash": "1d5ff911",
    "execution_start": 1638987340551,
    "execution_millis": 1,
    "cell_id": "00010-8fb6f59d-ea07-4a98-b843-0479e0494652",
    "deepnote_cell_type": "code"
   },
   "source": "# Experiment 1 - drop on top of the paddle\nball_q0_exp1 = [1, 1, 1, 1, paddle_state_pos[0], paddle_state_pos[1], paddle_state_pos[2] + height, 0, 0, 0, 0, 0, 0]\n\n# Experiment 2 - drop from (+ve x) offset with (-ve x) velocity\nball_q0_exp2 = [1, 1, 1, 1, paddle_state_pos[0] + offset, paddle_state_pos[1], paddle_state_pos[2] + height, 0, 0, 0, -1, 0, 0]\n\n# Experiment 3 - drop from (-ve x) offset with (+ve x) velocity\nball_q0_exp3 = [1, 1, 1, 1, paddle_state_pos[0] - offset, paddle_state_pos[1], paddle_state_pos[2] + height, 0, 0, 0, 1, 0, 0]\n\n# Experiment 4 - drop from (-ve y) offset with (+ve y) velocity\nball_q0_exp4 = [1, 1, 1, 1, paddle_state_pos[0], paddle_state_pos[1] - offset, paddle_state_pos[2] + height, 0, 0, 0, 0, 1, 0]\n\n# Experiment 5 - drop from (+ve y) offset with (-ve y) velocity\nball_q0_exp5 = [1, 1, 1, 1, paddle_state_pos[0], paddle_state_pos[1] + offset, paddle_state_pos[2] + height, 0, 0, 0, 0, -1, 0]\n\n# Experiment 6 - drop from (-ve x, -ve y) offset with (+ve x, +ve y) velocity\nball_q0_exp6 = [1, 1, 1, 1, paddle_state_pos[0] - offset, paddle_state_pos[1] - offset, paddle_state_pos[2] + height, 0, 0, 0, 1, 1, 0]\n\n# Experiment 7 - drop from (-ve x, +ve y) offset with (+ve x, -ve y) velocity\nball_q0_exp7 = [1, 1, 1, 1, paddle_state_pos[0] - offset, paddle_state_pos[1] + offset, paddle_state_pos[2] + height, 0, 0, 1, -1, 0, 0]\n\n# Experiment 8 - drop from (+ve x, -ve y) offset with (-ve x, +ve y) velocity\nball_q0_exp8 = [1, 1, 1, 1, paddle_state_pos[0] + offset, paddle_state_pos[1] - offset, paddle_state_pos[2] + height, 0, 0, 0, -1, 1, 0]\n\n# Experiment 9 - drop from (+ve x, +ve y) offset with (-ve x, -ve y) velocity\nball_q0_exp9 = [1, 1, 1, 1, paddle_state_pos[0] + offset, paddle_state_pos[1] + offset, paddle_state_pos[2] + height, 0, 0, 0, -1, -1, 0]\n\nscenarios = [ball_q0_exp1, ball_q0_exp2, ball_q0_exp3, ball_q0_exp4, ball_q0_exp5, ball_q0_exp6, ball_q0_exp7, ball_q0_exp8, ball_q0_exp9]",
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "source": "### Utility Functions",
   "metadata": {
    "tags": [],
    "cell_id": "00011-b96f0468-adbb-431f-a6cf-a7b32b68a651",
    "deepnote_cell_type": "markdown"
   }
  },
  {
   "cell_type": "code",
   "metadata": {
    "tags": [],
    "deepnote_to_be_reexecuted": false,
    "source_hash": "5c9d8374",
    "execution_start": 1638987343127,
    "execution_millis": 17,
    "cell_id": "00012-8fce15d9-ba7f-4c7d-9f41-678427a4f344",
    "deepnote_cell_type": "code"
   },
   "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\ndef 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\ndef 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    if np.isclose(b_z, p_z, atol=atol):\n        return \"contact\"\n    else:\n        return \"rising\" if vz >= 0 else \"falling\"",
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "source": "### Define the Catcher class",
   "metadata": {
    "tags": [],
    "cell_id": "00013-5178fc77-2613-46fb-8383-0bbcd2befcf1",
    "deepnote_cell_type": "markdown"
   }
  },
  {
   "cell_type": "code",
   "metadata": {
    "tags": [],
    "deepnote_to_be_reexecuted": false,
    "source_hash": "19324687",
    "execution_start": 1638987345605,
    "execution_millis": 1,
    "cell_id": "00014-c74a210c-d957-457f-a13d-0ffbe59701b0",
    "deepnote_cell_type": "code"
   },
   "source": "class Catcher(LeafSystem):\n    \"\"\"\n    Catcher class that computes the trajectory of ball and outputs desired velocity estimates of the\n    paddle to reach the ball at a fixed height. This approach does not compute the desired angular \n    velocity estimates of the paddle.\n    \"\"\"\n\n    def __init__(self, plant, exp_idx):\n\n        LeafSystem.__init__(self)\n        \n        self.plant = plant\n        self.t_catch = None\n        self.made_contact = False\n        self.W = plant.world_frame()\n        self.P = plant.GetBodyByName(\"base_link\")\n        self.iiwa = plant.GetModelInstanceByName(\"iiwa7\")\n        self.plant_context = plant.CreateDefaultContext()\n        self.p_b_0 = np.array(ball_p_from_state(scenarios[exp_idx]))\n        self.v_b_0 = np.array(ball_v_from_state(scenarios[exp_idx]))\n\n        # Inputs\n        self.DeclareVectorInputPort(\"ball_state\", BasicVector(13))\n        self.DeclareVectorInputPort(\"iiwa_pos_measured\", BasicVector(7))\n        self.DeclareVectorInputPort(\"iiwa_velocity_estimated\", BasicVector(7))\n        \n        # Outputs\n        self.DeclareVectorOutputPort(\"paddle_desired_spatial_velocity\", BasicVector(6), self.CalcDesSpatialVelOutput)\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 CalcDesSpatialVelOutput(self, context, output):\n\n        q = self.GetInputPort(\"iiwa_pos_measured\").Eval(context)\n        v = self.GetInputPort(\"iiwa_velocity_estimated\").Eval(context)\n        self.plant.SetPositionsAndVelocities(self.plant_context, self.iiwa, np.hstack([q, v]))\n        \n        state_Ball = np.array(self.GetInputPort(\"ball_state\").Eval(context))\n        p_Ball = ball_p_from_state(state_Ball)\n        v_Ball = ball_v_from_state(state_Ball)\n\n        p_Paddle = self.plant.EvalBodyPoseInWorld(self.plant_context, self.P).translation()\n        w_Paddle = self.plant.EvalBodySpatialVelocityInWorld(self.plant_context, self.P).rotational()\n        v_Paddle = self.plant.EvalBodySpatialVelocityInWorld(self.plant_context, self.P).translational()\n\n        mode = get_mode(p_Ball, v_Ball, p_Paddle, atol=1e-01)\n\n        if not self.made_contact and mode == \"contact\":\n            self.made_contact = True        \n\n        gain = np.array([1., 1., 1.])\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_percent, y_offset_percent, z_offset_percent] = (dist_ball_and_paddle) / paddle_radius\n\n        w_p_x = y_offset_percent * max_angle\n        w_p_y = x_offset_percent * max_angle * -1\n        w_p_z = 0\n\n        w_P_desired = np.array([w_p_x, w_p_y, w_p_z] - w_Paddle) * gain\n        # w_P_desired = np.array([w_p_x, w_p_y, w_p_z]) * gain\n\n        if not self.made_contact:\n            \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 = self.p_b_0 + self.v_b_0 * self.t_catch + (a_b / 2) * self.t_catch**2\n            v_b_catch = self.v_b_0 + a_b * self.t_catch\n\n            v_p_0 = (p_catch - paddle_state_pos - 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 * context.get_time()\n\n        else:\n\n            # Ball should be moving slowly so switch to PD behavior after the intial contact\n            kwp = 3\n            kwd = 7\n            w_P_desired = kwp * w_P_desired + kwd * (v_Ball - v_Paddle)\n\n            kvp = 3\n            kvd = 0.1\n            v_P_desired = kvp * (dist_ball_and_paddle) + kvd * (v_Ball - v_Paddle)\n \n        V_P = np.concatenate([w_P_desired, v_P_desired])\n        output.SetFromVector(V_P)\n",
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "source": "### Experimental Setup",
   "metadata": {
    "tags": [],
    "cell_id": "00015-20638fee-fdd7-4ab9-a858-5d88dae1fa28",
    "deepnote_cell_type": "markdown"
   }
  },
  {
   "cell_type": "code",
   "metadata": {
    "deepnote_output_heights": [
     596.8125,
     596.8125
    ],
    "deepnote_to_be_reexecuted": false,
    "source_hash": "ba7a3182",
    "execution_start": 1638987348491,
    "execution_millis": 1,
    "cell_id": "00016-8de74d5e-64c5-460b-9696-81ef65d9931f",
    "deepnote_cell_type": "code"
   },
   "source": "class Demo:\n    def __init__(self, time_step=0.001, exp_idx = 0):\n        \"\"\"\n        Robotic Kuka iiwa with paddle end effector\n\n        Args:\n            time_step (float, optional): time step for internal manipulation station controller. Defaults to 0.001.\n            exp_idx (int, optional): experiment index to run. Defaults to 0.\n        \"\"\"\n        \n        self.time = 0\n        self.metric = 0\n        \n        diagram, plant, scene_graph = MakeIiwaPaddleStation(time_step=time_step)\n        \n        self.plant = plant\n        self.builder = DiagramBuilder()\n        self.station = self.builder.AddSystem(diagram)\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        self.plant.mutable_gravity_field().set_gravity_vector([0, 0, g])\n\n        diff_ik_sys = self.builder.AddSystem(DifferentialIKSystem(self.plant, DiffIKQP))\n        diff_ik_sys.set_name(\"diff_ik_sys\")\n        catcher = self.builder.AddSystem(Catcher(self.plant, exp_idx))\n        catcher.set_name(\"catcher\")\n        integrator = self.builder.AddSystem(Integrator(7))\n        integrator.set_name(\"integrator\")\n        \n        self.builder.Connect(integrator.get_output_port(), self.station.GetInputPort(\"iiwa_position\"))\n        self.builder.Connect(self.station.GetOutputPort(\"ball_state\"), catcher.GetInputPort(\"ball_state\"))\n        self.builder.Connect(diff_ik_sys.GetOutputPort(\"iiwa_velocity_command\"), integrator.get_input_port())\n        self.builder.Connect(self.station.GetOutputPort(\"iiwa_position_measured\"), catcher.GetInputPort(\"iiwa_pos_measured\"))\n        self.builder.Connect(catcher.GetOutputPort(\"paddle_desired_spatial_velocity\"), diff_ik_sys.GetInputPort(\"spatial_velocity\"))\n        self.builder.Connect(self.station.GetOutputPort(\"iiwa_velocity_estimated\"), catcher.GetInputPort(\"iiwa_velocity_estimated\"))\n        self.builder.Connect(self.station.GetOutputPort(\"iiwa_position_measured\"), diff_ik_sys.GetInputPort(\"iiwa_position_measured\"))\n        self.builder.Connect(self.station.GetOutputPort(\"iiwa_velocity_estimated\"), diff_ik_sys.GetInputPort(\"iiwa_velocity_measured\"))\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        self.plant.SetPositions(self.plant_context, self.plant.GetModelInstanceByName(\"iiwa7\"), np.array(iiwa_q0))\n        self.plant.SetPositionsAndVelocities(self.plant_context, self.plant.GetModelInstanceByName(\"ball\"), np.array(scenarios[exp_idx]))\n\n        self.station.GetInputPort(\"iiwa_feedforward_torque\").FixValue(self.station_context, np.zeros((7,1)))\n        iiwa_q = self.plant.GetPositions(self.plant_context, self.plant.GetModelInstanceByName(\"iiwa7\"))\n        integrator.GetMyContextFromRoot(self.context).get_mutable_continuous_state_vector().SetFromVector(iiwa_q)\n\n    def step(self, simulate=True, duration=0.1, final=True):\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        \"\"\"\n        if simulate:\n\n            self.visualizer.StartRecording()\n            self.simulator.AdvanceTo(self.time + duration)\n            self.visualizer.StopRecording()\n\n            # Compute the stability metric\n            contact_results = self.plant.get_contact_results_output_port().Eval(self.plant_context)\n            for i in range(contact_results.num_point_pair_contacts()):\n                info = contact_results.point_pair_contact_info(i)\n                bodyA = self.plant.get_body(info.bodyA_index()).name()\n                bodyB = self.plant.get_body(info.bodyB_index()).name()\n                if (bodyA == \"ball\" and bodyB == \"base_link\") or (bodyA == \"base_link\" and bodyB == \"ball\"):\n                    self.metric += duration\n\n            if final:\n                self.visualizer.PublishRecording()\n                return self.time + duration\n\n        self.time += duration",
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "source": "### Experiments",
   "metadata": {
    "tags": [],
    "cell_id": "00017-3d151972-5ea8-4e7c-93c0-e08052ee5995",
    "deepnote_cell_type": "markdown"
   }
  },
  {
   "cell_type": "code",
   "metadata": {
    "tags": [],
    "deepnote_to_be_reexecuted": false,
    "source_hash": "c374890b",
    "execution_start": 1638987354088,
    "execution_millis": 85229,
    "deepnote_output_heights": [
     40.375,
     577.625
    ],
    "cell_id": "00018-0f7e85fd-f98d-4f95-b23f-3a7f7863a0d9",
    "deepnote_cell_type": "code"
   },
   "source": "time_step = .0005\n\nfor idx, experiment in enumerate(scenarios):\n    \n    start_time = time.perf_counter()\n    demo = Demo(time_step=time_step, exp_idx=idx)\n\n    seconds = simulation_time\n    for i in range(int(seconds * 20)):\n        t = demo.step(duration=0.05, final=i==seconds * 20 - 1)\n        if t is not None and t != seconds:\n            break\n    end_time = time.perf_counter()\n\n    metric_val = demo.metric\n    print(\"Experiment: \", idx)\n    print(\"Stability Metric: \", metric_val)\n    print(\"Total Time(s): \", (end_time - start_time))",
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "source": "",
   "metadata": {
    "tags": [],
    "cell_id": "00019-d91c9769-26e0-42c5-ae7e-b57a06519b64",
    "deepnote_cell_type": "code"
   },
   "outputs": [],
   "execution_count": null
  },
  {
   "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=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>\nCreated in <span style='font-weight:600;margin-left:4px;'>Deepnote</span></a>",
   "metadata": {
    "tags": [],
    "created_in_deepnote_cell": true,
    "deepnote_cell_type": "markdown"
   }
  }
 ],
 "nbformat": 4,
 "nbformat_minor": 5,
 "metadata": {
  "deepnote": {},
  "deepnote_execution_queue": [],
  "deepnote_notebook_id": "5cdc1b87-ab19-42ad-9ea9-92bcbb9e8208",
  "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"
  }
 }
}