From 15f120f2077f65bd89b55be3c8404e7798d9e922 Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Thu, 9 Sep 2021 21:19:30 +0200 Subject: [PATCH] Reload scene config --- active_grasp/policy.py | 3 +- active_grasp/simulation.py | 14 ++- cfg/{ => scenes}/test.yaml | 0 launch/active_grasp.rviz | 186 +++++++++++++++++-------------------- 4 files changed, 93 insertions(+), 110 deletions(-) rename cfg/{ => scenes}/test.yaml (100%) diff --git a/active_grasp/policy.py b/active_grasp/policy.py index 4ea9323..aca0e99 100644 --- a/active_grasp/policy.py +++ b/active_grasp/policy.py @@ -78,8 +78,7 @@ class Policy: return grasps[indices], scores[indices] def score_fn(self, grasp): - # return grasp.quality - return grasp.pose.translation[2] + return grasp.quality def update(self, img, pose): raise NotImplementedError diff --git a/active_grasp/simulation.py b/active_grasp/simulation.py index 09854d1..793fef9 100644 --- a/active_grasp/simulation.py +++ b/active_grasp/simulation.py @@ -40,7 +40,7 @@ class Simulation: self.arm = BtPandaArm(panda_urdf) self.gripper = BtPandaGripper(self.arm) self.model = Model(panda_urdf, self.arm.base_frame, self.arm.ee_frame) - self.camera = BtCamera(320, 240, 1.047, 0.1, 1.0, self.arm.uid, 11) + self.camera = BtCamera(320, 240, 0.96, 0.2, 1.0, self.arm.uid, 11) def seed(self, seed): self.rng = np.random.default_rng(seed) @@ -123,7 +123,6 @@ def find_urdfs(root): class RandomScene(Scene): def __init__(self): super().__init__() - # self.center = np.r_[0.5, 0.0, 0.2] self.center = np.r_[0.6, 0.0, 0.1] self.length = 0.3 self.origin = self.center - np.r_[0.5 * self.length, 0.5 * self.length, 0.0] @@ -161,19 +160,24 @@ class RandomScene(Scene): class CustomScene(Scene): def __init__(self, config_name): super().__init__() - config_path = Path(rospack.get_path("active_grasp")) / "cfg" / config_name - with config_path.open("r") as f: + self.config_path = ( + Path(rospack.get_path("active_grasp")) / "cfg" / "scenes" / config_name + ) + + def load_config(self): + with self.config_path.open("r") as f: self.scene = yaml.load(f) 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 load(self, rng): + self.load_config() self.load_support(self.center) for object in self.scene["objects"]: self.load_object( self.get_ycb_urdf_path(object["object_id"]), - Rotation.from_euler("xyz", object["rpy"]), + Rotation.from_euler("xyz", object["rpy"], degrees=True), self.center + np.asarray(object["xyz"]), ) diff --git a/cfg/test.yaml b/cfg/scenes/test.yaml similarity index 100% rename from cfg/test.yaml rename to cfg/scenes/test.yaml diff --git a/launch/active_grasp.rviz b/launch/active_grasp.rviz index 65dea62..b86d753 100644 --- a/launch/active_grasp.rviz +++ b/launch/active_grasp.rviz @@ -6,7 +6,7 @@ Panels: Expanded: - /TF1/Frames1 Splitter Ratio: 0.5 - Tree Height: 805 + Tree Height: 338 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -25,7 +25,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: SceneCloud + SyncSource: DepthImage Preferences: PromptSaveOnExit: true Toolbars: @@ -51,6 +51,18 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /camera/depth/image_rect_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: DepthImage + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false - Alpha: 0.20000000298023224 Autocompute Intensity Bounds: false Autocompute Value Bounds: @@ -86,10 +98,7 @@ Visualization Manager: Marker Topic: visualization_marker_array Name: Markers Namespaces: - bbox: true - best_grasp: true - grasps: true - path: true + {} Queue Size: 100 Value: true - Acceleration_Scaling_Factor: 0.1 @@ -125,10 +134,6 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - camera_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false panda_hand: Alpha: 1 Show Axes: false @@ -211,7 +216,7 @@ Visualization Manager: Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: panda_arm + Planning Group: hand Query Goal State: true Query Start State: false Show Workspace: false @@ -234,10 +239,6 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - camera_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false panda_hand: Alpha: 1 Show Axes: false @@ -324,74 +325,38 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - camera_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_leftfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_rightfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: false Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.0020000000949949026 + Style: Spheres + Topic: /camera/depth/color/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: @@ -432,10 +397,6 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - camera_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false panda_hand: Alpha: 1 Show Axes: false @@ -535,14 +496,18 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false - camera_optical_frame: + camera_depth_optical_frame: + Value: true + panda_EE: + Value: true + panda_K: Value: true panda_hand: Value: false panda_leftfinger: Value: false panda_link0: - Value: false + Value: true panda_link1: Value: false panda_link2: @@ -561,8 +526,6 @@ Visualization Manager: Value: false panda_rightfinger: Value: false - task: - Value: true world: Value: false Marker Alpha: 1 @@ -582,17 +545,30 @@ Visualization Manager: panda_link6: panda_link7: panda_link8: - panda_hand: - camera_optical_frame: + camera_depth_optical_frame: + {} + panda_EE: + panda_K: {} + panda_hand: panda_leftfinger: {} panda_rightfinger: {} - task: - {} Update Interval: 0 Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /camera/color/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: ColorImage + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -621,7 +597,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.5005543231964111 + Distance: 1.8815758228302002 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -629,29 +605,33 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0.3386422395706177 - Y: -0.016611307859420776 - Z: 0.429339200258255 + X: 0.4086047410964966 + Y: 0.23318199813365936 + Z: 0.28523021936416626 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.39479339122772217 + Pitch: 0.3297969102859497 Target Frame: - Yaw: 5.172863483428955 + Yaw: 5.052877902984619 Saved: ~ Window Geometry: + ColorImage: + collapsed: false + DepthImage: + collapsed: false Displays: collapsed: false - Height: 1019 + Height: 960 Hide Left Dock: false Hide Right Dock: true MotionPlanning: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -659,7 +639,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: true - Width: 1516 - X: 1039 - Y: 114 + collapsed: false + Width: 1920 + X: 0 + Y: 27