From 723e54a5e1030ffe43e0408b2539bb3bac023f26 Mon Sep 17 00:00:00 2001
From: Furkan Davulcu <furkan.davulcu@picknik.ai>
Date: Fri, 26 Jun 2026 09:20:07 +0200
Subject: [PATCH 1/2] fix(dual_arm_sim): reset the correct vector in Create
 Point Cloud Vector From Masks

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
---
 .../objectives/create_point_cloud_vector_from_masks.xml         | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/moveit_pro_franka_configs/dual_arm_sim/objectives/create_point_cloud_vector_from_masks.xml b/src/moveit_pro_franka_configs/dual_arm_sim/objectives/create_point_cloud_vector_from_masks.xml
index 6c52cef8f..f260fe258 100644
--- a/src/moveit_pro_franka_configs/dual_arm_sim/objectives/create_point_cloud_vector_from_masks.xml
+++ b/src/moveit_pro_franka_configs/dual_arm_sim/objectives/create_point_cloud_vector_from_masks.xml
@@ -12,7 +12,7 @@
     point_cloud="/wrist_camera/camera_info"
   >
     <Control ID="Sequence">
-      <Action ID="ResetPoseStampedVector" vector="{pose_stamped_vector}" />
+      <Action ID="ResetVector" vector="{point_cloud_vector}" />
       <Decorator
         ID="ForEach"
         vector_in="{masks3d}"

From 2fe3f11abf20673744d55a3958a7660079e7aafc Mon Sep 17 00:00:00 2001
From: Furkan Davulcu <furkan.davulcu@picknik.ai>
Date: Fri, 26 Jun 2026 09:20:07 +0200
Subject: [PATCH 2/2] fix(dual_arm_sim): wrap Sort Blocks loop in ForceSuccess
 for graceful completion

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
---
 .../dual_arm_sim/objectives/sort_blocks.xml   | 554 +++++++++---------
 1 file changed, 280 insertions(+), 274 deletions(-)

diff --git a/src/moveit_pro_franka_configs/dual_arm_sim/objectives/sort_blocks.xml b/src/moveit_pro_franka_configs/dual_arm_sim/objectives/sort_blocks.xml
index 11673a1a7..070fe2181 100644
--- a/src/moveit_pro_franka_configs/dual_arm_sim/objectives/sort_blocks.xml
+++ b/src/moveit_pro_franka_configs/dual_arm_sim/objectives/sort_blocks.xml
@@ -1,296 +1,302 @@
 <?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Sort Blocks">
-  <BehaviorTree ID="Sort Blocks" _description="" _favorite="true">
+  <BehaviorTree
+    ID="Sort Blocks"
+    _description="Clears colored blocks off the table by color, right arm for red and left for green. A CLIPSeg no-masks error is expected when a color runs out of blocks."
+    _favorite="true"
+  >
     <Control ID="Sequence">
       <Action
         ID="LogMessage"
         log_level="warn"
         message="Work in progress. The robot may miss blocks. Enable 'info' logs to see cycle time"
       />
-      <Decorator ID="KeepRunningUntilFailure">
-        <Control ID="Fallback">
-          <Control ID="Sequence">
-            <Action ID="BiasedCoinFlip" success_probability="0.500000" />
-            <Action ID="StopwatchBegin" timepoint="{timepoint_right}" />
-            <SubTree
-              ID="Find Red Block"
-              _collapsed="false"
-              object_centroid="{object_centroid}"
-            />
-            <Action
-              ID="TransformPose"
-              input_pose="{object_centroid}"
-              output_pose="{object_centroid}"
-              translation_xyz="0;0;-0.1"
-            />
+      <Decorator ID="ForceSuccess">
+        <Decorator ID="KeepRunningUntilFailure">
+          <Control ID="Fallback">
             <Control ID="Sequence">
