a little clean up

airo_planner/multiple_goal_planner.py

@@ -121,7 +121,7 @@ def _plan_to_goal_configurations(
                 continue
 
             if return_first_success:
-                logger.info(f"Returning first successful path (planning time: {time.time() - start_time:.2f} s).")
+                logger.success(f"Returning first successful path (planning time: {time.time() - start_time:.2f} s).")
                 return path
             paths.append(path)
 
@@ -131,14 +131,16 @@ def _plan_to_goal_configurations(
             raise NoPathFoundError(start_configuration, goal_configurations)
 
         self._all_paths = paths  # Saved for debugging
-        logger.info(f"Found {len(paths)} paths towards IK solutions, (planning time: {end_time - start_time:.2f} s).")
+        logger.success(
+            f"Found {len(paths)} paths towards IK solutions, (planning time: {end_time - start_time:.2f} s)."
+        )
 
         solution_path = self.choose_path_fn(paths)
 
         if solution_path is None:
             raise RuntimeError(f"Path choosing function did not return a path out of {len(paths)} options.")
 
-        logger.info(f"Chose path with {len(solution_path)} waypoints.")
+        logger.success(f"Chose path with {len(solution_path)} waypoints.")
 
         return solution_path
 

airo_planner/ompl/single_arm_ompl_planner.py

@@ -124,6 +124,8 @@ def plan_to_joint_configuration(
         if path is None:
             raise NoPathFoundError(start_configuration, goal_configuration)
 
+        logger.success(f"Successfully found path (with {len(path)} waypoints).")
+
         return path
 
     def _plan_to_joint_configuration_stacked(

notebooks/03_dual_arm_planning.ipynb

@@ -82,6 +82,7 @@
     "robot_diagram_builder = RobotDiagramBuilder()\n",
     "\n",
     "meshcat = add_meshcat(robot_diagram_builder)\n",
+    "meshcat.SetCameraPose([-1.5, 0, 1.0], [0, 0, 0])\n",
     "\n",
     "arm_y = 0.45\n",
     "X_W_L = RigidTransform(rpy=RollPitchYaw([0, 0, np.pi / 2]), p=[0, arm_y, 0])\n",
@@ -458,6 +459,16 @@
     ")"
    ]
   },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "scene.meshcat.Delete(\"stretch_pose_left\")\n",
+    "scene.meshcat.Delete(\"stretch_pose_right\")"
+   ]
+  },
   {
    "cell_type": "markdown",
    "metadata": {},
@@ -533,6 +544,8 @@
    "source": [
     "robot_diagram_builder = RobotDiagramBuilder()\n",
     "meshcat = add_meshcat(robot_diagram_builder)\n",
+    "meshcat.SetCameraPose([-1.5, 0, 1.0], [0, 0, 0])\n",
+    "\n",
     "(arm_left_index, arm_right_index), (gripper_left_index, gripper_right_index) = add_cloth_competition_dual_ur5e_scene(\n",
     "    robot_diagram_builder, X_W_L, X_W_R, add_cylinder=True\n",
     ")\n",
@@ -909,6 +922,15 @@
     ")"
    ]
   },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "scene_with_cloth.meshcat.Delete(\"grasp_pose_hard\")"
+   ]
+  },
   {
    "cell_type": "code",
    "execution_count": null,