Mujoco 6.0 (#343)

* updates to mujoco config Co-authored-by: Sebastian Jahr <[email protected]> Co-authored-by: Mario Prats <[email protected]>
PickNikRobotics · Aug 13, 2024 · 7d9d249 · 7d9d249
1 parent ae6030f
commit 7d9d249
Show file tree

Hide file tree

Showing 51 changed files with 762 additions and 371 deletions.
diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
@@ -15,7 +15,7 @@ on:
 
 jobs:
   integration-test-in-studio-container:
-    uses: PickNikRobotics/moveit_pro_ci/.github/workflows/workspace_integration_test.yaml@main
+    uses: PickNikRobotics/moveit_pro_ci/.github/workflows/workspace_integration_test.yaml@virtual-buffer
     with:
       image_tag: ${{ github.event.inputs.image_tag || null }}
       colcon_test_args: "--executor sequential"

diff --git a/.gitmodules b/.gitmodules
@@ -1,6 +1,7 @@
 [submodule "src/external_dependencies/ros2_robotiq_gripper"]
 	path = src/external_dependencies/ros2_robotiq_gripper
 	url = https://github.com/PickNikRobotics/ros2_robotiq_gripper.git
+	branch = humble
 [submodule "src/external_dependencies/picknik_accessories"]
 	path = src/external_dependencies/picknik_accessories
 	url = https://github.com/PickNikRobotics/picknik_accessories.git

diff --git a/README.md b/README.md
@@ -19,6 +19,7 @@ This workspace contains several MoveIt Pro configuration packages for UR arms th
 graph TB
 Base[picknik_ur_base_config] --> Site[picknik_ur_site_config]
 Base --> Mock[picknik_ur_mock_hw_config]
+Base --> Simulation[picknik_ur_mujoco_config]
 Site --> Gazebo[picknik_ur_gazebo_config]
 Site --> Picknik[Other PickNik configs]
 Gazebo --> ScanAndPlan[picknik_ur_gazebo_scan_and_plan_config]

diff --git a/src/external_dependencies/ros2_robotiq_gripper b/src/external_dependencies/ros2_robotiq_gripper
diff --git a/src/picknik_ur_base_config/config/config.yaml b/src/picknik_ur_base_config/config/config.yaml
@@ -55,7 +55,7 @@ hardware:
       - prefix: ""
       - use_fake_hardware: "true"
       - use_pinch_links: "true"
-      - fake_sensor_commands: "false"
+      - mock_sensor_commands: "false"
       - headless_mode: "true"
       - robot_ip: "0.0.0.0"
       - joint_limits_parameters_file:

diff --git a/src/picknik_ur_base_config/objectives/close_gripper.xml b/src/picknik_ur_base_config/objectives/close_gripper.xml
@@ -1,8 +1,8 @@
-<?xml version="1.0"?>
+<?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Close Gripper">
-    <BehaviorTree ID="Close Gripper" _description="Close the gripper">
-        <Control ID="Sequence" name="root">
-            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-        </Control>
-    </BehaviorTree>
+  <BehaviorTree ID="Close Gripper" _description="Close the gripper">
+    <Control ID="Sequence" name="root">
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929" timeout="15.000000"/>
+    </Control>
+  </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml b/src/picknik_ur_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Get IMarker Pose From Mesh Visualization">
   <!--//////////-->
-  <BehaviorTree ID="Get IMarker Pose From Mesh Visualization" _description="Visualize a mesh in the UI and use an IMarker to get a grasp pose based on that mesh" _favorite="false">
+  <BehaviorTree ID="Get IMarker Pose From Mesh Visualization" _description="Visualize a mesh in the UI and use an IMarker to get a grasp pose based on that mesh" _favorite="false" _subtreeOnly="true">
     <Control ID="Sequence" name="TopLevelSequence">
       <Action ID="VisualizeMesh" marker_lifetime="0.0"/>
       <Action ID="AddPoseStampedToVector" input="{mesh_pose}"/>
@@ -13,7 +13,7 @@
       <output_port name="adjusted_poses" default="{adjusted_poses}"/>
       <input_port name="mesh_path" default="{mesh_path}"/>
       <input_port name="mesh_pose" default="{mesh_pose}"/>
-      <input_port name="_collapsed" default="false"/>
+      <input_port name="_collapsed" default="true"/>
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml b/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
@@ -1,20 +1,17 @@
-<?xml version="1.0"?>
+<?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Interpolate to Joint State">
-    <BehaviorTree ID="Interpolate to Joint State" _description="Move to a specified joint state using joint interpolation">
-        <Control ID="Fallback" name="root">
-            <Control ID="Sequence">
-                <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
-                <Action ID="InitializeMTCTask" task_id="interpolate_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{interpolate_to_waypoint_task}"/>
-                <Action ID="SetupMTCCurrentState" task="{interpolate_to_waypoint_task}"/>
-                <Action ID="SetupMTCInterpolateToJointState" joint_state="{target_joint_state}" name="SetupMTCInterpolateToJointState_First" planning_group_name="manipulator" task="{interpolate_to_waypoint_task}"/>
-                <Action ID="PlanMTCTask" solution="{interpolate_to_waypoint_solution}" task="{interpolate_to_waypoint_task}"/>
-                <Action ID="ExecuteMTCTask" solution="{interpolate_to_waypoint_solution}"/>
-                <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
-            </Control>
-            <Control ID="Sequence">
-                <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
-                <Action ID="AlwaysFailure"/>
-            </Control>
-        </Control>
-    </BehaviorTree>
+  <BehaviorTree ID="Interpolate to Joint State" _description="Move to a specified joint state using joint interpolation" _subtreeOnly="true">
+    <Control ID="Sequence">
+      <Action ID="InitializeMTCTask" task_id="interpolate_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{interpolate_to_waypoint_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{interpolate_to_waypoint_task}"/>
+      <Action ID="SetupMTCInterpolateToJointState" joint_state="{target_joint_state}" name="SetupMTCInterpolateToJointState_First" planning_group_name="manipulator" task="{interpolate_to_waypoint_task}"/>
+      <Action ID="PlanMTCTask" solution="{interpolate_to_waypoint_solution}" task="{interpolate_to_waypoint_task}"/>
+      <Action ID="ExecuteMTCTask" solution="{interpolate_to_waypoint_solution}"/>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Interpolate to Joint State">
+      <input_port name="target_joint_state" default="{target_joint_state}"/>
+    </SubTree>
+  </TreeNodesModel>
 </root>
diff --git a/src/picknik_ur_base_config/objectives/move_to_joint_state.xml b/src/picknik_ur_base_config/objectives/move_to_joint_state.xml
@@ -1,20 +1,17 @@
-<?xml version="1.0"?>
+<?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Joint State">
-    <BehaviorTree ID="Move to Joint State" _description="Move to a specified joint state">
-        <Control ID="Fallback" name="root">
-            <Control ID="Sequence">
-                <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
-                <Action ID="InitializeMTCTask" task_id="move_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{move_to_waypoint_task}"/>
-                <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
-                <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="manipulator" task="{move_to_waypoint_task}" planner_interface="moveit_default" />
-                <Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
-                <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
-                <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
-            </Control>
-            <Control ID="Sequence">
-                <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
-                <Action ID="AlwaysFailure"/>
-            </Control>
-        </Control>
-    </BehaviorTree>
+  <BehaviorTree ID="Move to Joint State" _description="Move to a specified joint state" _subtreeOnly="true">
+    <Control ID="Sequence">
+      <Action ID="InitializeMTCTask" task_id="move_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{move_to_waypoint_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
+      <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="manipulator" task="{move_to_waypoint_task}" planner_interface="moveit_default"/>
+      <Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
+      <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Move to Joint State">
+      <input_port name="target_joint_state" default="{target_joint_state}"/>
+    </SubTree>
+  </TreeNodesModel>
 </root>
diff --git a/src/picknik_ur_base_config/objectives/move_to_pose.xml b/src/picknik_ur_base_config/objectives/move_to_pose.xml
@@ -1,21 +1,18 @@
-<?xml version="1.0"?>
+<?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Pose">
-    <BehaviorTree ID="Move to Pose" _description="Uses inverse kinematics to move the robot to a set gripper position">
-        <Control ID="Fallback" name="root">
-            <Control ID="Sequence">
-                <Action ID="RetrievePoseParameter" timeout_sec="-1" pose="{target_pose}"/>
-                <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="{controller_names}" task="{move_to_pose_task}"/>
-                <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
-                <Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}" planner_interface="moveit_default"/>
-                <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
-                <SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
-                <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
-                <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
-            </Control>
-            <Control ID="Sequence">
-                <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
-                <Action ID="AlwaysFailure"/>
-            </Control>
-        </Control>
-    </BehaviorTree>
+  <BehaviorTree ID="Move to Pose" _description="Uses inverse kinematics to move the robot to a set gripper position" _subtreeOnly="true">
+    <Control ID="Sequence">
+      <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{move_to_pose_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
+      <Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}" planner_interface="moveit_default"/>
+      <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
+      <SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}"/>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Move to Pose">
+      <input_port name="target_pose" default="{target_pose}"/>
+    </SubTree>
+  </TreeNodesModel>
 </root>