-              <SubTree ID="Open Right Gripper" _collapsed="true" />
-              <Action
-                ID="InitializeMTCTask"
-                task="{mtc_task}"
-                timeout="-1.000000"
-                trajectory_monitoring="false"
-                controller_names="joint_trajectory_controller"
-                task_id="right_grasp_task"
-              />
-              <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
-              <Action
-                ID="SetupMTCPlanToPose"
-                acceleration_scale_factor="1.000000"
-                ik_frame="right_fr3_hand_tcp"
-                keep_orientation="false"
-                keep_orientation_link_names="right_fr3_hand_tcp"
-                keep_orientation_tolerance="0.100000"
-                link_padding="0.000000"
-                max_iterations="5000"
-                monitored_stage="current state"
-                planning_group_name="right_manipulator"
-                target_pose="{object_centroid}"
-                task="{mtc_task}"
-                trajectory_sampling_rate="100"
-                velocity_scale_factor="1.000000"
-              />
-              <Action
-                ID="SetupMTCMoveAlongFrameAxis"
-                acceleration_scale="1.000000"
-                axis_frame="world"
-                axis_x="0.000000"
-                axis_y="0.000000"
-                axis_z="-1.000000"
-                hand_frame="right_fr3_hand_tcp"
-                ignore_environment_collisions="false"
-                max_distance="0.100000"
-                min_distance="0.100000"
-                planning_group_name="right_manipulator"
-                task="{mtc_task}"
-                velocity_scale="1.000000"
-              />
-              <Action
-                ID="PlanMTCTask"
-                solution="{mtc_solution}"
-                task="{mtc_task}"
-              />
-              <Action
-                ID="ExecuteMTCTask"
-                goal_duration_tolerance="-1.000000"
-                solution="{mtc_solution}"
-              />
-              <Action ID="WaitForDuration" delay_duration="1.0" />
-              <SubTree ID="Close Right Gripper" _collapsed="true" />
-              <Action
-                ID="InitializeMTCTask"
-                task="{mtc_task}"
-                timeout="-1.000000"
-                trajectory_monitoring="false"
-                controller_names="joint_trajectory_controller"
-                task_id="right_place_task"
-              />
-              <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
-              <Action
-                ID="RetrieveWaypoint"
-                joint_group_name="right_manipulator"
-                waypoint_joint_state="{target_joint_state}"
-                waypoint_name="Place Right"
-              />
-              <Action
-                ID="SetupMTCMoveAlongFrameAxis"
-                acceleration_scale="1.000000"
-                axis_frame="world"
-                axis_x="0.000000"
-                axis_y="0.000000"
-                axis_z="1.000000"
-                hand_frame="right_fr3_hand_tcp"
-                ignore_environment_collisions="false"
-                max_distance="0.1"
-                min_distance="0.100000"
-                planning_group_name="right_manipulator"
-                task="{mtc_task}"
-                velocity_scale="1.000000"
-              />
-              <Action
-                ID="SetupMTCPlanToJointState"
-                acceleration_scale_factor="1.000000"
-                joint_state="{target_joint_state}"
-                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="PlanMTCTask"
-                solution="{mtc_solution}"
-                task="{mtc_task}"
+              <Action ID="BiasedCoinFlip" success_probability="0.500000" />
+              <Action ID="StopwatchBegin" timepoint="{timepoint_right}" />
+              <SubTree
+                ID="Find Red Block"
+                _collapsed="false"
+                object_centroid="{object_centroid}"
               />
               <Action
-                ID="ExecuteMTCTask"
-                goal_duration_tolerance="-1.000000"
-                solution="{mtc_solution}"
-              />
-              <SubTree ID="Open Right Gripper" _collapsed="true" />
-              <SubTree
-                ID="Move to Waypoint"
-                _collapsed="true"
-                acceleration_scale_factor="1.0"
-                controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-                controller_names="joint_trajectory_controller"
-                joint_group_name="manipulator"
-                keep_orientation="false"
-                keep_orientation_tolerance="0.05"
-                link_padding="0.01"
-                seed="0"
-                velocity_scale_factor="1.0"
-                waypoint_name="Home"
+                ID="TransformPose"
+                input_pose="{object_centroid}"
+                output_pose="{object_centroid}"
+                translation_xyz="0;0;-0.1"
               />
