Kitchen scene

src/moveit_pro_franka_configs/franka_base_config/config/moveit/franka.srdf

@@ -1,23 +1,28 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot name="franka">
+
   <!-- Planning groups -->
-  <group name="arm">
-    <chain base_link="fr3_link0" tip_link="fr3_link8"/>
-  </group>
-  <group name="hand">
-    <link name="fr3_hand"/>
+
+  <!-- All gripper links, excluding arm. -->
+  <group name="gripper">
+    <link name="fr3_hand_tcp"/> <!-- root of gripper -->
     <link name="fr3_leftfinger"/>
     <link name="fr3_rightfinger"/>
-    <link name="fr3_hand_tcp"/>
     <link name="grasp_link"/>
     <joint name="fr3_finger_joint1"/>
-    <passive_joint name="$fr3_finger_joint2"/>
+    <passive_joint name="fr3_finger_joint2"/>
   </group>
+
+  <!-- All robot links, including gripper. -->
   <group name="manipulator">
     <chain base_link="fr3_link0" tip_link="grasp_link"/>
   </group>
+
+  <!-- End effectors -->
+  <end_effector name="hand_tcp" parent_link="grasp_link" group="gripper" parent_group="manipulator"/>
+
   <!-- Arm group states -->
-  <group_state name="ready" group="arm">
+  <group_state name="ready" group="manipulator">
     <joint name="fr3_joint1" value="0"/>
     <joint name="fr3_joint2" value="-0.7853"/>
     <joint name="fr3_joint3" value="0"/>
@@ -26,16 +31,14 @@
     <joint name="fr3_joint6" value="1.5707"/>
     <joint name="fr3_joint7" value="0.7853"/>
   </group_state>
-  <group_state name="open" group="hand">
+  <group_state name="open" group="gripper">
     <joint name="fr3_finger_joint1" value="0.035"/>
     <joint name="fr3_finger_joint2" value="0.035"/>
   </group_state>
-  <group_state name="close" group="hand">
+  <group_state name="close" group="gripper">
     <joint name="fr3_finger_joint1" value="0"/>
     <joint name="fr3_finger_joint2" value="0"/>
   </group_state>
-  <!-- End effectors -->
-  <end_effector name="hand_tcp" parent_link="fr3_link8" group="hand" parent_group="arm"/>
 
   <!-- Arm collisions -->
   <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="fr3_link0"/>

src/moveit_pro_franka_configs/franka_base_config/objectives/segment_image_from_no_negative_text_prompt_subtree.xml

@@ -0,0 +1,64 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root
+  BTCPP_format="4"
+  main_tree_to_execute="Segment Image from No Negative Text Prompt Subtree"
+>
+  <!--//////////-->
+  <BehaviorTree
+    ID="Segment Image from No Negative Text Prompt Subtree"
+    _description="Run text based segmentation and visualize the masks."
+    _favorite="true"
+  >
+    <Control ID="Sequence">
+      <Action
+        ID="GetImage"
+        timeout_sec="5.000000"
+        topic_name="{image_topic_name}"
+        message_out="{image}"
+      />
+      <Action
+        ID="GetMasks2DFromTextQuery"
+        image="{image}"
+        masks2d="{masks2d}"
+        clipseg_model_path="{clipseg_model_path}"
+        clip_model_path="{clip_model_path}"
+        model_package="{model_package}"
+        prompts="{prompts}"
+        erosion_size="{erosion_size}"
+        threshold="{threshold}"
+      />
+      <Action
+        ID="PublishMask2D"
+        image="{image}"
+        masks="{masks2d}"
+        masks_visualization_topic="{masks_visualization_topic}"
+        opacity="0.500000"
+        bounding_box_detection_class="{prompts}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Segment Image from No Negative Text Prompt Subtree">
+      <MetadataFields>
+        <Metadata runnable="false" />
+        <Metadata subcategory="Perception - ML" />
+      </MetadataFields>
+      <MetadataFields>
+        <Metadata runnable="false" />
+        <Metadata subcategory="Perception - ML" />
+      </MetadataFields>
+      <input_port name="clip_model_path" default="{clip_model_path}" />
+      <input_port name="clipseg_model_path" default="{clipseg_model_path}" />
+      <input_port name="erosion_size" default="{erosion_size}" />
+      <input_port name="image_topic_name" default="{image_topic_name}" />
+      <input_port
+        name="masks_visualization_topic"
+        default="{masks_visualization_topic}"
+      />
+      <output_port name="masks2d" default="{masks2d}" />
+      <input_port name="model_package" default="{model_package}" />
+      <input_port name="prompts" default="{prompts}" />
+      <input_port name="threshold" default="{threshold}" />
+    </SubTree>
+  </TreeNodesModel>
+</root>

src/moveit_pro_franka_configs/franka_arm_sim/CMakeLists.txt → src/moveit_pro_franka_configs/kitchen_sim/CMakeLists.txt

@@ -1,12 +1,13 @@
 cmake_minimum_required(VERSION 3.22)
-project(franka_arm_sim)
+project(kitchen_sim)
 find_package(ament_cmake REQUIRED)
 
 install(
   DIRECTORY
     config
     description
     launch
+    models
     objectives
     waypoints
   DESTINATION

src/moveit_pro_franka_configs/franka_arm_sim/README.md → src/moveit_pro_franka_configs/kitchen_sim/README.md

@@ -1,4 +1,4 @@
-# franka_arm_sim
+# kitchen_sim
 
 A MoveIt Pro base configuration for the Franka arm.
 For detailed documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/).

src/moveit_pro_franka_configs/franka_arm_sim/config/config.yaml → src/moveit_pro_franka_configs/kitchen_sim/config/config.yaml

