From 0ac5ca20aaee8803b357af8920b6157220d739fe Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Wed, 10 Nov 2021 12:15:33 +0100 Subject: [PATCH] Minor --- src/active_grasp/simulation.py | 10 ++++++++-- test/test_sim_scene.py | 3 ++- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/active_grasp/simulation.py b/src/active_grasp/simulation.py index 641e66d..fbdd78c 100644 --- a/src/active_grasp/simulation.py +++ b/src/active_grasp/simulation.py @@ -10,6 +10,7 @@ from robot_helpers.spatial import Rotation from vgn.perception import UniformTSDFVolume from vgn.utils import find_urdfs, load_cfg, view_on_sphere from vgn.detection import VGN, select_local_maxima +import vgn.visualizer as vis rospack = rospkg.RosPack() pkg_root = Path(rospack.get_path("active_grasp")) @@ -94,7 +95,7 @@ class Simulation: # First, reconstruct the scene from many views tsdf = UniformTSDFVolume(self.scene.length, 40) r = 2.0 * self.scene.length - theta = np.pi / 6.0 + theta = np.pi / 4.0 phis = np.linspace(0.0, 2.0 * np.pi, 5) for view in [view_on_sphere(center, r, theta, phi) for phi in phis]: depth_img = self.camera.get_image(view)[1] @@ -103,7 +104,12 @@ class Simulation: # Then check whether VGN can find any grasps on the target out = self.vgn.predict(tsdf_grid) - grasps, _ = select_local_maxima(voxel_size, out, threshold=0.9) + grasps, qualities = select_local_maxima(voxel_size, out, threshold=0.9) + + # vis.scene_cloud(voxel_size, tsdf.get_scene_cloud()) + # vis.grasps(grasps, qualities, 0.05) + # vis.show() + for grasp in grasps: pose = origin * grasp.pose tip = pose.rotation.apply([0, 0, 0.05]) + pose.translation diff --git a/test/test_sim_scene.py b/test/test_sim_scene.py index cd4aea6..b671e7e 100644 --- a/test/test_sim_scene.py +++ b/test/test_sim_scene.py @@ -6,7 +6,8 @@ def main(): scene_id = "random" vgn_path = "../vgn/assets/models/vgn_conv.pth" sim = Simulation(gui, scene_id, vgn_path) - bbox = sim.reset() + while True: + sim.reset() if __name__ == "__main__":