diff --git a/cfg/active_grasp.rviz b/cfg/active_grasp.rviz index 16f7276..12afbaa 100644 --- a/cfg/active_grasp.rviz +++ b/cfg/active_grasp.rviz @@ -256,10 +256,7 @@ Visualization Manager: Name: Markers Namespaces: bbox: true - grasp: true - grasps: true path: true - roi: true views: true Queue Size: 100 Value: true @@ -427,7 +424,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.261960744857788 + Distance: 1.094533920288086 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -435,17 +432,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0.5695413947105408 - Y: -0.03970015048980713 - Z: 0.45675671100616455 + X: 0.32280832529067993 + Y: 0.16504701972007751 + Z: 0.43913549184799194 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.295397013425827 + Pitch: 0.2053970992565155 Target Frame: - Yaw: 5.118584632873535 + Yaw: 0.8653952479362488 Saved: - Class: rviz/Orbit Distance: 1.2000000476837158 @@ -486,4 +483,4 @@ Window Geometry: collapsed: true Width: 1095 X: 1260 - Y: 86 + Y: 27 diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml index 471a5d1..5c38f8f 100644 --- a/cfg/active_grasp.yaml +++ b/cfg/active_grasp.yaml @@ -2,7 +2,8 @@ bt_sim: gui: True gripper_force: 10 # scene: random - scene: $(find active_grasp)/cfg/sim/challenging_scene_2.yaml + scene: manual + # scene: $(find active_grasp)/cfg/sim/challenging_scene_2.yaml hw: roi_calib_file: $(find active_grasp)/cfg/hw/T_base_tag.txt @@ -13,7 +14,7 @@ grasp_controller: ee_grasp_offset: [0.0, 0.0, -0.383, 0.924, 0.0, 0.0, 0.065] # offset to panda_link8 control_rate: 100 linear_vel: 0.05 - move_to_target_threshold: 0.05 # meter + move_to_target_threshold: 0.01 # meter camera: frame_id: camera_depth_optical_frame info_topic: /camera/depth/camera_info diff --git a/cfg/sim/mustard.yaml b/cfg/sim/mustard.yaml deleted file mode 100644 index 89be7ad..0000000 --- a/cfg/sim/mustard.yaml +++ /dev/null @@ -1,7 +0,0 @@ -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: ycb/006_mustard_bottle - xyz: [0.0, 0.0, 0.0] - rpy: [0, 0, -50] - scale: 0.8 diff --git a/scripts/run.py b/scripts/run.py index f2fc7a4..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=5) + 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 f95f875..eb1dc29 100644 --- a/src/active_grasp/active_perception_policy.py +++ b/src/active_grasp/active_perception_policy.py @@ -269,7 +269,11 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy): # gsnet_input_points = target_points_list # gsnet_input_points = merged_points_list self.publish_pointcloud(gsnet_input_points) - gsnet_grasping_poses = np.asarray(self.request_grasping_pose(gsnet_input_points)) + received_points = False + while(received_points == False): + gsnet_grasping_poses = np.asarray(self.request_grasping_pose(gsnet_input_points)) + received_points = True + print(gsnet_grasping_poses[0].keys()) # DEBUG: publish grasps # self.publish_grasps(gsnet_grasping_poses) diff --git a/src/active_grasp/simulation.py b/src/active_grasp/simulation.py index e198847..d94a845 100644 --- a/src/active_grasp/simulation.py +++ b/src/active_grasp/simulation.py @@ -232,11 +232,57 @@ class RandomScene(Scene): return q +class ManualScene(Scene): + def __init__(self): + super().__init__() + self.config_path = pkg_root / "cfg/sim" + self.scene_index = 0 + # Visit the directory and read all the yaml files + self.scenes = [] + for file in self.config_path.iterdir(): + if file.suffix == ".yaml": + self.scenes.append(file) + self.num_scenes = len(self.scenes) + + def load_config(self): + self.scene = load_yaml(self.scenes[self.scene_index]) + self.center = np.asarray(self.scene["center"]) + self.length = 0.3 + self.origin = self.center - np.r_[0.5 * self.length, 0.5 * self.length, 0.0] + + + def generate(self, rng): + self.load_config() + self.add_support(self.center) + + for object in self.scene["objects"]: + urdf = urdfs_dir / object["object_id"] / "model.urdf" + ori = Rotation.from_euler("xyz", object["rpy"], degrees=True) + pos = self.center + np.asarray(object["xyz"]) + scale = object.get("scale", 1) + if randomize := object.get("randomize", False): + angle = rng.uniform(-randomize["rot"], randomize["rot"]) + ori = Rotation.from_euler("z", angle, degrees=True) * ori + b = np.asarray(randomize["pos"]) + pos += rng.uniform(-b, b) + self.add_object(urdf, ori, pos, scale) + for _ in range(60): + p.stepSimulation() + + self.scene_index += 1 + if(self.scene_index >= self.num_scenes): + self.scene_index = 0 + + return self.scene["q"] + + def get_scene(scene_id): if scene_id.endswith(".yaml"): return YamlScene(scene_id) elif scene_id == "random": return RandomScene() + elif scene_id == "manual": + return ManualScene() else: raise ValueError("Unknown scene {}.".format(scene_id))