Modify objectives to use Get String/Pose/JointState From User behavior (

#25) Co-authored-by: Sebastian Castro <[email protected]>
PickNikRobotics · Jun 7, 2023 · 06e5cac · 06e5cac
1 parent 3dfcdf3
commit 06e5cac
Show file tree

Hide file tree

Showing 13 changed files with 45 additions and 32 deletions.
diff --git a/src/picknik_ur_base_config/objectives/close_cabinet_door.xml b/src/picknik_ur_base_config/objectives/close_cabinet_door.xml
@@ -8,7 +8,8 @@
             <Control ID="Sequence">
                 <Action ID="InitializeMTCTask" task_id="close_cabinet_door" controller_names="/joint_trajectory_controller" task="{close_cabinet_door_task}"/>
                 <Action ID="SetupMTCCurrentState" task="{close_cabinet_door_task}"/>
-                <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{close_cabinet_door.target_pose}" task="{close_cabinet_door_task}"/>
+                <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}"/>
                 <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}" />

diff --git a/src/picknik_ur_base_config/objectives/inspect_surface.xml b/src/picknik_ur_base_config/objectives/inspect_surface.xml
@@ -4,7 +4,8 @@
         <Control ID="Sequence">
             <Action ID="InitializeMTCTask" task_id="inspect_surface" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
             <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
-            <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{inspect_surface.target_pose}" task="{move_to_pose_task}"/>
+            <Action ID="GetPoseFromUser" parameter_name="inspect_surface.target_pose" parameter_value="{target_pose}" />
+            <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}"/>
             <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
             <Fallback name="wait_for_approval_if_user_available">
                 <Inverter>

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
@@ -2,7 +2,14 @@
 <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">
         <Control ID="Sequence" name="root">
-            <SubTree ID="InterpolateToJointStateWaypoint" target_joint_state="{interpolate_to_joint_state.target_joint_state}"/>
+            <Control ID="Fallback" name="log_error_if_parameter_not_set">
+                <Action ID="GetJointStateFromUser" parameter_name="interpolate_to_joint_state.target_joint_state" parameter_value="{target_joint_state}" />
+                <Control ID="Sequence">
+                    <Action ID="LogMessage" message="This Objective is meant to be used by joint sliders in the Joints tab. Try it out there!" log_level="error" />
+                    <Action ID="AlwaysFailure" />
+                </Control>
+            </Control>
+            <SubTree ID="InterpolateToJointStateWaypoint" target_joint_state="{target_joint_state}"/>
         </Control>
     </BehaviorTree>
     <BehaviorTree ID="InterpolateToJointStateWaypoint">

diff --git a/src/picknik_ur_base_config/objectives/interpolate_to_named_state.xml b/src/picknik_ur_base_config/objectives/interpolate_to_named_state.xml
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,6 +1,9 @@
 <?xml version="1.0"?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Joint State">
     <BehaviorTree ID="Move to Joint State" _description="Single-node behavior to move to a specified joint state">
-        <Action ID="MoveToJointState" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="{move_to_joint_state.waypoint_name}" />
+        <Control ID="Sequence">
+            <Action ID="GetStringFromUser" parameter_name="move_to_joint_state.waypoint_name" parameter_value="{waypoint_name}" />
+            <Action ID="MoveToJointState" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="{waypoint_name}" />
+        </Control>
     </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_base_config/objectives/move_to_known_pose.xml b/src/picknik_ur_base_config/objectives/move_to_known_pose.xml
@@ -2,7 +2,15 @@
 <root BTCPP_format="4" main_tree_to_execute="Move to Known Pose">
     <BehaviorTree ID="Move to Known Pose" _description="Move to a known pose">
         <Control ID="Sequence" name="root">
-            <SubTree ID="MoveToWaypoint" waypoint_name="{move_to_known_pose.target_state_name}"/>
+            <!-- When the user clicks on different pose buttons on Endpoint tab, the frontend sets move_to_known_pose.target_state_name parameter value-->
+            <Control ID="Fallback" name="log_error_if_parameter_not_set">
+                <Action ID="GetStringFromUser" parameter_name="move_to_known_pose.target_state_name" parameter_value="{target_state_name}"/>
+                <Control ID="Sequence">
+                    <Action ID="LogMessage" message="This Objective is meant to be used by Move To Pose buttons in the Endpoint tab. Try it out there!" log_level="error" />
+                    <Action ID="AlwaysFailure" />
+                </Control>
+            </Control>
+            <SubTree ID="MoveToWaypoint" waypoint_name="{target_state_name}"/>
         </Control>
     </BehaviorTree>
     <BehaviorTree ID="MoveToWaypoint">

