Remove getPoseFromUser from remainging objectives, replace with new s…

…equence including getPointsFromUser to AdjustPose. pick_and_place_object still a WIP.
PickNikRobotics · Apr 5, 2024 · 36bd829 · 36bd829
1 parent 6241cbf
commit 36bd829
Show file tree

Hide file tree

Showing 5 changed files with 57 additions and 24 deletions.
diff --git a/src/picknik_ur_gazebo_config/objectives/apriltag1_grasp_offset.yaml b/src/picknik_ur_gazebo_config/objectives/apriltag1_grasp_offset.yaml
@@ -1,10 +1,10 @@
 GraspOffset:
   position:
-    x: -0.1
-    y: 0.0
-    z: 0.05
+    x: 0.0037551670003079241
+    y: 0.40295562984993416
+    z: 0.2738516292962398
   orientation:
-    x: 0.707
-    y: -0.707
-    z: 0.0
-    w: 0.0
+    x: -0.62071616682997577
+    y: 0.78356758411692673
+    z: 0.0076953251135729161
+    w: 0.025962729603437859
diff --git a/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml b/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml
@@ -2,11 +2,18 @@
 <root BTCPP_format="4" main_tree_to_execute="Teach Grasp Offset From Apriltag">
     <BehaviorTree ID="Teach Grasp Offset From Apriltag" _description="Saves a grasp offset to yaml file from Apriltag observation using an interactive marker" _favorite="true">
         <Control ID="Sequence" name="root">
-            <Action ID="GetPoseFromUser" parameter_name="calculate_offset.grasp_pose" parameter_value="{user_grasp_pose}" />
+            <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="grasp_pose" point_prompts="Click on the Apriltag" pixel_coords="{pixel_coords}"/>
+            <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
+            <Action ID="GetPoseFromPixelCoords" point_cloud="{point_cloud}" pixel_coords="{pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{output_poses}"/>
+            <Action ID="AdjustPoseWithIMarker" prompts="Adjust IMarker to desired pose" initial_poses="{output_poses}" adjusted_poses="{adjusted_poses}"/>
             <SubTree ID="Sample April Tag" num_samples="5" tag_id="1" apriltag_config="apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
             <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
-            <Action ID="CalculatePoseOffset" source_pose="{tag_pose_world}" destination_pose="{user_grasp_pose}" source_to_destination_pose="{tag_to_user_grasp_pose}"/>
-            <Action ID="SavePoseToYaml" yaml_filename="apriltag1_grasp_offset" namespace="GraspOffset" message="{tag_to_user_grasp_pose}"/>
+            <Decorator ID="ForEachPoseStamped" vector_in="{adjusted_poses}" out="{user_grasp_pose}">
+                <Control ID="Sequence">
+                    <Action ID="CalculatePoseOffset" source_pose="{tag_pose_world}" destination_pose="{user_grasp_pose}" source_to_destination_pose="{tag_to_user_grasp_pose}"/>
+                    <Action ID="SavePoseToYaml" yaml_filename="apriltag1_grasp_offset" namespace="GraspOffset" message="{tag_to_user_grasp_pose}"/>
+                </Control>
+            </Decorator>
         </Control>
     </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_site_config/objectives/pick_object.xml b/src/picknik_ur_site_config/objectives/pick_object.xml
@@ -3,17 +3,23 @@
     <BehaviorTree ID="Pick Object" _description="Pick up and lift a small object" _favorite="true">
         <Control ID="Sequence" name="root">
             <SubTree ID="Open Gripper"/>
-            <!-- Wrap in a ForceSuccess decorator so this can be used as both a top-level tree and subtree -->
-            <Decorator ID="ForceSuccess">
-                <Action ID="GetPoseFromUser" parameter_name="pick_object.grasp_pose" parameter_value="{grasp_pose}" />
-            </Decorator>
+            <Control ID="Sequence">
+                <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="grasp_pose" point_prompts="Select the object you wish to pick" pixel_coords="{pixel_coords}"/>
+                <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
+                <Action ID="GetPoseFromPixelCoords" point_cloud="{point_cloud}" pixel_coords="{pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{output_poses}"/>
+                <Action ID="AdjustPoseWithIMarker" prompts="Adjust IMarker to desired pose" initial_poses="{output_poses}" adjusted_poses="{adjusted_poses}"/>
+            </Control>
             <Action ID="LoadObjectiveParameters" config_file_name="pick_object_config.yaml" parameters="{parameters}"/>
             <Action ID="InitializeMTCTask" task_id="pick_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{pick_object_task}"/>
             <Action ID="SetupMTCCurrentState" task="{pick_object_task}"/>
