From 191bc552fc9f4fa762547f1090289be0c4159870 Mon Sep 17 00:00:00 2001 From: victorlouisdg Date: Fri, 8 Mar 2024 11:00:08 +0100 Subject: [PATCH] add static_gripper to add_manipulator() --- airo_drake/building/manipulator.py | 10 +++++ notebooks/02_visualization.ipynb | 8 ++-- notebooks/03_collision_checking.ipynb | 53 +++------------------------ pyproject.toml | 2 +- 4 files changed, 22 insertions(+), 51 deletions(-) diff --git a/airo_drake/building/manipulator.py b/airo_drake/building/manipulator.py index 86a7722..38b38d8 100644 --- a/airo_drake/building/manipulator.py +++ b/airo_drake/building/manipulator.py @@ -20,6 +20,7 @@ def add_manipulator( gripper_name: str, arm_transform: HomogeneousMatrixType | None = None, gripper_transform: HomogeneousMatrixType | None = None, + static_gripper: bool = False, ) -> Tuple[ModelInstanceIndex, ModelInstanceIndex]: """Add a manipulator (a robot arm with a gripper) to the robot diagram builder. Looks up the URDF files for the robot and gripper and welds them together. @@ -33,6 +34,7 @@ def add_manipulator( gripper_name: The name of the gripper, must be known by airo-models arm_transform: The transform of the robot arm, if None, we use supply a robot-specific default. gripper_transform: The transform of the gripper, if None, we supply a default for the robot-gripper pair. + static_gripper: If True, will fix all gripper joints to their default. Useful when you don't want the gripper DoFs in the plant. Returns: The robot and gripper index. @@ -47,6 +49,14 @@ def add_manipulator( gripper_urdf_path = airo_models.get_urdf_path(gripper_name) arm_index = parser.AddModels(arm_urdf_path)[0] + + if static_gripper: + gripper_urdf = airo_models.urdf.read_urdf(gripper_urdf_path) + airo_models.urdf.make_static(gripper_urdf) + gripper_urdf_path = airo_models.urdf.write_urdf_to_tempfile( + gripper_urdf, gripper_urdf_path, prefix=f"{gripper_name}_static_" + ) + gripper_index = parser.AddModels(gripper_urdf_path)[0] # Weld some frames together diff --git a/notebooks/02_visualization.ipynb b/notebooks/02_visualization.ipynb index 2aa4f23..c05eab6 100644 --- a/notebooks/02_visualization.ipynb +++ b/notebooks/02_visualization.ipynb @@ -256,11 +256,13 @@ " return robot_diagram\n", "\n", "\n", - "robot_diagram_without_gripper = create_robot_diagram_without_gripper()\n", + "robot_diagram_builder = RobotDiagramBuilder()\n", + "add_manipulator(robot_diagram_builder, \"ur5e\", \"robotiq_2f_85\", static_gripper=True)\n", + "robot_diagram_with_static_gripper, _ = finish_build(robot_diagram_builder)\n", "\n", - "print(\"DoFs in scene without gripper:\", robot_diagram_without_gripper.plant().num_positions())\n", + "print(\"DoFs in scene with static gripper:\", robot_diagram_with_static_gripper.plant().num_positions())\n", "joint_trajectory = time_parametrize_toppra(\n", - " robot_diagram_without_gripper.plant(), joint_path, joint_acceleration_limit=1.0\n", + " robot_diagram_with_static_gripper.plant(), joint_path, joint_acceleration_limit=1.0\n", ")" ] }, diff --git a/notebooks/03_collision_checking.ipynb b/notebooks/03_collision_checking.ipynb index e025621..2c30393 100644 --- a/notebooks/03_collision_checking.ipynb +++ b/notebooks/03_collision_checking.ipynb @@ -36,52 +36,14 @@ "source": [ "import numpy as np\n", "from pydrake.planning import RobotDiagramBuilder\n", - "from airo_drake import add_floor, add_meshcat, finish_build, X_URTOOL0_ROBOTIQ, X_URBASE_ROSBASE\n", - "from airo_drake import SingleArmScene\n", - "import airo_models\n", + "from airo_drake import SingleArmScene, add_floor, add_manipulator, add_meshcat, finish_build\n", "\n", "robot_diagram_builder = RobotDiagramBuilder()\n", "\n", "meshcat = add_meshcat(robot_diagram_builder)\n", + "arm_index, gripper_index = add_manipulator(robot_diagram_builder, \"ur5e\", \"robotiq_2f_85\", static_gripper=True)\n", "add_floor(robot_diagram_builder)\n", - "\n", - "plant = robot_diagram_builder.plant()\n", - "parser = robot_diagram_builder.parser()\n", - "parser.SetAutoRenaming(True)\n", - "\n", - "# Load URDF files\n", - "arm_name = \"ur5e\"\n", - "gripper_name = \"robotiq_2f_85\"\n", - "arm_urdf_path = airo_models.get_urdf_path(arm_name)\n", - "gripper_urdf_path = airo_models.get_urdf_path(gripper_name)\n", - "\n", - "# Make the gripper static\n", - "gripper_urdf = airo_models.urdf.read_urdf(gripper_urdf_path)\n", - "airo_models.urdf.replace_value(gripper_urdf, \"@type\", \"revolute\", \"fixed\")\n", - "airo_models.urdf.delete_key(gripper_urdf, \"mimic\")\n", - "airo_models.urdf.delete_key(gripper_urdf, \"transmission\")\n", - "gripper_static_urdf_path = airo_models.urdf.write_urdf_to_tempfile(\n", - " gripper_urdf, gripper_urdf_path, prefix=f\"{gripper_name}_static_\"\n", - ")\n", - "\n", - "# Use static gripper\n", - "arm_index = parser.AddModels(arm_urdf_path)[0]\n", - "gripper_index = parser.AddModels(gripper_static_urdf_path)[0]\n", - "\n", - "# Weld some frames together\n", - "world_frame = plant.world_frame()\n", - "arm_frame = plant.GetFrameByName(\"base_link\", arm_index)\n", - "arm_tool_frame = plant.GetFrameByName(\"tool0\", arm_index)\n", - "gripper_frame = plant.GetFrameByName(\"base_link\", gripper_index)\n", - "\n", - "arm_rigid_transform = X_URBASE_ROSBASE\n", - "gripper_rigid_transform = X_URTOOL0_ROBOTIQ\n", - "\n", - "plant.WeldFrames(world_frame, arm_frame, arm_rigid_transform)\n", - "plant.WeldFrames(arm_tool_frame, gripper_frame, gripper_rigid_transform)\n", - "\n", - "robot_diagram, context = finish_build(robot_diagram_builder, meshcat)\n", - "del robot_diagram_builder # no longer needed\n", + "robot_diagram, context = finish_build(robot_diagram_builder)\n", "\n", "scene = SingleArmScene(robot_diagram, arm_index, gripper_index, meshcat)\n", "scene" @@ -93,6 +55,7 @@ "metadata": {}, "outputs": [], "source": [ + "plant = scene.robot_diagram.plant()\n", "plant.num_positions(), plant.num_positions(arm_index), plant.num_positions(gripper_index)" ] }, @@ -424,8 +387,8 @@ "metadata": {}, "outputs": [], "source": [ - "for path in paths:\n", - " print(path_collisions_as_emojis(collision_checker.CheckConfigsCollisionFree(path)))" + "for i, path in enumerate(paths):\n", + " print(f\"path {i}: {path_collisions_as_emojis(collision_checker.CheckConfigsCollisionFree(path))}\")" ] }, { @@ -652,10 +615,6 @@ " level_tip_path.append(tip_position_rotated)\n", "\n", "\n", - "print(level_tip_path[0])\n", - "print(level_tip_path[1])\n", - "print(level_tip_path[-1])\n", - "\n", "for i, tip_position in enumerate(level_tip_path):\n", " meshcat.SetTransform(f\"lever/tip_position_{i}\", RigidTransform(p=tip_position))\n", " meshcat.SetObject(f\"lever/tip_position_{i}\", Sphere(0.005), magenta)" diff --git a/pyproject.toml b/pyproject.toml index c2c2862..012924e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -18,7 +18,7 @@ classifiers = [ "Operating System :: OS Independent", ] -dependencies = ["airo-models", "drake", "ur-analytic-ik>=0.0.4", "loguru"] +dependencies = ["airo-models>=0.0.8", "drake", "ur-analytic-ik>=0.0.4", "loguru"] [project.urls] Homepage = "https://github.com/airo-ugent/airo-drake"