diff --git a/src/picknik_ur_base_config/objectives/move_to_waypoint.xml b/src/picknik_ur_base_config/objectives/move_to_waypoint.xml
@@ -1,6 +1,6 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Waypoint">
-  <BehaviorTree ID="Move to Waypoint" _description="This Objective is used when moving to one of the saved waypoints in your site configuration">
+  <BehaviorTree ID="Move to Waypoint" _description="This Objective is used when moving to one of the saved waypoints in your site configuration" _subtreeOnly="true">
     <Control ID="Sequence" name="root">
       <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="{waypoint_name}" joint_group_name="{joint_group_name}"/>
       <Action ID="InitializeMTCTask" task_id="move_to_named_pose" controller_names="{controller_names}" task="{move_to_waypoint_task}"/>

diff --git a/src/picknik_ur_base_config/objectives/open_gripper.xml b/src/picknik_ur_base_config/objectives/open_gripper.xml
@@ -1,9 +1,9 @@
-<?xml version="1.0"?>
+<?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Open Gripper">
-    <!-- ////////// -->
-    <BehaviorTree ID="Open Gripper" _description="Open the gripper">
-        <Control ID="Sequence" name="root">
-            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
-        </Control>
-    </BehaviorTree>
+  <!--//////////-->
+  <BehaviorTree ID="Open Gripper" _description="Open the gripper">
+    <Control ID="Sequence" name="root">
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.05" timeout="15.000000"/>
+    </Control>
+  </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_base_config/objectives/request_teleoperation.xml b/src/picknik_ur_base_config/objectives/request_teleoperation.xml
@@ -1,42 +1,72 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Request Teleoperation">
-  <!-- The integer value used here for teleoperation mode comes from the moveit_studio_sdk_msgs/TeleoperationMode ROS message. -->
-  <BehaviorTree ID="Request Teleoperation" _description="Handles the different variations of teleoperation from the web UI, with the option of interactive user prompts and choosing the initial mode. Should be used as a subtree with port remapping as part of an another Objective." _favorite="false" _hardcoded="false">
+  <!--The integer value used here for teleoperation mode comes from the moveit_studio_sdk_msgs/TeleoperationMode ROS message.-->
+  <BehaviorTree ID="Request Teleoperation" _description="Handles the different variations of teleoperation from the web UI, with the option of interactive user prompts and choosing the initial mode. Should be used as a subtree with port remapping as part of an another Objective." _favorite="false" _hardcoded="false" _subtreeOnly="true">
     <Control ID="Sequence">
       <Action ID="Script" code="teleop_mode := 0"/>
       <Control ID="Parallel" success_count="1" failure_count="1">
         <Action ID="DoTeleoperateAction" enable_user_interaction="{enable_user_interaction}" user_interaction_prompt="{user_interaction_prompt}" initial_teleop_mode="{initial_teleop_mode}" current_teleop_mode="{teleop_mode}"/>
         <Decorator ID="KeepRunningUntilFailure">
           <Control ID="Sequence">