+              <Control ID="Sequence">
+                <SubTree ID="Open Right Gripper" _collapsed="true" />
+                <Action
+                  ID="InitializeMTCTask"
+                  task="{mtc_task}"
+                  timeout="-1.000000"
+                  trajectory_monitoring="false"
+                  controller_names="joint_trajectory_controller"
+                  task_id="right_grasp_task"
+                />
+                <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
+                <Action
+                  ID="SetupMTCPlanToPose"
+                  acceleration_scale_factor="1.000000"
+                  ik_frame="right_fr3_hand_tcp"
+                  keep_orientation="false"
+                  keep_orientation_link_names="right_fr3_hand_tcp"
+                  keep_orientation_tolerance="0.100000"
+                  link_padding="0.000000"
+                  max_iterations="5000"
+                  monitored_stage="current state"
+                  planning_group_name="right_manipulator"
+                  target_pose="{object_centroid}"
+                  task="{mtc_task}"
+                  trajectory_sampling_rate="100"
+                  velocity_scale_factor="1.000000"
+                />
+                <Action
+                  ID="SetupMTCMoveAlongFrameAxis"
+                  acceleration_scale="1.000000"
+                  axis_frame="world"
+                  axis_x="0.000000"
+                  axis_y="0.000000"
+                  axis_z="-1.000000"
+                  hand_frame="right_fr3_hand_tcp"
+                  ignore_environment_collisions="false"
+                  max_distance="0.100000"
+                  min_distance="0.100000"
+                  planning_group_name="right_manipulator"
+                  task="{mtc_task}"
+                  velocity_scale="1.000000"
+                />
+                <Action
+                  ID="PlanMTCTask"
+                  solution="{mtc_solution}"
+                  task="{mtc_task}"
+                />
+                <Action
+                  ID="ExecuteMTCTask"
+                  goal_duration_tolerance="-1.000000"
+                  solution="{mtc_solution}"
+                />
+                <Action ID="WaitForDuration" delay_duration="1.0" />
+                <SubTree ID="Close Right Gripper" _collapsed="true" />
+                <Action
+                  ID="InitializeMTCTask"
+                  task="{mtc_task}"
+                  timeout="-1.000000"
+                  trajectory_monitoring="false"
+                  controller_names="joint_trajectory_controller"
+                  task_id="right_place_task"
+                />
+                <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
+                <Action
+                  ID="RetrieveWaypoint"
+                  joint_group_name="right_manipulator"
+                  waypoint_joint_state="{target_joint_state}"
+                  waypoint_name="Place Right"
+                />
+                <Action
+                  ID="SetupMTCMoveAlongFrameAxis"
+                  acceleration_scale="1.000000"
+                  axis_frame="world"
+                  axis_x="0.000000"
+                  axis_y="0.000000"
+                  axis_z="1.000000"
+                  hand_frame="right_fr3_hand_tcp"
+                  ignore_environment_collisions="false"
+                  max_distance="0.1"
+                  min_distance="0.100000"
+                  planning_group_name="right_manipulator"
+                  task="{mtc_task}"
+                  velocity_scale="1.000000"
+                />
+                <Action
+                  ID="SetupMTCPlanToJointState"
+                  acceleration_scale_factor="1.000000"
+                  joint_state="{target_joint_state}"
+                  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="PlanMTCTask"
+                  solution="{mtc_solution}"
+                  task="{mtc_task}"
+                />
+                <Action
+                  ID="ExecuteMTCTask"
+                  goal_duration_tolerance="-1.000000"
+                  solution="{mtc_solution}"
+                />
+                <SubTree ID="Open Right Gripper" _collapsed="true" />
+                <SubTree
+                  ID="Move to Waypoint"
+                  _collapsed="true"
+                  acceleration_scale_factor="1.0"
+                  controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+                  controller_names="joint_trajectory_controller"
+                  joint_group_name="manipulator"
+                  keep_orientation="false"
+                  keep_orientation_tolerance="0.05"
+                  link_padding="0.01"
+                  seed="0"
+                  velocity_scale_factor="1.0"
+                  waypoint_name="Home"
+                />
+              </Control>
+              <Action ID="StopwatchEnd" timepoint="{timepoint_right}" />
             </Control>
-            <Action ID="StopwatchEnd" timepoint="{timepoint_right}" />
-          </Control>
-          <Control ID="Sequence">
-            <Action ID="StopwatchBegin" timepoint="{timepoint_left}" />
-            <SubTree
-              ID="Find Green Block"
-              _collapsed="true"
-              object_centroid="{object_centroid}"
-            />
-            <Action
-              ID="TransformPose"
-              input_pose="{object_centroid}"
-              output_pose="{object_centroid}"
-              translation_xyz="0;0;-0.1"
-            />
             <Control ID="Sequence">
