Scan and plan objective (#139)

* New scan and plan objective. * Add trajectory to objectives directory. * Lint. * Trajectory is now relative to object's frame. * Remove old parameter. * Fix indentation. * Fix indentation. * Renumbered poses. * Grouped in subtrees.
PickNikRobotics · Dec 6, 2023 · 7733e10 · 7733e10
1 parent 42e25a6
commit 7733e10
Show file tree

Hide file tree

Showing 5 changed files with 226 additions and 0 deletions.
diff --git a/src/picknik_ur_gazebo_scan_and_plan_config/CMakeLists.txt b/src/picknik_ur_gazebo_scan_and_plan_config/CMakeLists.txt
@@ -7,6 +7,8 @@ install(
   DIRECTORY
     config
     description
+    waypoints
+    objectives
   DESTINATION
     share/${PROJECT_NAME}
 )

diff --git a/src/picknik_ur_gazebo_scan_and_plan_config/config/config.yaml b/src/picknik_ur_gazebo_scan_and_plan_config/config/config.yaml
@@ -9,3 +9,14 @@ based_on_package: "picknik_ur_gazebo_config"
 optional_feature_params:
   gazebo_world_package_name: "picknik_ur_gazebo_scan_and_plan_config"
   gazebo_world_path: "description/simulation_worlds/scan_and_plan_world.sdf"
+
+objectives:
+  # Override with a new set of waypoints based on the Gazebo world.
+  waypoints_file:
+    package_name: "picknik_ur_gazebo_scan_and_plan_config"
+    relative_path: "waypoints/waypoints.yaml"
+  # Add new simulation Objectives in addition to the one in the base config package.
+  objective_library_paths:
+    sim:
+      package_name: "picknik_ur_gazebo_scan_and_plan_config"
+      relative_path: "objectives"
diff --git a/src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_inspection.xml b/src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_inspection.xml
@@ -0,0 +1,47 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Object Inspection">
+  <!-- ////////// -->
+  <BehaviorTree ID="Object Inspection" _description="Object location estimated first with ICP; then, the object is 3D-reconstructed from multiple views. " _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="EstimateObjectPose" 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}"/>
+      <Action ID="LoadPoseStampedVectorFromYaml" file_path="/home/studio-user/.config/moveit_studio/picknik_ur_gazebo_scan_and_plan_config/objectives/trajectory_around_hercules.yaml" output="{pose_stamped_msgs}"/>
+      <Decorator ID="ForEachPoseStamped" vector_in="{pose_stamped_msgs}" out="{trajectory_pose}">
+        <Control ID="Sequence">
+          <Action ID="TransformPoseWithPose" input_pose="{trajectory_pose}" transform_pose="{model_to_real_pose}" output_pose="{target_pose}"/>
+          <SubTree ID="MoveToPose" target_pose="{target_pose}"/>
+          <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_world}"/>
+          <Action ID="AddPointCloudToVector" point_cloud="{point_cloud_world}" point_cloud_vector="{point_cloud_vector}"/>
+        </Control>
+      </Decorator>
+      <Action ID="ClearSnapshot"/>
+      <Action ID="MergePointClouds" point_clouds="{point_cloud_vector}" grid_resolution_meters="0.001" merged_cloud="{merged_cloud}"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{merged_cloud}" sensor_name="scene_scan_camera" point_cloud_uuid="" pcd_topic="/pcd_pointcloud_captures"/>
+      <Action ID="SavePointCloudToFile" point_cloud="{merged_cloud}" file_path="~/.config/moveit_studio/saved_behavior_data" file_prefix="pointcloud"/>
+    </Control>
+  </BehaviorTree>
+  <BehaviorTree ID="MoveToPose">
+    <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="{target_pose}" use_all_planners="false" task="{move_to_pose_task}"/>
+      <Action ID="PlanMTCTask" task="{move_to_pose_task}" solution="{move_to_pose_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}"/>
+    </Control>
+  </BehaviorTree>
+  <BehaviorTree ID="EstimateObjectPose">
+    <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>
diff --git a/src/picknik_ur_gazebo_scan_and_plan_config/objectives/trajectory_around_hercules.yaml b/src/picknik_ur_gazebo_scan_and_plan_config/objectives/trajectory_around_hercules.yaml
@@ -0,0 +1,41 @@
+pose1:
+ header:
+  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
+
+pose2:
+ header:
+  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
+
+pose3:
+ header:
+  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
diff --git a/src/picknik_ur_gazebo_scan_and_plan_config/waypoints/waypoints.yaml b/src/picknik_ur_gazebo_scan_and_plan_config/waypoints/waypoints.yaml
@@ -0,0 +1,125 @@
+- description: ''
+  favorite: false
+  joint_state:
+    effort: []
+    header:
+      frame_id: world
+      stamp:
+        nanosec: 0
+        sec: 0
+    name:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    position:
+      - -0.6458566060390687
+      - -0.3877845139911879
+      - -1.8848677930083997
+      - -1.5467516570804267
+      - 2.0648023306748278
+      - -0.3257925328124284
+    velocity: []
+  name: Look at Object
+- description: ''
+  favorite: false
+  joint_state:
+    effort: []
+    header:
+      frame_id: world
+      stamp:
+        nanosec: 0
+        sec: 0
+    name:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    position:
+      - -0.61
+      - -0.78
+      - -0.45
+      - -2.49
+      - 1.64
+      - 0
+    velocity: []
+  name: Right Corner
+- description: ''
+  favorite: true
+  joint_state:
+    effort: []
+    header:
+      frame_id: world
+      stamp:
+        nanosec: 0
+        sec: 0
+    name:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    position:
+      - 0
+      - -0.78
+      - -0.51
+      - -2.4
+      - 1.65
+      - 0
+    velocity: []
+  name: Home
+- description: ''
+  favorite: false
+  joint_state:
+    effort: []
+    header:
+      frame_id: world
+      stamp:
+        nanosec: 0
+        sec: 0
+    name:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    position:
+      - 0.58
+      - -0.78
+      - -0.45
+      - -2.33
+      - 1.64
+      - 0
+    velocity: []
+  name: Left Corner
+- description: ''
+  favorite: false
+  joint_state:
+    effort: []
+    header:
+      frame_id: world
+      stamp:
+        nanosec: 0
+        sec: 0
+    name:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    position:
+      - 0
+      - -1.41
+      - -0.7
+      - -2.12
+      - 1.67
+      - 0
+    velocity: []
+  name: Forward Down