Reorganize for generic config package inheritance

src/picknik_ur_gazebo_config/config/site_config.yaml → src/picknik_ur_gazebo_config/config/config.yaml

@@ -3,7 +3,7 @@
 #
 
 # Name of the package to specialize
-based_on_package: "picknik_ur_base_config"
+based_on_package: "picknik_ur_site_config"
 
 # Optional parameters that can be read in your launch files for specific functionality
 optional_feature_params:

src/picknik_ur_site_config/config/site_config.yaml → src/picknik_ur_site_config/config/config.yaml

@@ -6,6 +6,11 @@ hardware:
   ip: "0.0.0.0"
 
 objectives:
+  behavior_loader_plugins:
+    visual_servo_behaviors:
+      - "moveit_visual_servo::behaviors::VisualServoBehaviorLoader"
+    gibson:
+      - "gibson_behavior::GibsonBehaviorsLoader"
   objective_library_paths:
     # You must use a unique key for each package.
     # The picknik_ur_base_config uses "core"

src/picknik_ur_site_config/objectives/apriltag_servoing.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+  <root BTCPP_format="4" main_tree_to_execute="Apriltag Servoing">
+    <!-- ////////// -->
+    <BehaviorTree ID="Apriltag Servoing" _description="Uses fiducial markers and MoveIt Servo to jog towards a grapple fixture and grasp the handle">
+        <Control ID="Sequence">
+            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
+            <Action ID="LoadObjectiveParameters" config_file_name="apriltag_servoing_config.yaml" parameters="{parameters}"/>
+            <Control ID="Parallel" success_count="1" failure_count="1">
+                <Action ID="GetApriltagPose" parameters="{parameters}" camera_stream_topic="/wrist_mounted_camera/color/image_raw" detection_transform_stamped_topic="/apriltag_detections"/>
+                <Control ID="Sequence">
+                    <Action ID="GraspPoseStreamer" detections_streaming_topic="/apriltag_detections" goal_pose_streaming_topic="/visual_servo_goal_pose" pose_error_topic="/visual_servo_pose_error" goal_frame_id="base_link" target_frame_id="frame_id0" goal_linear_threshold="0.05" goal_angular_threshold="0.1" goal_pose_translation_x="0" goal_pose_translation_y="0.1" goal_pose_translation_z="0.10" goal_pose_orientation_x="1.0" goal_pose_orientation_y="0" goal_pose_orientation_z="0" goal_pose_orientation_w="0"/>
+                    <Action ID="GraspPoseStreamer" detections_streaming_topic="/apriltag_detections" goal_pose_streaming_topic="/visual_servo_goal_pose" pose_error_topic="/visual_servo_pose_error" goal_frame_id="base_link" target_frame_id="frame_id0" goal_linear_threshold="0.02" goal_angular_threshold="0.1" goal_pose_translation_x="0.0" goal_pose_translation_y="0.1" goal_pose_translation_z="0.02" goal_pose_orientation_x="1.0" goal_pose_orientation_y="0" goal_pose_orientation_z="0" goal_pose_orientation_w="0"/>
+                </Control>
+                <Action ID="PoseStreamFilter" input_pose_topic="/visual_servo_goal_pose" output_pose_topic="/visual_servo_goal_pose_filtered" filter_coefficient="0.1"/>
+                <Action ID="VisualServo" parameters="{parameters}" goal_pose_topic="/visual_servo_goal_pose_filtered" controller_name="streaming_controller" pose_error_topic="/visual_servo_pose_error"/>
+            </Control>
+            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
+        </Control>
+    </BehaviorTree>
+</root>

src/picknik_ur_site_config/objectives/apriltag_servoing_config.yaml

@@ -0,0 +1,25 @@
+GetApriltagPose:
+  apriltag_family_name: 16h5
+  apriltag_ids:
+    - 0
+  apriltag_names:
+    - frame_id0
+  apriltag_sizes:
+    - 0.028
+  max_hamming: 0
+  nthreads: 1
+  quad_decimate: 1
+  quad_sigma: 0
+  refine_edges: true
+  z_up: true
+VisualServo:
+  ff_velocity_gain_angular: 0
+  ff_velocity_gain_linear: 0
+  include_ff: false
+  max_angular_output: 1
+  max_linear_output: 1
+  proportional_gain_angular: '10'
+  proportional_gain_linear: '150'
+  servo_spin_rate: '0.02'
+  target_servo_frame: 'manual_grasp_link'
+  fixed_servo_frame: 'base_link'

src/picknik_ur_site_config/objectives/open_cabinet_door_ml.xml