@@ -24,7 +24,7 @@ hardware:
 
   robot_description:
     urdf:
-      package: "franka_arm_sim"
+      package: "kitchen_sim"
       path: "description/franka.urdf.xacro"
     srdf:
       package: "franka_base_config"
@@ -34,6 +34,7 @@ hardware:
     # [Optional]
     urdf_params:
       - mujoco_model: "description/mujoco/scene.xml"
+      - mujoco_viewer: false
     simulated_hardware_launch_file:
       package: "moveit_studio_agent"
       path: "launch/blank.launch.py"
@@ -65,24 +66,25 @@ objectives:
       package_name: "moveit_pro_objectives"
       relative_path: "objectives/perception"
     sim_objectives:
-      package_name: "franka_arm_sim"
+      package_name: "kitchen_sim"
       relative_path: "objectives"
   # Specify the location of the saved waypoints file.
   # [Required]
   waypoints_file:
-    package_name: "franka_arm_sim"
+    package_name: "kitchen_sim"
     relative_path: "waypoints/waypoints.yaml"
 # Configuration for launching ros2_control processes.
 # [Required, if using ros2_control]
 ros2_control:
   config:
-    package: "franka_arm_sim"
+    package: "kitchen_sim"
     path: "config/control/franka_ros2_control.yaml"
   # MoveIt Pro will load and activate these controllers at start up to ensure they are available.
   # If not specified, it is up to the user to ensure the appropriate controllers are active and available
   # for running the application.
   # [Optional, default=[]]
   controllers_active_at_startup:
+    - "franka_gripper_controller"
     - "joint_state_broadcaster"
     - "joint_trajectory_controller"
   # Load but do not start these controllers so they can be activated later if needed.

src/moveit_pro_franka_configs/franka_arm_sim/config/control/franka_ros2_control.yaml → src/moveit_pro_franka_configs/kitchen_sim/config/control/franka_ros2_control.yaml

@@ -9,7 +9,8 @@ controller_manager:
       type: joint_trajectory_controller/JointTrajectoryController
     velocity_force_controller:
       type: velocity_force_controller/VelocityForceController
-
+    franka_gripper_controller:
+      type: position_controllers/GripperActionController
 joint_trajectory_controller:
   ros__parameters:
     command_interfaces:
@@ -34,6 +35,14 @@ joint_trajectory_controller:
       fr3_joint6: { p: 150., d: 10., i: 0., i_clamp: 1. }
       fr3_joint7: { p: 50., d: 5., i: 0., i_clamp: 1. }
 
+franka_gripper_controller:
+  ros__parameters:
+    default: true
+    joint: fr3_finger_joint1
+    allow_stalling: true
+    stall_timeout: 0.1
+    goal_tolerance: 0.005
+
 servo_controller:
   ros__parameters:
     command_interfaces:

src/moveit_pro_franka_configs/franka_arm_sim/description/franka.urdf.xacro → src/moveit_pro_franka_configs/kitchen_sim/description/franka.urdf.xacro

@@ -22,9 +22,49 @@
                       gazebo_effort="false"
                       no_prefix="false"
                       arm_prefix=""
-                      connected_to= "world">
+                      connected_to= "world"
+                      xyz="0 0 0.9">
   </xacro:franka_robot>
 
+  <!-- Pedestal the robot stands on -->
+  <link name="pedestal">
+    <!-- simple mass/inertia for a solid cylinder -->
+    <inertial>
+      <mass value="30.0"/> <!-- adjust as needed -->
+      <!-- Inertia for solid cylinder: Ixx=Iyy=(1/12)m(3r^2+h^2), Izz=(1/2)m r^2 -->
+      <!-- For r=0.2 m, h=0.9 m, m=30 kg: Ixx=Iyy≈2.325, Izz≈0.6 -->
+      <origin xyz="0 0 0.45"/> <!-- center of mass at half height -->
+      <inertia ixx="2.325" ixy="0.0" ixz="0.0"
+               iyy="2.325" iyz="0.0"
+               izz="0.6"/>
+    </inertial>
+
+    <visual>
+      <!-- place the cylinder so its base sits at z=0 and top at z=0.9 -->
+      <origin xyz="0 0 0.45"/>
+      <geometry>
+        <cylinder radius="0.2" length="0.9"/>
+      </geometry>
+      <material name="pedestal_grey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0.45"/>
+      <geometry>
+        <cylinder radius="0.2" length="0.9"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Fix pedestal to the world at the origin -->
+  <joint name="pedestal_to_world" type="fixed">
+    <parent link="world"/>
+    <child link="pedestal"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
     <!-- This link aligns the axes of the grasp frame to our required convention for motion planning tasks. -->
    <link name="grasp_link" />
    <joint name="grasp_link_joint" type="fixed">
@@ -111,12 +151,24 @@
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
+      <joint name="fr3_finger_joint1">
+        <command_interface name="position">
+          <param name="min">-0.0</param>
+          <param name="max">0.04</param>
+        </command_interface>
+        <state_interface name="position">
+          <param name="initial_value">0.035</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
       <hardware>
         <plugin>picknik_mujoco_ros/MujocoSystem</plugin>
         <param name="mujoco_model">$(arg mujoco_model)</param>
-        <param name="mujoco_model_package">franka_arm_sim</param>
+        <param name="mujoco_model_package">kitchen_sim</param>
         <param name="render_publish_rate">10</param>
         <param name="tf_publish_rate">60</param>
+        <param name="mujoco_viewer">$(arg mujoco_viewer)</param>
       </hardware>
     </ros2_control>
 </robot>