make rrt examples publish meshcat animations

Author: RussTedrake

data

@@ -18,8 +18,6 @@
    },
    "outputs": [],
    "source": [
-    "import time\n",
-    "\n",
     "import numpy as np\n",
     "from pydrake.all import (\n",
     "    AddMultibodyPlantSceneGraph,\n",
@@ -31,7 +29,7 @@
     ")\n",
     "\n",
     "from manipulation import running_as_notebook\n",
-    "from manipulation.scenarios import AddShape"
+    "from manipulation.scenarios import AddShape\n"
    ]
   },
   {
@@ -41,7 +39,7 @@
    "outputs": [],
    "source": [
     "# Start the visualizer.\n",
-    "meshcat = StartMeshcat()"
+    "meshcat = StartMeshcat()\n"
    ]
   },
   {
@@ -69,26 +67,41 @@
     "\n",
     "    meshcat.Delete()\n",
     "    meshcat.Set2dRenderMode(xmin=0, xmax=1, ymin=0, ymax=1)\n",
+    "    meshcat.StartRecording(set_visualizations_while_recording=False)\n",
     "\n",
     "    start = np.empty((N, 3))\n",
     "    end = np.empty((N, 3))\n",
-    "    id = 0\n",
+    "    last_plotted_index = 0\n",
+    "    plot_count = 1\n",
     "    for n in range(1, N):\n",
     "        q_sample = rng.random((1, 2))[0]\n",
     "        distance_sq = np.sum((Q[:n] - q_sample) ** 2, axis=1)\n",
     "        closest = np.argmin(distance_sq)\n",
     "        distance = np.sqrt(distance_sq[closest])\n",
     "        if distance > 0.1:\n",
     "            q_sample = Q[closest] + (0.1 / distance) * (q_sample - Q[closest])\n",
-    "        start[n] = [Q[closest, 0], 0, Q[closest, 1]]\n",
-    "        end[n] = [q_sample[0], 0, q_sample[1]]\n",
+    "        start[n - 1] = [Q[closest, 0], 0, Q[closest, 1]]\n",
+    "        end[n - 1] = [q_sample[0], 0, q_sample[1]]\n",
     "        if (n < 1000 and n % 100 == 1) or n % 1000 == 1:\n",
-    "            meshcat.SetLineSegments(\"rrt\", start[: n + 1].T, end[: n + 1].T)\n",
-    "            time.sleep(0.1)  # sleep to slow things down.\n",
+    "            meshcat.SetLineSegments(\n",
+    "                f\"rrt/{last_plotted_index}\",\n",
+    "                start[last_plotted_index:n].T,\n",
+    "                end[last_plotted_index:n].T,\n",
+    "            )\n",
+    "            meshcat.SetProperty(\n",
+    "                f\"rrt/{last_plotted_index}\", \"visible\", False, 0\n",
+    "            )\n",
+    "            meshcat.SetProperty(\n",
+    "                f\"rrt/{last_plotted_index}\", \"visible\", True, 0.25 * plot_count\n",
+    "            )\n",
+    "            plot_count = plot_count + 1\n",
+    "            last_plotted_index = n\n",
     "        Q[n] = q_sample\n",
     "\n",
+    "    meshcat.PublishRecording()\n",
+    "\n",
     "\n",
-    "basic_rrt()"
+    "basic_rrt()\n"
    ]
   },
   {
@@ -173,6 +186,7 @@
     "    meshcat.Set2dRenderMode(xmin=0, xmax=1, ymin=0, ymax=1)\n",
     "\n",
     "    visualizer = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)\n",
+    "    meshcat.StartRecording(set_visualizations_while_recording=False)\n",
     "\n",
     "    diagram = builder.Build()\n",
     "    context = diagram.CreateDefaultContext()\n",
@@ -193,6 +207,8 @@
     "\n",
     "    max_length = thickness / 4\n",
     "    n = 1\n",
+    "    last_plotted_index = 0\n",
+    "    plot_count = 1\n",
     "    while n < N:\n",
     "        q_sample = rng.random((1, 2))[0]\n",
     "        distance_sq = np.sum((Q[:n] - q_sample) ** 2, axis=1)\n",
@@ -207,17 +223,28 @@
     "        ):\n",
     "            # Then the sample point is in collision...\n",
     "            continue\n",
-    "        start[n] = [Q[closest, 0], 0, Q[closest, 1]]\n",
-    "        end[n] = [q_sample[0], 0, q_sample[1]]\n",
+    "        start[n - 1] = [Q[closest, 0], 0, Q[closest, 1]]\n",
+    "        end[n - 1] = [q_sample[0], 0, q_sample[1]]\n",
     "        if (n < 1000 and n % 100 == 1) or n % 1000 == 1:\n",
-    "            meshcat.SetLineSegments(\"rrt\", start[: n + 1].T, end[: n + 1].T)\n",
-    "            time.sleep(0.1)  # sleep to slow things down.\n",
-    "\n",
+    "            meshcat.SetLineSegments(\n",
+    "                f\"rrt/{last_plotted_index}\",\n",
+    "                start[last_plotted_index:n].T,\n",
+    "                end[last_plotted_index:n].T,\n",
+    "            )\n",
+    "            meshcat.SetProperty(\n",
+    "                f\"rrt/{last_plotted_index}\", \"visible\", False, 0\n",
+    "            )\n",
+    "            meshcat.SetProperty(\n",
+    "                f\"rrt/{last_plotted_index}\", \"visible\", True, 0.25 * plot_count\n",
+    "            )\n",
+    "            plot_count = plot_count + 1\n",
+    "            last_plotted_index = n\n",
     "        Q[n] = q_sample\n",
     "        n += 1\n",
+    "    meshcat.PublishRecording()\n",
     "\n",
     "\n",
-    "rrt_bugtrap()"
+    "rrt_bugtrap()\n"
    ]
   },
   {
@@ -249,7 +276,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.8.10"
+   "version": "3.11.2"
   },
   "vscode": {
    "interpreter": {