Merge branch 'main' into bilal_henry_hackathon_2025

Author: henrygerardmoore

src/factory_sim/objectives/plan_and_execute_with_orientation_samples.xml

@@ -70,14 +70,22 @@
           controller_names="{controller_names}"
           task="{mtc_task}"
           trajectory_monitoring="false"
+          timeout="-1.000000"
         />
         <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
         <Action
-          ID="SetupMTCConnectWithTrajectory"
-          constraints="{constraints}"
-          planner_interface="pro_rrt"
-          planning_group_name="{joint_group_name}"
+          ID="SetupMTCConnectWithProRRT"
+          acceleration_scale_factor="1.000000"
+          keep_orientation="false"
+          keep_orientation_link_names="grasp_link"
+          keep_orientation_tolerance="0.100000"
+          link_padding="0.000000"
+          max_iterations="5000"
+          planning_group_name="manipulator"
+          seed="0"
           task="{mtc_task}"
+          trajectory_sampling_rate="100"
+          velocity_scale_factor="1.000000"
         />
         <Action
           ID="SetupMTCBatchPoseIK"
@@ -152,6 +160,7 @@
                     keep_orientation_tolerance="0.05"
                     link_padding="0.01"
                     velocity_scale_factor="1.0"
+                    seed="0"
                   />
                 </Control>
               </Decorator>

src/factory_sim/objectives/plan_and_execute_with_world_vertical_orientation.xml

@@ -6,13 +6,23 @@
     _description="Free space plan to an approach pose then free space plan to a target pose with optional orientation constraints. With orientation constraints the gripper will maintain its orientation relative to the world vertical axis when moving from the approach to target pose."
   >
     <Control ID="Sequence" name="Plan to approach then target">
-      <Action ID="SwitchController" activate_controllers="{controller_names}" />
+      <Action
+        ID="SwitchController"
+        activate_controllers="{controller_names}"
+        activate_asap="true"
+        automatic_deactivation="true"
+        controller_list_action_name="/controller_manager/list_controllers"
+        controller_switch_action_name="/controller_manager/switch_controller"
+        strictness="1"
+        timeout="-1.000000"
+      />
       <Action
         ID="InitializeMTCTask"
         controller_names="{controller_names}"
         task="{mtc_task}"
         task_id="move_to_pose"
         trajectory_monitoring="false"
+        timeout="-1.000000"
       />
       <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
       <Action
@@ -94,6 +104,7 @@
                   keep_orientation_tolerance="0.05"
                   link_padding="0.01"
                   velocity_scale_factor="1.0"
+                  seed="0"
                 />
               </Control>
             </Decorator>

src/factory_sim/objectives/plan_path_along_surface.xml

@@ -121,11 +121,18 @@
         />
         <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
         <Action
-          ID="SetupMTCConnectWithTrajectory"
-          planner_interface="pro_rrt"
+          ID="SetupMTCConnectWithProRRT"
+          acceleration_scale_factor="1.000000"
+          keep_orientation="false"
+          keep_orientation_link_names="grasp_link"
+          keep_orientation_tolerance="0.100000"
+          link_padding="0.000000"
+          max_iterations="5000"
           planning_group_name="manipulator"
+          seed="0"
           task="{mtc_task}"
-          constraints="{constraints}"
+          trajectory_sampling_rate="100"
+          velocity_scale_factor="1.000000"
         />
         <Action
           ID="SetupMTCBatchPoseIK"

src/factory_sim/objectives/reachability_analysis_-_blocks.xml

@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Reachability Analysis - blocks">
   <!--//////////-->
   <BehaviorTree

src/grinding_sim/description/scene.xml

@@ -398,7 +398,7 @@
 
   <keyframe>
     <key
-      name="retract"
+      name="default"
       qpos="1.57055 -0.999904 -1.80019 -2.45625 -1.57086 -3.14161 0.0256934"
       ctrl="1.57055 -0.999904 -1.80019 -2.45625 -1.57086 -3.14161 115"
     />