-            <!-- Closing and opening the gripper -->
+            <!--Closing and opening the gripper-->
             <Decorator ID="ForceSuccess" _skipIf="teleop_mode != 7">
               <SubTree ID="Close Gripper"/>
             </Decorator>
             <Decorator ID="ForceSuccess" _skipIf="teleop_mode != 6">
               <SubTree ID="Open Gripper"/>
             </Decorator>
-            <!-- Joint sliders interpolate to joint state -->
+            <!--Joint sliders interpolate to joint state-->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 5">
               <Control ID="Sequence">
                 <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
-                <SubTree ID="Interpolate to Joint State"/>
+                <Control ID="Fallback" name="root">
+                  <Control ID="Sequence">
+                    <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
+                    <SubTree ID="Interpolate to Joint State" _collapsed="false" target_joint_state="{target_joint_state}"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                  </Control>
+                  <Control ID="Sequence">
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
+                  </Control>
+                </Control>
               </Control>
             </Decorator>
-            <!-- Interactive markers move to pose -->
+            <!--Interactive markers move to pose-->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 4">
               <Control ID="Sequence">
                 <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
-                <SubTree ID="Move to Pose" controller_names="/joint_trajectory_controller"/>
+                <Control ID="Fallback" name="root">
+                  <Control ID="Sequence">
+                    <Action ID="RetrievePoseParameter" timeout_sec="-1" pose="{target_pose}"/>
+                    <SubTree ID="Move to Pose" _collapsed="false" target_pose="{target_pose}"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                  </Control>
+                  <Control ID="Sequence">
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
+                  </Control>
+                </Control>
               </Control>
             </Decorator>