-              <SubTree ID="Open Left Gripper" _collapsed="true" />
-              <Action
-                ID="InitializeMTCTask"
-                task="{mtc_task}"
-                timeout="-1.000000"
-                trajectory_monitoring="false"
-                controller_names="joint_trajectory_controller"
-                task_id="left_grasp_task"
-              />
-              <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
-              <Action
-                ID="SetupMTCPlanToPose"
-                acceleration_scale_factor="1.000000"
-                ik_frame="left_fr3_hand_tcp"
-                keep_orientation="false"
-                keep_orientation_link_names="left_fr3_hand_tcp"
-                keep_orientation_tolerance="0.100000"
-                link_padding="0.000000"
-                max_iterations="5000"
-                monitored_stage="current state"
-                planning_group_name="left_manipulator"
-                target_pose="{object_centroid}"
-                task="{mtc_task}"
-                trajectory_sampling_rate="100"
-                velocity_scale_factor="1.000000"
-              />
-              <Action
-                ID="SetupMTCMoveAlongFrameAxis"
-                acceleration_scale="1.000000"
-                axis_frame="world"
-                axis_x="0.000000"
-                axis_y="0.000000"
-                axis_z="-1.000000"
-                hand_frame="left_fr3_hand_tcp"
-                ignore_environment_collisions="false"
-                max_distance="0.1"
-                min_distance="0.100000"
-                planning_group_name="left_manipulator"
-                task="{mtc_task}"
-                velocity_scale="1.000000"
-              />
-              <Action
-                ID="PlanMTCTask"
-                solution="{mtc_solution}"
-                task="{mtc_task}"
-              />
-              <Action
-                ID="ExecuteMTCTask"
-                goal_duration_tolerance="-1.000000"
-                solution="{mtc_solution}"
-              />
-              <Action ID="WaitForDuration" delay_duration="1.0" />
-              <SubTree ID="Close Left Gripper" _collapsed="true" />
-              <Action
-                ID="InitializeMTCTask"
-                task="{mtc_task}"
-                timeout="-1.000000"
-                trajectory_monitoring="false"
-                controller_names="joint_trajectory_controller"
-                task_id="left_place_task"
-              />
-              <Action
-                ID="RetrieveWaypoint"
-                joint_group_name="left_manipulator"
-                waypoint_joint_state="{target_joint_state}"
-                waypoint_name="Place Left"
-              />
-              <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
-              <Action
-                ID="SetupMTCMoveAlongFrameAxis"
-                acceleration_scale="1.000000"
-                axis_frame="world"
-                axis_x="0.000000"
-                axis_y="0.000000"
-                axis_z="1.000000"
-                hand_frame="left_fr3_hand_tcp"
-                ignore_environment_collisions="false"
-                max_distance="0.1"
-                min_distance="0.100000"
-                planning_group_name="left_manipulator"
-                task="{mtc_task}"
-                velocity_scale="1.000000"
-              />
-              <Action
-                ID="SetupMTCPlanToJointState"
-                acceleration_scale_factor="1.000000"
-                joint_state="{target_joint_state}"
-                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="PlanMTCTask"
-                solution="{mtc_solution}"
-                task="{mtc_task}"
-              />
-              <Action
-                ID="ExecuteMTCTask"
-                goal_duration_tolerance="-1.000000"
-                solution="{mtc_solution}"
-              />
-              <SubTree ID="Open Left Gripper" _collapsed="true" />
+              <Action ID="StopwatchBegin" timepoint="{timepoint_left}" />
               <SubTree
-                ID="Move to Waypoint"
+                ID="Find Green Block"
                 _collapsed="true"
-                acceleration_scale_factor="1.0"
-                controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-                controller_names="joint_trajectory_controller"
-                joint_group_name="manipulator"
-                keep_orientation="false"
-                keep_orientation_tolerance="0.05"
-                link_padding="0.01"
-                seed="0"
-                velocity_scale_factor="1.0"
-                waypoint_name="Home"
+                object_centroid="{object_centroid}"
+              />
+              <Action
+                ID="TransformPose"
+                input_pose="{object_centroid}"
+                output_pose="{object_centroid}"
+                translation_xyz="0;0;-0.1"
               />