src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml

@@ -215,6 +215,7 @@
                       task="{task}"
                       task_id=""
                       trajectory_monitoring="false"
+                      timeout="-1.000000"
                     />
                     <Action ID="SetupMTCCurrentState" task="{task}" />
                     <Action
@@ -225,11 +226,18 @@
                       task="{task}"
                     />
                     <Action
-                      ID="SetupMTCConnectWithTrajectory"
-                      constraints="{constraints}"
-                      planner_interface="pro_rrt"
+                      ID="SetupMTCConnectWithProRRT"
+                      acceleration_scale_factor="1.000000"
+                      keep_orientation="false"
+                      keep_orientation_link_names="grasp_link"
+                      keep_orientation_tolerance="0.100000"
+                      link_padding="0.000000"
+                      max_iterations="5000"
                       planning_group_name="manipulator"
+                      seed="0"
                       task="{task}"
+                      trajectory_sampling_rate="100"
+                      velocity_scale_factor="1.000000"
                     />
                     <Action
                       ID="SetupMTCMoveAlongFrameAxis"

src/lab_sim/config/config.yaml

@@ -47,9 +47,13 @@ objectives:
       - "moveit_studio::behaviors::ServoBehaviorsLoader"
       - "moveit_studio::behaviors::VisionBehaviorsLoader"
       - "moveit_studio::behaviors::ConverterBehaviorsLoader"
+      - "moveit_studio::behaviors::MujocoBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]
   objective_library_paths:
+    mujoco_objectives:
+      package_name: "moveit_pro_objectives"
+      relative_path: "objectives/mujoco"
     sim_objectives:
       package_name: "lab_sim"
       relative_path: "objectives"

src/lab_sim/description/scene.xml

@@ -73,6 +73,7 @@
 
   <keyframe>
     <key
+      name="default"
       qpos="-1.168 0.776711 0.469754 1 5.72696e-06 -4.26533e-08 -9.38821e-05 -1.34591 1.07777 0.902167 1 -2.70778e-08 7.60222e-07 -8.18971e-05 -1.1503 1.07777 0.902163 -0.873311 -1.32611e-06 -2.31428e-06 -0.487163 -0.871046 0.729603 0.473006 -0.839083 -3.76738e-06 -5.23752e-06 -0.544003 -0.699998 0.85 0.472753 -0.759699 6.25044e-07 1.49157e-06 -0.650275 -0.5 0.69 0.472779 -0.75966 -8.69884e-06 3.09906e-06 -0.65032 -0.360001 0.609999 0.472778 -0.759663 -6.29279e-06 3.61581e-06 -0.650317 -0.299999 0.7 0.472779 -0.75966 -5.34077e-06 1.33814e-06 -0.65032 -3.70361e-09 0.9 0.47375 -0.839072 -1.82055e-05 -5.31435e-05 0.544021 -0.1 0.85 0.47375 0.283662 4.83891e-05 2.85336e-05 -0.958924 0.0952783 0.840677 0.47375 -0.852936 4.00776e-05 -3.93632e-05 -0.522015 0.2 0.75 0.498669 1 3.49711e-17 2.9976e-18 1.51884e-30 1.73912e-05 0.749992 0.498669 1 0.00016562 0.000360588 4.6626e-11 -0.2 0.75 0.498669 1 2.94315e-17 -7.92697e-18 -4.14434e-30 1.56909e-06 -3.71262e-07 -1.88491 1.10086 -1.25647 -1.57075 -3.63345e-07 0.0525545 0.000140183 0.0525603 -0.050625 0.0525545 0.000140184 0.0525567 -0.0506181"
       qvel="0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0"
       ctrl="0 -1.8849 1.0997 -1.2566 -1.57075 0 0 0"

src/lab_sim/objectives/3_waypoint_pick_and_place.xml

@@ -13,11 +13,19 @@
         waypoint_name="Look at Table"
         joint_group_name="manipulator"
         controller_names="joint_trajectory_controller"