@@ -0,0 +1,76 @@
+<?xml version="1.0"?>
+<root BTCPP_format="4" main_tree_to_execute="Open Cabinet Door ML">
+    <!-- ////////// -->
+    <BehaviorTree ID="Open Cabinet Door ML"
+        _description="Move to Home waypoint, and then open a cabinet door detected by a network">
+        <Control ID="Sequence" name="Setup">
+            <Decorator ID="ForceSuccess">
+                <Action ID="CallTriggerService" 
+                    service_name="/io_and_status_controller/zero_ftsensor" />
+            </Decorator>
+            <Action ID="UpdateAdmittanceController"
+                config_file_name="open_door_admittance_controller_config.yaml" />
+            <Action ID="LoadObjectiveParameters" config_file_name="open_door_affordance_config.yaml"
+                parameters="{parameters}" />
+            <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="" />
+            <Action ID="MoveToJointState"
+                waypoint_name="Home"
+                planning_group_name="manipulator"
+                controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+                use_all_planners="false" />
+            <Action ID="DetectObjects"
+                rgb_image_topic="/wrist_mounted_camera/color/image_raw"
+                depth_image_topic="/wrist_mounted_camera/depth/image_rect_raw"
+                camera_info_topic="/wrist_mounted_camera/color/camera_info"
+                detect_objects_action_name="/detect_objects"
+                object_detections_goal="{object_detections_goal}"
+                object_detections_result="{object_detections_result}"
+                min_probability="0.1" />
+            <Action ID="GetCabinetDoorFromDetections"
+                object_detections_goal="{object_detections_goal}"
+                object_detections_result="{object_detections_result}"
+                fit_object_3d_service_name="fit_object_3d"
+                grasp_pose="{detected_grasp_pose}"
+                hinge_axis_pose_start="{hinge_axis_pose_start}"
+                hinge_axis_pose_end="{hinge_axis_pose_end}" />
+            <Action ID="GetHingeAxisFromSurfaceSelection"
+                target_grasp_pose="{detected_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}"
+                grasp_pose="{affordance_grasp_pose}" />
+            <Action ID="MoveGripperAction"
+                gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0" />
+            <Control ID="Sequence" name="OpenDoorAffordanceMain">
+                <Action ID="InitializeMTCTask"
+                    task_id="open_cabinet_door_ml"
+                    controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+                    task="{open_door_affordance_task}" />
+                <Action ID="SetupMTCCurrentState" task="{open_door_affordance_task}" />
+                <Action ID="SetupMTCAffordanceTemplate"
+                    name="SetupMTCOpenDoorAffordance"
+                    grasp_pose="{affordance_grasp_pose}"
+                    parameters="{parameters}"
+                    screw_axis_pose="{screw_axis_pose}"
+                    screw_origin_pose="{screw_origin_pose}"
+                    task="{open_door_affordance_task}" />
+                <Action ID="PlanMTCTask" solution="{open_door_affordance_solution}"
+                    task="{open_door_affordance_task}" />
+                <Fallback name="wait_for_approval_if_user_available">
+                    <Inverter>
+                        <Action ID="IsUserAvailable" />
+                    </Inverter>
+                    <Action ID="WaitForUserTrajectoryApproval"
+                        solution="{open_door_affordance_solution}" />
+                </Fallback>
+                <Action ID="ExecuteMTCTask" solution="{open_door_affordance_solution}" />
+            </Control>
+        </Control>
+    </BehaviorTree>
+</root>

src/picknik_ur_site_config/objectives/open_door_affordance.xml

@@ -2,11 +2,18 @@
 <root BTCPP_format="4" main_tree_to_execute="Open Cabinet Door">
     <BehaviorTree ID="Open Cabinet Door" _description="Open a cabinet door by grasping and pulling its handle">
         <Control ID="Sequence" name="Setup">
+            <!-- Tare the force torque sensor -->
+            <Decorator ID="ForceSuccess">
+                <Action ID="CallTriggerService" service_name="/io_and_status_controller/zero_ftsensor" />
+            </Decorator>
             <!-- Set admittance control parameters at the start of the Objective -->
             <Action ID="UpdateAdmittanceController" config_file_name="open_door_admittance_controller_config.yaml"/>
             <!-- Execute the Objective -->
             <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}" />
             <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0" />
             <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}" />

src/picknik_ur_site_config/objectives/push_button_ml.xml

