From 539ec2161a01e07464da8c1e069b3f4f68aca97c Mon Sep 17 00:00:00 2001 From: 0nhc Date: Fri, 18 Oct 2024 13:37:02 -0500 Subject: [PATCH] update --- cfg/active_grasp.rviz | 12 ++++++------ cfg/active_grasp.yaml | 1 + cfg/sim/challenging_scene_1.yaml | 11 +++++++++++ scripts/run.py | 2 +- src/active_grasp/active_perception_policy.py | 7 ++++--- 5 files changed, 23 insertions(+), 10 deletions(-) create mode 100644 cfg/sim/challenging_scene_1.yaml diff --git a/cfg/active_grasp.rviz b/cfg/active_grasp.rviz index 7936e10..c3d16ff 100644 --- a/cfg/active_grasp.rviz +++ b/cfg/active_grasp.rviz @@ -7,7 +7,7 @@ Panels: - /TF1/Frames1 - /Markers1/Namespaces1 Splitter Ratio: 0.6881287693977356 - Tree Height: 452 + Tree Height: 104 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -385,7 +385,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 2.2946388721466064 + Distance: 1.376084327697754 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -428,12 +428,12 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 1376 + Height: 612 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -443,5 +443,5 @@ Window Geometry: Views: collapsed: true Width: 1244 - X: 1316 - Y: 27 + X: 1184 + Y: 195 diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml index 83c9bf8..f96a272 100644 --- a/cfg/active_grasp.yaml +++ b/cfg/active_grasp.yaml @@ -2,6 +2,7 @@ bt_sim: gui: True gripper_force: 10 scene: random + # scene: $(find active_grasp)/cfg/sim/challenging_scene_1.yaml hw: roi_calib_file: $(find active_grasp)/cfg/hw/T_base_tag.txt diff --git a/cfg/sim/challenging_scene_1.yaml b/cfg/sim/challenging_scene_1.yaml new file mode 100644 index 0000000..3a270bf --- /dev/null +++ b/cfg/sim/challenging_scene_1.yaml @@ -0,0 +1,11 @@ +center: [0.5, 0.2, 0.25] +q: [0.0, -1.39, 0.0, -2.36, 0.0, 1.57, 0.79] +objects: +- object_id: ap_dataset/objects/apple + xyz: [0.0, 0.0, 0.2] + rpy: [90, 0, 0] + scale: 0.001 +- object_id: ap_dataset/occluders/flower_pot_021 + xyz: [0.0, 0.0, 0.2] + rpy: [90, 0, 0] + scale: 0.001 diff --git a/scripts/run.py b/scripts/run.py index d072912..9c820d0 100755 --- a/scripts/run.py +++ b/scripts/run.py @@ -46,7 +46,7 @@ def main(): def create_parser(): parser = argparse.ArgumentParser() parser.add_argument("policy", type=str, choices=registry.keys()) - parser.add_argument("--runs", type=int, default=10) + parser.add_argument("--runs", type=int, default=20) parser.add_argument("--wait-for-input", action="store_true") parser.add_argument("--logdir", type=Path, default="logs") parser.add_argument("--seed", type=int, default=1) diff --git a/src/active_grasp/active_perception_policy.py b/src/active_grasp/active_perception_policy.py index b124b63..301bd9a 100644 --- a/src/active_grasp/active_perception_policy.py +++ b/src/active_grasp/active_perception_policy.py @@ -162,7 +162,7 @@ class ActivePerceptionMultiViewPolicy(MultiViewPolicy): target_points = np.asarray(target_points) target_points = target_points.reshape(1, target_points.shape[0], 3) - self.pcdvis.update_points(target_points) + # self.pcdvis.update_points(target_points) target_points = torch.from_numpy(target_points).float().to("cuda:0") scene_points = np.asarray(scene_points) scene_points = scene_points.reshape(1, scene_points.shape[0], 3) @@ -212,8 +212,9 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy): target_points_np = target_points.cpu().numpy()[0,:,:] central_point_of_target = np.mean(target_points_np, axis=0) - look_at_center = torch.from_numpy(central_point_of_target).float().to("cuda:0") + print(f"Central Point of Target: {central_point_of_target}, length: {np.linalg.norm(central_point_of_target)}") + print(f"camera position: {current_cam_pose[:3, 3]}") nbv_tensor = self.get_transformed_mat(current_cam_pose, est_delta_rot_mat, look_at_center) @@ -320,7 +321,7 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy): target_points = np.asarray(target_points) target_points = target_points.reshape(1, target_points.shape[0], 3) - self.pcdvis.update_points(target_points) + # self.pcdvis.update_points(target_points) target_points = torch.from_numpy(target_points).float().to("cuda:0") scene_points = np.asarray(scene_points) scene_points = scene_points.reshape(1, scene_points.shape[0], 3)