+        acceleration_scale_factor="1.0"
+        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+        keep_orientation="false"
+        keep_orientation_tolerance="0.05"
+        link_padding="0.01"
+        seed="0"
+        velocity_scale_factor="1.0"
       />
       <Action
         ID="MoveGripperAction"
         gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
         position="0"
+        timeout="10.000000"
       />
       <!--Run pick and place on loop-->
       <Decorator ID="KeepRunningUntilFailure">
@@ -29,34 +37,64 @@
               waypoint_name="Pick Cube"
               joint_group_name="manipulator"
               controller_names="joint_trajectory_controller"
+              acceleration_scale_factor="1.0"
+              controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+              keep_orientation="false"
+              keep_orientation_tolerance="0.05"
+              link_padding="0.01"
+              seed="0"
+              velocity_scale_factor="1.0"
             />
             <Action
               ID="MoveGripperAction"
               gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
               position="0.7929"
+              timeout="10.000000"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Look at Table"
               joint_group_name="manipulator"
               controller_names="joint_trajectory_controller"
+              acceleration_scale_factor="1.0"
+              controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+              keep_orientation="false"
+              keep_orientation_tolerance="0.05"
+              link_padding="0.01"
+              seed="0"
+              velocity_scale_factor="1.0"
             />
             <SubTree
               ID="Move to Waypoint"
               joint_group_name="manipulator"
               controller_names="joint_trajectory_controller"
               waypoint_name="Place Cube"
+              acceleration_scale_factor="1.0"
+              controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+              keep_orientation="false"
+              keep_orientation_tolerance="0.05"
+              link_padding="0.01"
+              seed="0"
+              velocity_scale_factor="1.0"
             />
             <Action
               ID="MoveGripperAction"
               gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
               position="0"
+              timeout="10.000000"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Look at Table"
               joint_group_name="manipulator"
               controller_names="joint_trajectory_controller"
+              acceleration_scale_factor="1.0"
+              controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+              keep_orientation="false"
+              keep_orientation_tolerance="0.05"
+              link_padding="0.01"
+              seed="0"
+              velocity_scale_factor="1.0"
             />
           </Control>
           <!--Pick object from where it was placed, put it down on the original location, and go back to center pose-->
@@ -66,34 +104,64 @@
               waypoint_name="Place Cube"
               joint_group_name="manipulator"
               controller_names="joint_trajectory_controller"
+              acceleration_scale_factor="1.0"
+              controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+              keep_orientation="false"
+              keep_orientation_tolerance="0.05"
+              link_padding="0.01"
+              seed="0"
+              velocity_scale_factor="1.0"
             />
             <Action
               ID="MoveGripperAction"
               gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
               position="0.7929"
+              timeout="10.000000"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Look at Table"
               joint_group_name="manipulator"
               controller_names="joint_trajectory_controller"
+              acceleration_scale_factor="1.0"
+              controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+              keep_orientation="false"
+              keep_orientation_tolerance="0.05"
+              link_padding="0.01"
+              seed="0"
+              velocity_scale_factor="1.0"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Pick Cube"
               joint_group_name="manipulator"
               controller_names="joint_trajectory_controller"
+              acceleration_scale_factor="1.0"
+              controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+              keep_orientation="false"
+              keep_orientation_tolerance="0.05"
+              link_padding="0.01"
+              seed="0"
+              velocity_scale_factor="1.0"
             />
             <Action
               ID="MoveGripperAction"
               gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
               position="0"
+              timeout="10.000000"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Look at Table"
               joint_group_name="manipulator"
               controller_names="joint_trajectory_controller"
+              acceleration_scale_factor="1.0"
+              controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+              keep_orientation="false"
+              keep_orientation_tolerance="0.05"
+              link_padding="0.01"
+              seed="0"
+              velocity_scale_factor="1.0"
             />
           </Control>
         </Control>