-            <Action ID="SetupMTCPickObject" grasp_pose="{grasp_pose}" task="{pick_object_task}" parameters="{parameters}"/>
-            <Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/>
-            <SubTree ID="Wait for Trajectory Approval if User Available" solution="{pick_object_solution}"/>
-            <Action ID="ExecuteMTCTask" solution="{pick_object_solution}"/>
+            <Decorator ID="ForEachPoseStamped" vector_in="{output_poses}" out="{grasp_pose}">
+                <Control ID="Sequence">
+                    <Action ID="SetupMTCPickObject" grasp_pose="{grasp_pose}" task="{pick_object_task}" parameters="{parameters}"/>
+                    <Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/>
+                    <SubTree ID="Wait for Trajectory Approval if User Available" solution="{pick_object_solution}"/>
+                    <Action ID="ExecuteMTCTask" solution="{pick_object_solution}"/>
+                </Control>
+            </Decorator>
         </Control>
     </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_site_config/objectives/pick_place_object.xml b/src/picknik_ur_site_config/objectives/pick_place_object.xml
@@ -24,7 +24,13 @@
                             <!-- 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="GetPoseFromUser" parameter_name="pick_place_object.grasp_pose" parameter_value="{grasp_pose}" />
+                            <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="grasp_pose" point_prompts="Click on the object you wish to pick" pixel_coords="{pixel_coords}"/>
+                            <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
+                            <Action ID="GetPoseFromPixelCoords" point_cloud="{point_cloud}" pixel_coords="{pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{output_poses}"/>
+                            <Action ID="AdjustPoseWithIMarker" prompts="Adjust IMarker to desired pose" initial_poses="{output_poses}" adjusted_poses="{adjusted_poses}"/>
+                            <!--  -->
                             <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"/>
@@ -65,9 +71,16 @@
                                         <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="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" />
+                                            <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="grasp_pose" point_prompts="Click on the Apriltag" pixel_coords="{pixel_coords}"/>
+                                            <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
+                                            <Action ID="GetPoseFromPixelCoords" point_cloud="{point_cloud}" pixel_coords="{pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{output_poses}"/>
+                                            <Action ID="AdjustPoseWithIMarker" prompts="Adjust IMarker to desired pose" initial_poses="{output_poses}" adjusted_poses="{adjusted_poses}"/>
+                                            <Decorator ID="ForEachPoseStamped" vector_in="{adjusted_poses}" out="{grasp_pose}">
+                                                <Control ID="Sequence">
+                                                    <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>
+                                            </Decorator>
                                         </Control>
 
                                         <!-- ELSE -->

diff --git a/src/picknik_ur_site_config/objectives/teach_apriltag_grasp_offset.xml b/src/picknik_ur_site_config/objectives/teach_apriltag_grasp_offset.xml
@@ -2,11 +2,18 @@
 <root BTCPP_format="4" main_tree_to_execute="Teach Grasp Offset From Apriltag">
     <BehaviorTree ID="Teach Grasp Offset From Apriltag" _description="Saves a grasp offset to yaml file from Apriltag observation using an interactive marker" _favorite="true">
         <Control ID="Sequence" name="root">
-            <Action ID="GetPoseFromUser" parameter_name="calculate_offset.grasp_pose" parameter_value="{user_grasp_pose}" />
+            <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="grasp_pose" point_prompts="Click on the Apriltag" pixel_coords="{pixel_coords}"/>
+            <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
+            <Action ID="GetPoseFromPixelCoords" point_cloud="{point_cloud}" pixel_coords="{pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{output_poses}"/>
+            <Action ID="AdjustPoseWithIMarker" prompts="Adjust IMarker to desired pose" initial_poses="{output_poses}" adjusted_poses="{adjusted_poses}"/>
             <SubTree ID="Sample April Tag" num_samples="5" tag_id="1" apriltag_config="51mm_apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
             <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
-            <Action ID="CalculatePoseOffset" source_pose="{tag_pose_world}" destination_pose="{user_grasp_pose}" source_to_destination_pose="{tag_to_user_grasp_pose}"/>
-            <Action ID="SavePoseToYaml" yaml_filename="apriltag1_grasp_offset" namespace="GraspOffset" message="{tag_to_user_grasp_pose}"/>
+            <Decorator ID="ForEachPoseStamped" vector_in="{adjusted_poses}" out="{user_grasp_pose}">
+                <Control ID="Sequence">
+                    <Action ID="CalculatePoseOffset" source_pose="{tag_pose_world}" destination_pose="{user_grasp_pose}" source_to_destination_pose="{tag_to_user_grasp_pose}"/>
+                    <Action ID="SavePoseToYaml" yaml_filename="apriltag1_grasp_offset" namespace="GraspOffset" message="{tag_to_user_grasp_pose}"/>
+                </Control>
+            </Decorator>
         </Control>
     </BehaviorTree>
 </root>