From 19b0e80d3bb6a30106f71a55a908da3dbc86e4f8 Mon Sep 17 00:00:00 2001
From: Abishalini <abi.gpuram@gmail.com>
Date: Thu, 4 Jan 2024 12:59:05 -0700
Subject: [PATCH 1/6] Update SetupMTCMoveAlongFrameAxis parameters

---
 .../objectives/push_along_axis_config.yaml                   | 5 -----
 .../objectives/push_button_config.yaml                       | 5 -----
 src/picknik_ur_gazebo_config/objectives/push_button_ml.xml   | 3 +--
 src/picknik_ur_site_config/objectives/close_cabinet_door.xml | 3 +--
 .../objectives/push_along_axis_config.yaml                   | 5 -----
 src/picknik_ur_site_config/objectives/push_button.xml        | 2 +-
 src/picknik_ur_site_config/objectives/push_button_ml.xml     | 3 +--
 7 files changed, 4 insertions(+), 22 deletions(-)

diff --git a/src/picknik_ur_gazebo_config/objectives/push_along_axis_config.yaml b/src/picknik_ur_gazebo_config/objectives/push_along_axis_config.yaml
index 716c2dc6..36876760 100644
--- a/src/picknik_ur_gazebo_config/objectives/push_along_axis_config.yaml
+++ b/src/picknik_ur_gazebo_config/objectives/push_along_axis_config.yaml
@@ -1,8 +1,3 @@
-SetupMTCMoveAlongFrameAxis:
-  arm_group_name: "manipulator"
-  hand_frame_name: "manual_grasp_link"
-  velocity_acceleration_scaling_factor: 0.05
-
 AllowGripperCollisionWithOctomap:
   group_name: "gripper"
   object_name: "<octomap>"
diff --git a/src/picknik_ur_gazebo_config/objectives/push_button_config.yaml b/src/picknik_ur_gazebo_config/objectives/push_button_config.yaml
index 716c2dc6..36876760 100644
--- a/src/picknik_ur_gazebo_config/objectives/push_button_config.yaml
+++ b/src/picknik_ur_gazebo_config/objectives/push_button_config.yaml
@@ -1,8 +1,3 @@
-SetupMTCMoveAlongFrameAxis:
-  arm_group_name: "manipulator"
-  hand_frame_name: "manual_grasp_link"
-  velocity_acceleration_scaling_factor: 0.05
-
 AllowGripperCollisionWithOctomap:
   group_name: "gripper"
   object_name: "<octomap>"
diff --git a/src/picknik_ur_gazebo_config/objectives/push_button_ml.xml b/src/picknik_ur_gazebo_config/objectives/push_button_ml.xml
index 27351583..bb2a48df 100644
--- a/src/picknik_ur_gazebo_config/objectives/push_button_ml.xml
+++ b/src/picknik_ur_gazebo_config/objectives/push_button_ml.xml
@@ -43,8 +43,7 @@
                 <Action ID="SetupMTCUpdateGroupCollisionRule"
                     name="AllowGripperCollisionWithOctomap" parameters="{parameters}"
                     task="{push_along_axis_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" axis="z" max_distance="0.2"
-                    min_distance="0.05" parameters="{parameters}" task="{push_along_axis_task}" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule"
                     name="ForbidGripperCollisionWithOctomap" parameters="{parameters}"
                     task="{push_along_axis_task}" />
diff --git a/src/picknik_ur_site_config/objectives/close_cabinet_door.xml b/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
index a492c10f..d40b36f6 100644
--- a/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
+++ b/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
@@ -12,8 +12,7 @@
                 <Action ID="GetPoseFromUser" parameter_name="close_cabinet_door.target_pose" parameter_value="{target_pose}" />
                 <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{close_cabinet_door_task}" use_all_planners="false"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" axis="z" max_distance="1.5" min_distance="0.05" parameters="{parameters}" task="{close_cabinet_door_task}" />