diff --git a/src/picknik_ur_base_config/objectives/move_to_pose.xml b/src/picknik_ur_base_config/objectives/move_to_pose.xml
@@ -4,7 +4,8 @@
         <Control ID="Sequence">
             <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
             <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
-            <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{move_to_pose.target_pose}" task="{move_to_pose_task}"/>
+            <Action ID="GetPoseFromUser" parameter_name="move_to_pose.target_pose" parameter_value="{target_pose}" />
+            <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}"/>
             <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
             <Fallback name="wait_for_approval_if_user_available">
                 <Inverter>

diff --git a/src/picknik_ur_base_config/objectives/open_lever_handle_door.xml b/src/picknik_ur_base_config/objectives/open_lever_handle_door.xml
@@ -8,8 +8,10 @@
             <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" />
             <Action ID="PublishPointCloud" point_cloud="{point_cloud}"
                 point_cloud_topic="studio_vision_debug_snapshot" />
-            <Action ID="GetDoorHandle" handle_pivot_pose="{get_door_handle_pose.handle_pivot_pose}"
-                handle_tip_pose="{get_door_handle_pose.handle_tip_pose}" parameters="{parameters}"
+            <Action ID="GetPoseFromUser" parameter_name="get_door_handle_pose.handle_pivot_pose" parameter_value="{handle_pivot_pose}" />
+            <Action ID="GetPoseFromUser" parameter_name="get_door_handle_pose.handle_tip_pose" parameter_value="{handle_tip_pose}" />
+            <Action ID="GetDoorHandle" handle_pivot_pose="{handle_pivot_pose}"
+                handle_tip_pose="{handle_tip_pose}" parameters="{parameters}"
                 point_cloud="{point_cloud}" target_handle_length="{handle_length}"
                 target_handle_pose="{handle_pose}" target_handle_z_offset="{handle_z_offset}" />
             <SubTree ID="CloseGripper" />

diff --git a/src/picknik_ur_base_config/objectives/pick_object.xml b/src/picknik_ur_base_config/objectives/pick_object.xml
@@ -3,7 +3,8 @@
     <BehaviorTree ID="Pick Object" _description="Pick up and lift a small object">
         <Control ID="Sequence" name="root">
             <SubTree ID="OpenGripper"/>
-            <SubTree ID="PickObject" grasp_pose="{pick_object.grasp_pose}"/>
+            <Action ID="GetPoseFromUser" parameter_name="pick_object.grasp_pose" parameter_value="{grasp_pose}" />
+            <SubTree ID="PickObject" grasp_pose="{grasp_pose}"/>
         </Control>
     </BehaviorTree>
     <BehaviorTree ID="PickObject">

diff --git a/src/picknik_ur_base_config/objectives/pick_place_object.xml b/src/picknik_ur_base_config/objectives/pick_place_object.xml
@@ -24,7 +24,8 @@
                             <!-- Detect all cuboids in that point cloud-->
                             <Action ID="FindSingularCuboids" point_cloud="{point_cloud}" parameters="{parameters}" detected_shapes="{cuboids}"/>
                             <!-- Find the closest cuboid to the pose calculated from the user's original click -->
-                            <Action ID="GetClosestObjectToPose" objects="{cuboids}" pose="{pick_place_object.grasp_pose}" distance_threshold="0.3" closest_object="{cuboid_object}"/>
+                            <Action ID="GetPoseFromUser" parameter_name="pick_place_object.grasp_pose" parameter_value="{grasp_pose}" />
+                            <Action ID="GetClosestObjectToPose" objects="{cuboids}" pose="{grasp_pose}" distance_threshold="0.3" closest_object="{cuboid_object}"/>
                             <!-- Add this cuboid to the planning scene. -->
                             <Action ID="ModifyObjectInPlanningScene" object="{cuboid_object}" apply_planning_scene_service="/apply_planning_scene"/>
                              <!-- Use point cloud to update the collision octomap (happens after ModifyObjectInPlanningScene so the cuboid is excluded from the octomap)-->
