add static_gripper to add_manipulator()

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

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",
     ")"
    ]
   },

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)"

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"