+              <Control ID="Sequence">
+                <SubTree ID="Open Left Gripper" _collapsed="true" />
+                <Action
+                  ID="InitializeMTCTask"
+                  task="{mtc_task}"
+                  timeout="-1.000000"
+                  trajectory_monitoring="false"
+                  controller_names="joint_trajectory_controller"
+                  task_id="left_grasp_task"
+                />
+                <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
+                <Action
+                  ID="SetupMTCPlanToPose"
+                  acceleration_scale_factor="1.000000"
+                  ik_frame="left_fr3_hand_tcp"
+                  keep_orientation="false"
+                  keep_orientation_link_names="left_fr3_hand_tcp"
+                  keep_orientation_tolerance="0.100000"
+                  link_padding="0.000000"
+                  max_iterations="5000"
+                  monitored_stage="current state"
+                  planning_group_name="left_manipulator"
+                  target_pose="{object_centroid}"
+                  task="{mtc_task}"
+                  trajectory_sampling_rate="100"
+                  velocity_scale_factor="1.000000"
+                />
+                <Action
+                  ID="SetupMTCMoveAlongFrameAxis"
+                  acceleration_scale="1.000000"
+                  axis_frame="world"
+                  axis_x="0.000000"
+                  axis_y="0.000000"
+                  axis_z="-1.000000"
+                  hand_frame="left_fr3_hand_tcp"
+                  ignore_environment_collisions="false"
+                  max_distance="0.1"
+                  min_distance="0.100000"
+                  planning_group_name="left_manipulator"
+                  task="{mtc_task}"
+                  velocity_scale="1.000000"
+                />
+                <Action
+                  ID="PlanMTCTask"
+                  solution="{mtc_solution}"
+                  task="{mtc_task}"
+                />
+                <Action
+                  ID="ExecuteMTCTask"
+                  goal_duration_tolerance="-1.000000"
+                  solution="{mtc_solution}"
+                />
+                <Action ID="WaitForDuration" delay_duration="1.0" />
+                <SubTree ID="Close Left Gripper" _collapsed="true" />
+                <Action
+                  ID="InitializeMTCTask"
+                  task="{mtc_task}"
+                  timeout="-1.000000"
+                  trajectory_monitoring="false"
+                  controller_names="joint_trajectory_controller"
+                  task_id="left_place_task"
+                />
+                <Action
+                  ID="RetrieveWaypoint"
+                  joint_group_name="left_manipulator"
+                  waypoint_joint_state="{target_joint_state}"
+                  waypoint_name="Place Left"
+                />
+                <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
+                <Action
+                  ID="SetupMTCMoveAlongFrameAxis"
+                  acceleration_scale="1.000000"
+                  axis_frame="world"
+                  axis_x="0.000000"
+                  axis_y="0.000000"
+                  axis_z="1.000000"
+                  hand_frame="left_fr3_hand_tcp"
+                  ignore_environment_collisions="false"
+                  max_distance="0.1"
+                  min_distance="0.100000"
+                  planning_group_name="left_manipulator"
+                  task="{mtc_task}"
+                  velocity_scale="1.000000"
+                />
+                <Action
+                  ID="SetupMTCPlanToJointState"
+                  acceleration_scale_factor="1.000000"
+                  joint_state="{target_joint_state}"
+                  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="PlanMTCTask"
+                  solution="{mtc_solution}"
+                  task="{mtc_task}"
+                />
+                <Action
+                  ID="ExecuteMTCTask"
+                  goal_duration_tolerance="-1.000000"
+                  solution="{mtc_solution}"
+                />
+                <SubTree ID="Open Left Gripper" _collapsed="true" />
+                <SubTree
+                  ID="Move to Waypoint"
+                  _collapsed="true"
+                  acceleration_scale_factor="1.0"
+                  controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+                  controller_names="joint_trajectory_controller"
+                  joint_group_name="manipulator"
+                  keep_orientation="false"
+                  keep_orientation_tolerance="0.05"
+                  link_padding="0.01"
+                  seed="0"
+                  velocity_scale_factor="1.0"
+                  waypoint_name="Home"
+                />
+              </Control>
+              <Action ID="StopwatchEnd" timepoint="{timepoint_left}" />
             </Control>
-            <Action ID="StopwatchEnd" timepoint="{timepoint_left}" />
           </Control>
-        </Control>
+        </Decorator>
       </Decorator>
     </Control>
   </BehaviorTree>