-                <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{close_cabinet_door_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />                <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
                 <Action ID="PlanMTCTask" solution="{full_close_cabinet_door_solution}" task="{close_cabinet_door_task}"/>
                 <Action ID="SplitMTCSolution" solution_in="{full_close_cabinet_door_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
                 <Action ID="WaitForUserTrajectoryApproval" solution="{full_close_cabinet_door_solution}"/>
diff --git a/src/picknik_ur_site_config/objectives/push_along_axis_config.yaml b/src/picknik_ur_site_config/objectives/push_along_axis_config.yaml
index 716c2dc6..36876760 100644
--- a/src/picknik_ur_site_config/objectives/push_along_axis_config.yaml
+++ b/src/picknik_ur_site_config/objectives/push_along_axis_config.yaml
@@ -1,8 +1,3 @@
-SetupMTCMoveAlongFrameAxis:
-  arm_group_name: "manipulator"
-  hand_frame_name: "manual_grasp_link"
-  velocity_acceleration_scaling_factor: 0.05
-
 AllowGripperCollisionWithOctomap:
   group_name: "gripper"
   object_name: "<octomap>"
diff --git a/src/picknik_ur_site_config/objectives/push_button.xml b/src/picknik_ur_site_config/objectives/push_button.xml
index c3ce7f90..ce0b0913 100644
--- a/src/picknik_ur_site_config/objectives/push_button.xml
+++ b/src/picknik_ur_site_config/objectives/push_button.xml
@@ -12,7 +12,7 @@
                 <Action ID="GetPoseFromUser" parameter_name="push_button.target_pose" parameter_value="{target_pose}" />
                 <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{push_button_task}" use_all_planners="false"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" axis="z" max_distance="0.2" min_distance="0.05" parameters="{parameters}" task="{push_button_task}" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_button_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />
                 <Action ID="PlanMTCTask" solution="{full_push_button_solution}" task="{push_button_task}"/>
                 <Action ID="SplitMTCSolution" solution_in="{full_push_button_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
diff --git a/src/picknik_ur_site_config/objectives/push_button_ml.xml b/src/picknik_ur_site_config/objectives/push_button_ml.xml
index 9d3acf35..bae0ea78 100644
--- a/src/picknik_ur_site_config/objectives/push_button_ml.xml
+++ b/src/picknik_ur_site_config/objectives/push_button_ml.xml
@@ -43,8 +43,7 @@
                 <Action ID="SetupMTCUpdateGroupCollisionRule"
                     name="AllowGripperCollisionWithOctomap" parameters="{parameters}"
                     task="{push_along_axis_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" axis="z" max_distance="0.2"
-                    min_distance="0.05" parameters="{parameters}" task="{push_along_axis_task}" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule"
                     name="ForbidGripperCollisionWithOctomap" parameters="{parameters}"
                     task="{push_along_axis_task}" />

From 1ac6e4fe89857b3978fa49a4baab82df2b8fe6cc Mon Sep 17 00:00:00 2001
From: Abishalini <abi.gpuram@gmail.com>
Date: Thu, 4 Jan 2024 13:03:23 -0700
Subject: [PATCH 2/6] Fix close cabinet

---
 src/picknik_ur_site_config/objectives/close_cabinet_door.xml | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/picknik_ur_site_config/objectives/close_cabinet_door.xml b/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
index d40b36f6..c63c4272 100644
--- a/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
+++ b/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
@@ -12,7 +12,8 @@
                 <Action ID="GetPoseFromUser" parameter_name="close_cabinet_door.target_pose" parameter_value="{target_pose}" />
                 <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{close_cabinet_door_task}" use_all_planners="false"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" task="{close_cabinet_door_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />                <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{close_cabinet_door_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />                
+                <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
                 <Action ID="PlanMTCTask" solution="{full_close_cabinet_door_solution}" task="{close_cabinet_door_task}"/>
                 <Action ID="SplitMTCSolution" solution_in="{full_close_cabinet_door_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
                 <Action ID="WaitForUserTrajectoryApproval" solution="{full_close_cabinet_door_solution}"/>

From ac8b6e27257518c33b8cea387a6e75e98ff37f19 Mon Sep 17 00:00:00 2001
From: Abishalini <abi.gpuram@gmail.com>
Date: Thu, 4 Jan 2024 12:59:05 -0700
Subject: [PATCH 3/6] Update SetupMTCMoveAlongFrameAxis parameters

---
 .../objectives/push_along_axis_config.yaml                   | 5 -----
 .../objectives/push_button_config.yaml                       | 5 -----
 src/picknik_ur_gazebo_config/objectives/push_button_ml.xml   | 3 +--
 src/picknik_ur_site_config/objectives/close_cabinet_door.xml | 3 +--
 .../objectives/push_along_axis_config.yaml                   | 5 -----
 src/picknik_ur_site_config/objectives/push_button.xml        | 2 +-
 src/picknik_ur_site_config/objectives/push_button_ml.xml     | 3 +--
 7 files changed, 4 insertions(+), 22 deletions(-)

diff --git a/src/picknik_ur_gazebo_config/objectives/push_along_axis_config.yaml b/src/picknik_ur_gazebo_config/objectives/push_along_axis_config.yaml
index 716c2dc6..36876760 100644
--- a/src/picknik_ur_gazebo_config/objectives/push_along_axis_config.yaml
+++ b/src/picknik_ur_gazebo_config/objectives/push_along_axis_config.yaml
@@ -1,8 +1,3 @@
-SetupMTCMoveAlongFrameAxis:
-  arm_group_name: "manipulator"
-  hand_frame_name: "manual_grasp_link"
-  velocity_acceleration_scaling_factor: 0.05
-
 AllowGripperCollisionWithOctomap:
   group_name: "gripper"
   object_name: "<octomap>"
diff --git a/src/picknik_ur_gazebo_config/objectives/push_button_config.yaml b/src/picknik_ur_gazebo_config/objectives/push_button_config.yaml
index 716c2dc6..36876760 100644
--- a/src/picknik_ur_gazebo_config/objectives/push_button_config.yaml
+++ b/src/picknik_ur_gazebo_config/objectives/push_button_config.yaml
@@ -1,8 +1,3 @@
-SetupMTCMoveAlongFrameAxis:
-  arm_group_name: "manipulator"
-  hand_frame_name: "manual_grasp_link"
-  velocity_acceleration_scaling_factor: 0.05
-
 AllowGripperCollisionWithOctomap:
   group_name: "gripper"
   object_name: "<octomap>"
diff --git a/src/picknik_ur_gazebo_config/objectives/push_button_ml.xml b/src/picknik_ur_gazebo_config/objectives/push_button_ml.xml
index 27351583..bb2a48df 100644
--- a/src/picknik_ur_gazebo_config/objectives/push_button_ml.xml
+++ b/src/picknik_ur_gazebo_config/objectives/push_button_ml.xml
@@ -43,8 +43,7 @@
                 <Action ID="SetupMTCUpdateGroupCollisionRule"
                     name="AllowGripperCollisionWithOctomap" parameters="{parameters}"
                     task="{push_along_axis_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" axis="z" max_distance="0.2"
-                    min_distance="0.05" parameters="{parameters}" task="{push_along_axis_task}" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule"
                     name="ForbidGripperCollisionWithOctomap" parameters="{parameters}"
                     task="{push_along_axis_task}" />
diff --git a/src/picknik_ur_site_config/objectives/close_cabinet_door.xml b/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
index a492c10f..d40b36f6 100644
--- a/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
+++ b/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
@@ -12,8 +12,7 @@
                 <Action ID="GetPoseFromUser" parameter_name="close_cabinet_door.target_pose" parameter_value="{target_pose}" />
                 <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{close_cabinet_door_task}" use_all_planners="false"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" axis="z" max_distance="1.5" min_distance="0.05" parameters="{parameters}" task="{close_cabinet_door_task}" />
-                <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{close_cabinet_door_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />                <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
                 <Action ID="PlanMTCTask" solution="{full_close_cabinet_door_solution}" task="{close_cabinet_door_task}"/>
                 <Action ID="SplitMTCSolution" solution_in="{full_close_cabinet_door_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
                 <Action ID="WaitForUserTrajectoryApproval" solution="{full_close_cabinet_door_solution}"/>
diff --git a/src/picknik_ur_site_config/objectives/push_along_axis_config.yaml b/src/picknik_ur_site_config/objectives/push_along_axis_config.yaml
index 716c2dc6..36876760 100644
--- a/src/picknik_ur_site_config/objectives/push_along_axis_config.yaml
+++ b/src/picknik_ur_site_config/objectives/push_along_axis_config.yaml
@@ -1,8 +1,3 @@
-SetupMTCMoveAlongFrameAxis:
-  arm_group_name: "manipulator"
-  hand_frame_name: "manual_grasp_link"
-  velocity_acceleration_scaling_factor: 0.05
-
 AllowGripperCollisionWithOctomap:
   group_name: "gripper"
   object_name: "<octomap>"
diff --git a/src/picknik_ur_site_config/objectives/push_button.xml b/src/picknik_ur_site_config/objectives/push_button.xml
index c3ce7f90..ce0b0913 100644
--- a/src/picknik_ur_site_config/objectives/push_button.xml
+++ b/src/picknik_ur_site_config/objectives/push_button.xml
@@ -12,7 +12,7 @@
                 <Action ID="GetPoseFromUser" parameter_name="push_button.target_pose" parameter_value="{target_pose}" />
                 <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{push_button_task}" use_all_planners="false"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" axis="z" max_distance="0.2" min_distance="0.05" parameters="{parameters}" task="{push_button_task}" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_button_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />
                 <Action ID="PlanMTCTask" solution="{full_push_button_solution}" task="{push_button_task}"/>
                 <Action ID="SplitMTCSolution" solution_in="{full_push_button_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
diff --git a/src/picknik_ur_site_config/objectives/push_button_ml.xml b/src/picknik_ur_site_config/objectives/push_button_ml.xml
index 9d3acf35..bae0ea78 100644
--- a/src/picknik_ur_site_config/objectives/push_button_ml.xml
+++ b/src/picknik_ur_site_config/objectives/push_button_ml.xml
@@ -43,8 +43,7 @@
                 <Action ID="SetupMTCUpdateGroupCollisionRule"
                     name="AllowGripperCollisionWithOctomap" parameters="{parameters}"
                     task="{push_along_axis_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" axis="z" max_distance="0.2"
-                    min_distance="0.05" parameters="{parameters}" task="{push_along_axis_task}" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule"
                     name="ForbidGripperCollisionWithOctomap" parameters="{parameters}"
                     task="{push_along_axis_task}" />

From f293612edd87b8e7477437ba4a74cedb357566a1 Mon Sep 17 00:00:00 2001
From: Abishalini <abi.gpuram@gmail.com>
Date: Thu, 4 Jan 2024 13:03:23 -0700
Subject: [PATCH 4/6] Fix close cabinet

---
 src/picknik_ur_site_config/objectives/close_cabinet_door.xml | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/picknik_ur_site_config/objectives/close_cabinet_door.xml b/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
index d40b36f6..c63c4272 100644
--- a/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
+++ b/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
@@ -12,7 +12,8 @@
                 <Action ID="GetPoseFromUser" parameter_name="close_cabinet_door.target_pose" parameter_value="{target_pose}" />
                 <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{close_cabinet_door_task}" use_all_planners="false"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" task="{close_cabinet_door_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />                <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{close_cabinet_door_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />                
+                <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
                 <Action ID="PlanMTCTask" solution="{full_close_cabinet_door_solution}" task="{close_cabinet_door_task}"/>
                 <Action ID="SplitMTCSolution" solution_in="{full_close_cabinet_door_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
                 <Action ID="WaitForUserTrajectoryApproval" solution="{full_close_cabinet_door_solution}"/>

From 6db9fc51237d4aac44096cc127d36f636819c62f Mon Sep 17 00:00:00 2001
From: Sebastian Castro <sebas.a.castro@gmail.com>
Date: Fri, 5 Jan 2024 18:32:05 -0500
Subject: [PATCH 5/6] Add missed changes

---
 src/picknik_ur_gazebo_config/objectives/close_cabinet_door.xml | 2 +-
 src/picknik_ur_gazebo_config/objectives/push_button.xml        | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/picknik_ur_gazebo_config/objectives/close_cabinet_door.xml b/src/picknik_ur_gazebo_config/objectives/close_cabinet_door.xml
index 3c74bc34..926b5b44 100644
--- a/src/picknik_ur_gazebo_config/objectives/close_cabinet_door.xml
+++ b/src/picknik_ur_gazebo_config/objectives/close_cabinet_door.xml
@@ -12,7 +12,7 @@
                 <Action ID="GetPoseFromUser" parameter_name="close_cabinet_door.target_pose" parameter_value="{target_pose}" />
                 <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{close_cabinet_door_task}" use_all_planners="false"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" axis="z" max_distance="1.5" min_distance="0.05" parameters="{parameters}" task="{close_cabinet_door_task}" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
                 <Action ID="PlanMTCTask" solution="{full_close_cabinet_door_solution}" task="{close_cabinet_door_task}"/>
                 <Action ID="SplitMTCSolution" solution_in="{full_close_cabinet_door_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
diff --git a/src/picknik_ur_gazebo_config/objectives/push_button.xml b/src/picknik_ur_gazebo_config/objectives/push_button.xml
index c55c3fb5..b57bfab3 100644
--- a/src/picknik_ur_gazebo_config/objectives/push_button.xml
+++ b/src/picknik_ur_gazebo_config/objectives/push_button.xml
@@ -12,7 +12,7 @@
                 <Action ID="GetPoseFromUser" parameter_name="push_button.target_pose" parameter_value="{target_pose}" />
                 <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{push_button_task}" use_all_planners="false"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" axis="z" max_distance="0.2" min_distance="0.05" parameters="{parameters}" task="{push_button_task}" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_button_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />
                 <Action ID="PlanMTCTask" solution="{full_push_button_solution}" task="{push_button_task}"/>
                 <Action ID="SplitMTCSolution" solution_in="{full_push_button_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />

From 0bc674f974d2c8127c61b9cb7afffeef49b9d7b7 Mon Sep 17 00:00:00 2001
From: Abishalini <abi.gpuram@gmail.com>
Date: Fri, 5 Jan 2024 17:03:20 -0700
Subject: [PATCH 6/6] Change axis frame

---
 src/picknik_ur_gazebo_config/objectives/close_cabinet_door.xml | 2 +-
 src/picknik_ur_gazebo_config/objectives/push_button.xml        | 2 +-
 src/picknik_ur_gazebo_config/objectives/push_button_ml.xml     | 2 +-
 src/picknik_ur_site_config/objectives/close_cabinet_door.xml   | 2 +-
 src/picknik_ur_site_config/objectives/push_button.xml          | 2 +-
 src/picknik_ur_site_config/objectives/push_button_ml.xml       | 2 +-
 6 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/picknik_ur_gazebo_config/objectives/close_cabinet_door.xml b/src/picknik_ur_gazebo_config/objectives/close_cabinet_door.xml
index 926b5b44..78346cc6 100644
--- a/src/picknik_ur_gazebo_config/objectives/close_cabinet_door.xml
+++ b/src/picknik_ur_gazebo_config/objectives/close_cabinet_door.xml
@@ -12,7 +12,7 @@
                 <Action ID="GetPoseFromUser" parameter_name="close_cabinet_door.target_pose" parameter_value="{target_pose}" />
                 <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{close_cabinet_door_task}" use_all_planners="false"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="manual_grasp_link" axis_frame="manual_grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
                 <Action ID="PlanMTCTask" solution="{full_close_cabinet_door_solution}" task="{close_cabinet_door_task}"/>
                 <Action ID="SplitMTCSolution" solution_in="{full_close_cabinet_door_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
diff --git a/src/picknik_ur_gazebo_config/objectives/push_button.xml b/src/picknik_ur_gazebo_config/objectives/push_button.xml
index b57bfab3..e0c554d8 100644
--- a/src/picknik_ur_gazebo_config/objectives/push_button.xml
+++ b/src/picknik_ur_gazebo_config/objectives/push_button.xml
@@ -12,7 +12,7 @@
                 <Action ID="GetPoseFromUser" parameter_name="push_button.target_pose" parameter_value="{target_pose}" />
                 <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{push_button_task}" use_all_planners="false"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_button_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_button_task}" hand_frame="manual_grasp_link" axis_frame="manual_grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />
                 <Action ID="PlanMTCTask" solution="{full_push_button_solution}" task="{push_button_task}"/>
                 <Action ID="SplitMTCSolution" solution_in="{full_push_button_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
diff --git a/src/picknik_ur_gazebo_config/objectives/push_button_ml.xml b/src/picknik_ur_gazebo_config/objectives/push_button_ml.xml
index bb2a48df..ae56060f 100644
--- a/src/picknik_ur_gazebo_config/objectives/push_button_ml.xml
+++ b/src/picknik_ur_gazebo_config/objectives/push_button_ml.xml
@@ -43,7 +43,7 @@
                 <Action ID="SetupMTCUpdateGroupCollisionRule"
                     name="AllowGripperCollisionWithOctomap" parameters="{parameters}"
                     task="{push_along_axis_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="manual_grasp_link" axis_frame="manual_grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule"
                     name="ForbidGripperCollisionWithOctomap" parameters="{parameters}"
                     task="{push_along_axis_task}" />
diff --git a/src/picknik_ur_site_config/objectives/close_cabinet_door.xml b/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
index c63c4272..85f45607 100644
--- a/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
+++ b/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
@@ -12,7 +12,7 @@
                 <Action ID="GetPoseFromUser" parameter_name="close_cabinet_door.target_pose" parameter_value="{target_pose}" />
                 <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{close_cabinet_door_task}" use_all_planners="false"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" task="{close_cabinet_door_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />                
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{close_cabinet_door_task}" hand_frame="manual_grasp_link" axis_frame="manual_grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />                
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
                 <Action ID="PlanMTCTask" solution="{full_close_cabinet_door_solution}" task="{close_cabinet_door_task}"/>
                 <Action ID="SplitMTCSolution" solution_in="{full_close_cabinet_door_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
diff --git a/src/picknik_ur_site_config/objectives/push_button.xml b/src/picknik_ur_site_config/objectives/push_button.xml
index ce0b0913..99832677 100644
--- a/src/picknik_ur_site_config/objectives/push_button.xml
+++ b/src/picknik_ur_site_config/objectives/push_button.xml
@@ -12,7 +12,7 @@
                 <Action ID="GetPoseFromUser" parameter_name="push_button.target_pose" parameter_value="{target_pose}" />
                 <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{push_button_task}" use_all_planners="false"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_button_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_button_task}" hand_frame="manual_grasp_link" axis_frame="manual_grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />
                 <Action ID="PlanMTCTask" solution="{full_push_button_solution}" task="{push_button_task}"/>
                 <Action ID="SplitMTCSolution" solution_in="{full_push_button_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
diff --git a/src/picknik_ur_site_config/objectives/push_button_ml.xml b/src/picknik_ur_site_config/objectives/push_button_ml.xml
index bae0ea78..082621f4 100644
--- a/src/picknik_ur_site_config/objectives/push_button_ml.xml
+++ b/src/picknik_ur_site_config/objectives/push_button_ml.xml
@@ -43,7 +43,7 @@
                 <Action ID="SetupMTCUpdateGroupCollisionRule"
                     name="AllowGripperCollisionWithOctomap" parameters="{parameters}"
                     task="{push_along_axis_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="manual_grasp_link" axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="manual_grasp_link" axis_frame="manual_grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule"
                     name="ForbidGripperCollisionWithOctomap" parameters="{parameters}"
                     task="{push_along_axis_task}" />