@@ -0,0 +1,101 @@
+<?xml version="1.0"?>
+<root BTCPP_format="4" main_tree_to_execute="Push Button ML">
+    <BehaviorTree ID="Push Button ML"
+        _description="Move to Home waypoint, and then push a button detected by a network with the end effector">
+        <Control ID="Sequence">
+            <Decorator ID="ForceSuccess">
+                <Action ID="CallTriggerService" 
+                    service_name="/io_and_status_controller/zero_ftsensor" />
+            </Decorator>                  
+            <Action ID="LoadObjectiveParameters" config_file_name="push_along_axis_config.yaml"
+                parameters="{parameters}" />
+            <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="" />
+            <Action ID="MoveToJointState"
+                waypoint_name="Home"
+                planning_group_name="manipulator"
+                controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+                use_all_planners="false" />
+            <Action ID="DetectObjects"
+                rgb_image_topic="/wrist_mounted_camera/color/image_raw"
+                depth_image_topic="/wrist_mounted_camera/depth/image_rect_raw"
+                camera_info_topic="/wrist_mounted_camera/color/camera_info"
+                detect_objects_action_name="/detect_objects"
+                object_detections_goal="{object_detections_goal}"
+                object_detections_result="{object_detections_result}"
+                min_probability="0.1" />
+            <Action ID="GetButtonFromDetections"
+                object_detections_goal="{object_detections_goal}"
+                object_detections_result="{object_detections_result}"
+                fit_object_3d_service_name="fit_object_3d"
+                button_pose="{button_pose}"
+                object_name="button" />
+            <SubTree ID="CloseGripper" />
+            <Action ID="SaveCurrentState" saved_robot_state="{initial_robot_state}" />
+            <Control ID="Sequence">
+                <Action ID="InitializeMTCTask" task_id="push_button_ml" controller_names="/joint_trajectory_controller"
+                    task="{push_along_axis_task}" />
+                <Action ID="SetupMTCCurrentState" task="{push_along_axis_task}" />
+                <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link"
+                    planning_group_name="manipulator" target_pose="{button_pose}"
+                    task="{push_along_axis_task}" />
+                <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="SetupMTCUpdateGroupCollisionRule"
+                    name="ForbidGripperCollisionWithOctomap" parameters="{parameters}"
+                    task="{push_along_axis_task}" />
+                <Action ID="PlanMTCTask" solution="{full_push_along_axis_solution}"
+                    task="{push_along_axis_task}" />
+                <Action ID="SplitMTCSolution" solution_in="{full_push_along_axis_solution}"
+                    index="3" solution_out_1="{move_to_approach_solution}"
+                    solution_out_2="{push_solution}" />
+                <Action ID="WaitForUserTrajectoryApproval"
+                    solution="{full_push_along_axis_solution}" />
+                <Action ID="ExecuteMTCTask" solution="{move_to_approach_solution}" />
+
+                <Action ID="SaveCurrentState" saved_robot_state="{pre_approach_robot_state}" />
+                <ForceSuccess>
+                    <ReactiveSequence>
+                        <Action ID="IsForceWithinThreshold" parameters="{parameters}" />
+                        <Action ID="ExecuteMTCTask" solution="{push_solution}" />
+                    </ReactiveSequence>
+                </ForceSuccess>
+            </Control>
+            <SubTree ID="RetreatToInitialPose"
+                pre_approach_robot_state="{pre_approach_robot_state}"
+                initial_robot_state="{initial_robot_state}" parameters="{parameters}" />
+        </Control>
+    </BehaviorTree>
+    <BehaviorTree ID="CloseGripper">
+        <Control ID="Sequence" name="close_gripper_main">
+            <Action ID="MoveGripperAction"
+                gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
+                position="0.7929" />
+        </Control>
+    </BehaviorTree>
+    <BehaviorTree ID="RetreatToInitialPose">
+        <Control ID="Sequence">
+            <Action ID="InitializeMTCTask" task_id="push_button_ml_retreat" controller_names="/joint_trajectory_controller"
+                task="{retreat_task}" />
+            <Action ID="SetupMTCCurrentState" task="{retreat_task}" />
+            <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap"
+                parameters="{parameters}" task="{retreat_task}" />
+            <Action ID="SetupMTCCartesianMoveToJointState" joint_state="{pre_approach_robot_state}"
+                planning_group_name="manipulator" task="{retreat_task}" />
+            <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap"
+                parameters="{parameters}" task="{retreat_task}" />
+            <Action ID="SetupMTCMoveToJointState" joint_state="{initial_robot_state}"
+                planning_group_name="manipulator" task="{retreat_task}" />
+            <Action ID="PlanMTCTask" solution="{return_to_initial_waypoint_solution}"
+                task="{retreat_task}" />
+            <Action ID="ExecuteMTCTask" solution="{return_to_initial_waypoint_solution}" />
+        </Control>
+    </BehaviorTree>
+</root>