-            <!-- Waypoint buttons move to joint state -->
+            <!--Waypoint buttons move to joint state-->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 3">
               <Control ID="Sequence">
                 <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
-                <SubTree ID="Move to Joint State"/>
+                <Control ID="Fallback" name="root">
+                  <Control ID="Sequence">
+                    <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
+                    <SubTree ID="Move to Joint State" _collapsed="false" target_joint_state="{target_joint_state}"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                  </Control>
+                  <Control ID="Sequence">
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
+                  </Control>
+                </Control>
               </Control>
             </Decorator>
-            <!-- Cartesian and joint jogging -->
+            <!--Cartesian and joint jogging-->
             <Control ID="Sequence" _while="teleop_mode == 2">
               <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
               <Action ID="TeleoperateTwist" controller_name="servo_controller"/>

diff --git a/src/picknik_ur_base_config/objectives/teleoperate.xml b/src/picknik_ur_base_config/objectives/teleoperate.xml
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Teleoperate">
   <!-- ////////// -->
-  <BehaviorTree ID="Teleoperate" _description="Handles the different variations of teleoperation from the web UI. Can be used standalone." _favorite="true" _hardcoded="false">
+  <BehaviorTree ID="Teleoperate" _description="Handles the different variations of teleoperation from the web UI. Can be used standalone." _favorite="false" _hardcoded="false">
     <SubTree ID="Request Teleoperation" enable_user_interaction="false" user_interaction_prompt="" initial_teleop_mode="3"/>
   </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_base_config/objectives/wait_for_trajectory_approval_if_user_available.xml b/src/picknik_ur_base_config/objectives/wait_for_trajectory_approval_if_user_available.xml
@@ -1,6 +1,6 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Wait for Trajectory Approval if User Available">
-  <BehaviorTree ID="Wait for Trajectory Approval if User Available" _description="Preview a shared pointer to an MTC Solution object via an input data port if a user is available.">
+  <BehaviorTree ID="Wait for Trajectory Approval if User Available" _description="Preview a shared pointer to an MTC Solution object via an input data port if a user is available." _subtreeOnly="true">
     <Control ID="Fallback" name="wait_for_approval_if_user_available">
       <Decorator ID="Inverter">
         <Action ID="IsUserAvailable"/>

diff --git a/src/picknik_ur_mujoco_config/config/control/picknik_ur.ros2_control.yaml b/src/picknik_ur_mujoco_config/config/control/picknik_ur.ros2_control.yaml
@@ -16,6 +16,7 @@ controller_manager:
     velocity_force_controller:
       type: velocity_force_controller/VelocityForceController
 
+
 joint_state_broadcaster:
   ros__parameters:
     use_local_topics: false
+2 −2		robotiq_description/urdf/2f_140.ros2_control.xacro
+2 −2		robotiq_description/urdf/2f_85.ros2_control.xacro
+2 −2		robotiq_description/urdf/robotiq_2f_140_macro.urdf.xacro
+2 −2		robotiq_description/urdf/robotiq_2f_85_macro.urdf.xacro