This repository has been archived by the owner on Dec 13, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 6
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Fix 3D object registration Objectives (#181)
- Loading branch information
Showing
4 changed files
with
62 additions
and
56 deletions.
There are no files selected for viewing
17 changes: 17 additions & 0 deletions
17
src/picknik_ur_gazebo_scan_and_plan_config/objectives/estimate_object_pose.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,17 @@ | ||
| <?xml version='1.0' encoding='UTF-8'?> | ||
| <root BTCPP_format="4" main_tree_to_execute="Estimate Object Pose"> | ||
| <!-- ////////// --> | ||
| <BehaviorTree ID="Estimate Object Pose" _description="Estimate an object pose from a reference mesh file using ICP on live point cloud data." _favorite="false" _hardcoded="false"> | ||
| <Control ID="Sequence"> | ||
| <Action ID="LoadPointCloudFromFile" file_path="{object_model_file_path}" frame_id="world" num_sampled_points="10000" random_seed="1234" point_cloud="{model_point_cloud}" scale="1.0" color="255;150;0"/> | ||
| <Action ID="GetSynchronizedCameraTopics" point_cloud_topic_name="/wrist_mounted_camera/depth/color/points" rgb_image_topic_name="/wrist_mounted_camera/color/image_raw" rgb_camera_info_topic_name="/wrist_mounted_camera/color/camera_info" point_cloud="{point_cloud}" rgb_image="{rgb_image}" rgb_camera_info="{rgb_camera_info}"/> | ||
| <Action ID="TransformPointCloudFrame" input_cloud="{point_cloud}" target_frame="world" output_cloud="{point_cloud}"/> | ||
| <Action ID="CreateStampedPose" reference_frame="world" position_xyz="{guess_position}" orientation_xyzw="{guess_orientation}" stamped_pose="{object_stamped_pose_estimate}"/> | ||
| <Action ID="TransformPointCloud" input_cloud="{model_point_cloud}" transform_pose="{object_stamped_pose_estimate}" output_cloud="{model_point_cloud}"/> | ||
| <Action ID="RegisterPointClouds" base_point_cloud="{model_point_cloud}" target_point_cloud="{point_cloud}" max_iterations="30" max_correspondence_distance="{icp_max_correspondence_distance}" target_pose_in_base_frame="{model_to_real_pose}"/> | ||
| <Action ID="TransformPointCloud" input_cloud="{model_point_cloud}" transform_pose="{model_to_real_pose}" output_cloud="{aligned_model_cloud}"/> | ||
| <Action ID="SendPointCloudToUI" point_cloud="{aligned_model_cloud}" sensor_name="scene_scan_camera" point_cloud_uuid="" pcd_topic="/pcd_pointcloud_captures"/> | ||
| <Action ID="TransformPoseWithPose" input_pose="{object_stamped_pose_estimate}" transform_pose="{model_to_real_pose}" output_pose="{model_to_real_pose}"/> | ||
| </Control> | ||
| </BehaviorTree> | ||
| </root> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
11 changes: 11 additions & 0 deletions
11
src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_registration.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,11 @@ | ||
| <?xml version='1.0' encoding='UTF-8'?> | ||
| <root BTCPP_format="4" main_tree_to_execute="Object Registration"> | ||
| <!-- ////////// --> | ||
| <BehaviorTree ID="Object Registration" _description="Estimate the pose of an object using ICP." _favorite="true" _hardcoded="false"> | ||
| <Control ID="Sequence" name="TopLevelSequence"> | ||
| <Action ID="ClearSnapshot"/> | ||
| <Action ID="MoveToWaypoint" waypoint_name="Look at Object" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/> | ||
| <SubTree ID="Estimate Object Pose" object_model_file_path="/opt/overlay_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/farnese-hercules-head.stl" guess_position="-0.5;0.0;0.0" guess_orientation="0.0;0.0;0.6442177;0.7648422" icp_max_correspondence_distance="0.5" model_to_real_pose="{model_to_real_pose}"/> | ||
| </Control> | ||
| </BehaviorTree> | ||
| </root> |
60 changes: 30 additions & 30 deletions
60
src/picknik_ur_gazebo_scan_and_plan_config/objectives/trajectory_around_hercules.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,41 +1,41 @@ | ||
| --- | ||
| header: | ||
| frame_id: "world" | ||
| frame_id: "world" | ||
| pose: | ||
| position: | ||
| x: 0.4 | ||
| y: -0.2 | ||
| z: 0.4 | ||
| orientation: | ||
| x: -0.6532815 | ||
| y: -0.2705981 | ||
| z: 0.270598 | ||
| w: 0.6532815 | ||
| position: | ||
| x: 0.4 | ||
| y: -0.2 | ||
| z: 0.4 | ||
| orientation: | ||
| x: -0.707733 | ||
| y: -0.2459841 | ||
| z: 0.2931526 | ||
| w: 0.5938581 | ||
|
|
||
| --- | ||
| header: | ||
| frame_id: "world" | ||
| frame_id: "world" | ||
| pose: | ||
| position: | ||
| x: 0.0 | ||
| y: -0.2 | ||
| z: 0.4 | ||
| orientation: | ||
| x: -0.7071068 | ||
| y: 0.0 | ||
| z: 0.0 | ||
| w: 0.7071068 | ||
| position: | ||
| x: 0.0 | ||
| y: -0.2 | ||
| z: 0.4 | ||
| orientation: | ||
| x: -0.7660447 | ||
| y: 0.0 | ||
| z: 0.0 | ||
| w: 0.6427873 | ||
|
|
||
| --- | ||
| header: | ||
| frame_id: "world" | ||
| frame_id: "world" | ||
| pose: | ||
| position: | ||
| x: -0.4 | ||
| y: -0.2 | ||
| z: 0.4 | ||
| orientation: | ||
| x: -0.6532815 | ||
| y: 0.2705981 | ||
| z: -0.270598 | ||
| w: 0.6532815 | ||
| position: | ||
| x: -0.4 | ||
| y: -0.2 | ||
| z: 0.4 | ||
| orientation: | ||
| x: -0.707733 | ||
| y: 0.2459841 | ||
| z: -0.2931526 | ||
| w: 0.5938581 |