@@ -67,7 +68,8 @@
                                         <Control ID="Sequence">
                                             <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" />
                                             <Action ID="FindSingularCuboids" parameters="{parameters}" point_cloud="{point_cloud}" detected_shapes="{cuboids}"/>
-                                            <Action ID="GetClosestObjectToPose" objects="{cuboids}" pose="{pick_place_object.grasp_pose}" distance_threshold="0.3" closest_object="{new_cuboid_object}"/>
+                                            <Action ID="GetPoseFromUser" parameter_name="pick_place_object.grasp_pose" parameter_value="{grasp_pose}" />
+                                            <Action ID="GetClosestObjectToPose" objects="{cuboids}" pose="{grasp_pose}" distance_threshold="0.3" closest_object="{new_cuboid_object}"/>
                                             <Action ID="CheckCuboidSimilarity" input_cuboid="{new_cuboid_object}" reference_cuboid="{cuboid_object}" base_frame="world" distance_threshold="0.04" orientation_threshold="3.14" />
                                         </Control>
 

diff --git a/src/picknik_ur_base_config/objectives/push_button.xml b/src/picknik_ur_base_config/objectives/push_button.xml
@@ -8,7 +8,8 @@
             <Control ID="Sequence">
                 <Action ID="InitializeMTCTask" task_id="push_button" controller_names="/joint_trajectory_controller" task="{push_button_task}"/>
                 <Action ID="SetupMTCCurrentState" task="{push_button_task}"/>
-                <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{push_button.target_pose}" task="{push_button_task}"/>
+                <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}"/>
                 <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="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />

diff --git a/src/picknik_ur_base_config/objectives/take_snapshot.xml b/src/picknik_ur_base_config/objectives/take_snapshot.xml
@@ -5,7 +5,8 @@
         <Control ID="Sequence">
             <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" />
             <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
-            <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures" point_cloud_uuid="{take_snapshot.uuid}"/>
+            <Action ID="GetStringFromUser" parameter_name="take_snapshot.uuid" parameter_value="{uuid}" />  
+            <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures" point_cloud_uuid="{uuid}"/>
         </Control>
     </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_gazebo_config/objectives/open_door_affordance.xml b/src/picknik_ur_gazebo_config/objectives/open_door_affordance.xml
@@ -3,7 +3,10 @@
     <BehaviorTree ID="Open Cabinet Door" _description="Open a cabinet door by grasping and pulling its handle">
         <Control ID="Sequence" name="Setup">
             <Action ID="LoadObjectiveParameters" config_file_name="open_door_affordance_config.yaml" parameters="{parameters}" />
-            <Action ID="GetHingeAxisFromSurfaceSelection" grasp_pose="{grasp_pose}" hinge_axis_pose_end="{process_door_selection.hinge_axis_pose_end}" hinge_axis_pose_start="{process_door_selection.hinge_axis_pose_start}" screw_axis_pose="{screw_axis_pose}" screw_origin_pose="{screw_origin_pose}" target_grasp_pose="{process_door_selection.grasp_pose}" />
+            <Action ID="GetPoseFromUser" parameter_name="process_door_selection.hinge_axis_pose_end" parameter_value="{hinge_axis_pose_end}" />
+            <Action ID="GetPoseFromUser" parameter_name="process_door_selection.hinge_axis_pose_start" parameter_value="{hinge_axis_pose_start}" />
+            <Action ID="GetPoseFromUser" parameter_name="process_door_selection.grasp_pose" parameter_value="{grasp_pose}" />
+            <Action ID="GetHingeAxisFromSurfaceSelection" grasp_pose="{grasp_pose}" hinge_axis_pose_end="{hinge_axis_pose_end}" hinge_axis_pose_start="{hinge_axis_pose_start}" screw_axis_pose="{screw_axis_pose}" screw_origin_pose="{screw_origin_pose}" target_grasp_pose="{grasp_pose}" />
             <Control ID="Sequence" name="OpenDoorAffordanceMain">
                 <Action ID="InitializeMTCTask" task_id="open_door_affordance" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{open_door_affordance_task}" />
                 <Action ID="SetupMTCCurrentState" task="{open_door_affordance_task}" />