diff --git a/airo_planner/multiple_goal_planner.py b/airo_planner/multiple_goal_planner.py index 6c71a2a..bc21d96 100644 --- a/airo_planner/multiple_goal_planner.py +++ b/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 diff --git a/airo_planner/ompl/single_arm_ompl_planner.py b/airo_planner/ompl/single_arm_ompl_planner.py index 623164f..af5593d 100644 --- a/airo_planner/ompl/single_arm_ompl_planner.py +++ b/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( diff --git a/notebooks/03_dual_arm_planning.ipynb b/notebooks/03_dual_arm_planning.ipynb index 847d22c..3faecef 100644 --- a/notebooks/03_dual_arm_planning.ipynb +++ b/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,