diff --git a/cfg/active_grasp.rviz b/cfg/active_grasp.rviz index 53b2b0c..e76879b 100644 --- a/cfg/active_grasp.rviz +++ b/cfg/active_grasp.rviz @@ -1,11 +1,12 @@ Panels: - Class: rviz/Displays - Help Height: 0 + Help Height: 70 Name: Displays Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.501006007194519 - Tree Height: 419 + Expanded: + - /TF1/Frames1 + Splitter Ratio: 0.6881287693977356 + Tree Height: 217 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -18,14 +19,13 @@ Panels: - Class: rviz/Views Expanded: - /Current View1 - - /Current View1/Focal Point1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 - SyncSource: DepthImage + SyncSource: PointCloud Preferences: PromptSaveOnExit: true Toolbars: @@ -56,6 +56,42 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false + camera_depth_optical_frame: + Value: true + panda_EE: + Value: false + panda_K: + Value: false + panda_NE: + Value: false + panda_hand: + Value: false + panda_leftfinger: + Value: false + panda_link0: + Value: false + panda_link1: + Value: false + panda_link2: + Value: false + panda_link3: + Value: false + panda_link4: + Value: false + panda_link5: + Value: false + panda_link6: + Value: false + panda_link7: + Value: false + panda_link8: + Value: false + panda_rightfinger: + Value: false + task: + Value: false + world: + Value: false Marker Alpha: 1 Marker Scale: 0.30000001192092896 Name: TF @@ -63,7 +99,27 @@ Visualization Manager: Show Axes: true Show Names: false Tree: - {} + world: + panda_link0: + panda_link1: + panda_link2: + panda_link3: + panda_link4: + panda_link5: + panda_link6: + panda_link7: + camera_depth_optical_frame: + {} + panda_link8: + panda_NE: + panda_EE: + panda_K: + {} + panda_hand: + panda_leftfinger: + {} + panda_rightfinger: + {} Update Interval: 0 Value: true - Alpha: 1 @@ -76,6 +132,70 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + camera_depth_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 Name: RobotModel Robot Description: robot_description TF Prefix: "" @@ -84,7 +204,7 @@ Visualization Manager: Visual Enabled: true - Acceleration_Scaling_Factor: 0.1 Class: moveit_rviz_plugin/MotionPlanning - Enabled: true + Enabled: false Move Group Namespace: "" MoveIt_Allow_Approximate_IK: false MoveIt_Allow_External_Program: false @@ -137,7 +257,7 @@ Visualization Manager: Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: hand + Planning Group: "" Query Goal State: true Query Start State: false Show Workspace: false @@ -163,7 +283,7 @@ Visualization Manager: Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: true - Value: true + Value: false Velocity_Scaling_Factor: 0.1 - Class: moveit_rviz_plugin/PlanningScene Enabled: false @@ -190,18 +310,6 @@ Visualization Manager: Show Robot Collision: false Show Robot Visual: false Value: false - - Class: rviz/Image - Enabled: false - 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: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -214,7 +322,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: RGB8 Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 @@ -229,9 +337,21 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: false + 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 + - Class: rviz/Image + Enabled: false Image Topic: /camera/depth/image_rect_raw Max Value: 1 Median window: 5 @@ -241,13 +361,25 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: true + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /camera/infra1/image_rect_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: InfraImage + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: visualization_marker_array Name: Markers Namespaces: - {} + bbox: true Queue Size: 100 Value: true - Alpha: 1 @@ -366,26 +498,46 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.5308048725128174 + Distance: 1.2000000476837158 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 + Field of View: 0.7799999713897705 Focal Point: - X: 0.34312182664871216 - Y: -0.10157600790262222 - Z: 0.30506056547164917 + X: 0.44999998807907104 + Y: 0 + Z: 0.4000000059604645 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.4247966408729553 + Pitch: 0.25999999046325684 Target Frame: - Yaw: 1.9687141180038452 - Saved: ~ + Yaw: 1.0399999618530273 + Saved: + - Class: rviz/Orbit + Distance: 1.2000000476837158 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7799999713897705 + Focal Point: + X: 0.44999998807907104 + Y: 0 + Z: 0.4000000059604645 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Panda + Near Clip Distance: 0.009999999776482582 + Pitch: 0.25999999046325684 + Target Frame: + Yaw: 1.0399999618530273 Window Geometry: ColorImage: collapsed: false @@ -393,14 +545,16 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 837 + Height: 960 Hide Left Dock: false Hide Right Dock: true + InfraImage: + collapsed: false MotionPlanning: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -408,7 +562,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: false - Width: 1098 - X: 821 + collapsed: true + Width: 1920 + X: 0 Y: 27 diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml index 18f82cc..aeb99ca 100644 --- a/cfg/active_grasp.yaml +++ b/cfg/active_grasp.yaml @@ -4,6 +4,10 @@ bt_sim: gripper_force: 10 scene: random +hw: + roi_calib_file: $(find active_grasp)/cfg/hw/T_base_tag.txt + scene_file: $(find active_grasp)/cfg/hw/scene01.yaml + grasp_controller: base_frame_id: panda_link0 ee_grasp_offset: [0.0, 0.0, -0.383, 0.924, 0.0, 0.0, 0.065] # offset to panda_link8 diff --git a/cfg/hw/T_base_tag.txt b/cfg/hw/T_base_tag.txt new file mode 100644 index 0000000..e5055df --- /dev/null +++ b/cfg/hw/T_base_tag.txt @@ -0,0 +1,4 @@ +9.998979853190785860e-01 1.338047579286792167e-02 4.998181909874715480e-03 3.216251487106734364e-01 +-1.341414276126873424e-02 9.998871470166096342e-01 6.764170678302552919e-03 -1.756941528330971392e-01 +-4.907110028114804005e-03 -6.830526959274958038e-03 9.999646314608484632e-01 -3.868988796629513338e-02 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/cfg/hw/apriltag.yaml b/cfg/hw/apriltag.yaml new file mode 100644 index 0000000..4a5259f --- /dev/null +++ b/cfg/hw/apriltag.yaml @@ -0,0 +1,12 @@ + # AprilTag 3 code parameters +tag_family: 'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12 +tag_threads: 2 # default: 2 +tag_decimate: 1.0 # default: 1.0 +tag_blur: 0.0 # default: 0.0 +tag_refine_edges: 1 # default: 1 +tag_debug: 0 # default: 0 +publish_tf: true # default: false + +# Definitions of tags to detect +standalone_tags: [{id: 0, size: 0.091}] +tag_bundles: [] diff --git a/cfg/hw/scene01.yaml b/cfg/hw/scene01.yaml new file mode 100644 index 0000000..a0d12b5 --- /dev/null +++ b/cfg/hw/scene01.yaml @@ -0,0 +1,4 @@ +target: + min: [0.1, 0.1, 0.0] + max: [0.2, 0.2, 0.1] +q0: [0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79] diff --git a/cfg/mustard.yaml b/cfg/sim/mustard.yaml similarity index 100% rename from cfg/mustard.yaml rename to cfg/sim/mustard.yaml diff --git a/launch/apriltag.launch b/launch/apriltag.launch new file mode 100644 index 0000000..93a3d6f --- /dev/null +++ b/launch/apriltag.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/launch/env.launch b/launch/env.launch index c40f708..1413d43 100644 --- a/launch/env.launch +++ b/launch/env.launch @@ -1,6 +1,6 @@ - + @@ -11,14 +11,14 @@ - + - + diff --git a/launch/panda.launch b/launch/hw.launch similarity index 97% rename from launch/panda.launch rename to launch/hw.launch index b8765ac..213a100 100644 --- a/launch/panda.launch +++ b/launch/hw.launch @@ -32,8 +32,10 @@ + + diff --git a/scripts/bt_sim_node.py b/scripts/bt_sim_node.py index f6947a8..e87cb66 100755 --- a/scripts/bt_sim_node.py +++ b/scripts/bt_sim_node.py @@ -317,7 +317,7 @@ class CameraPlugin(Plugin): if self.cam_noise: depth = apply_noise(depth) - msg = self.cv_bridge.cv2_to_imgmsg(depth) + msg = self.cv_bridge.cv2_to_imgmsg((1000 * depth).astype(np.uint16)) msg.header.stamp = stamp self.depth_pub.publish(msg) diff --git a/scripts/calibrate_roi.py b/scripts/calibrate_roi.py new file mode 100755 index 0000000..2f404f9 --- /dev/null +++ b/scripts/calibrate_roi.py @@ -0,0 +1,17 @@ +#!/usr/bin/env python3 + +import numpy as np +import rospy + +from robot_helpers.ros import tf + + +def main(): + rospy.init_node("calibrate_roi") + tf.init() + T_base_roi = tf.lookup("panda_link0", "tag_0") + np.savetxt("cfg/hw/T_base_tag.txt", T_base_roi.as_matrix()) + + +if __name__ == "__main__": + main() diff --git a/scripts/hw_node.py b/scripts/hw_node.py index bdc118f..b9f3d77 100755 --- a/scripts/hw_node.py +++ b/scripts/hw_node.py @@ -1,24 +1,40 @@ #!/usr/bin/env python3 from controller_manager_msgs.srv import * +import numpy as np import rospy from active_grasp.bbox import AABBox, to_bbox_msg +from active_grasp.rviz import Visualizer from active_grasp.srv import * - - +from robot_helpers.io import load_yaml from robot_helpers.ros.moveit import MoveItClient +from robot_helpers.ros.panda import PandaGripperClient +from robot_helpers.spatial import Transform class HwNode: def __init__(self): + self.load_parameters() + self.init_robot_connection() + self.init_visualizer() self.advertise_services() - self.switch_controller = rospy.ServiceProxy( + rospy.spin() + + def load_parameters(self): + self.cfg = rospy.get_param("hw") + self.T_base_roi = Transform.from_matrix(np.loadtxt(self.cfg["roi_calib_file"])) + + def init_robot_connection(self): + self.gripper = PandaGripperClient() + self.switch_to_joint_trajectory_controller = rospy.ServiceProxy( "controller_manager/switch_controller", SwitchController ) - self.moveit = MoveItClient("panda_arm") - rospy.spin() + + def init_visualizer(self): + self.vis = Visualizer() + rospy.Timer(rospy.Duration(1), self.draw_bbox) def advertise_services(self): rospy.Service("seed", Seed, self.seed) @@ -29,17 +45,33 @@ class HwNode: return SeedResponse() def reset(self, req): + q0, bbox = self.load_config() + + # Move to the initial configuration + self.switch_to_joint_trajectory_controller() + self.moveit.goto(q0) + self.gripper.move(0.04) + + return ResetResponse(to_bbox_msg(bbox)) + + def load_config(self): + scene_config = load_yaml(self.cfg["scene_file"]) + q0 = scene_config["q0"] + bbox_min = self.T_base_roi.apply(scene_config["target"]["min"]) + bbox_max = self.T_base_roi.apply(scene_config["target"]["max"]) + bbox = AABBox(bbox_min, bbox_max) + return q0, bbox + + def switch_to_joint_trajectory_controller(self): req = SwitchControllerRequest() req.start_controllers = ["position_joint_trajectory_controller"] req.stop_controllers = ["cartesian_velocity_controller"] + req.strictness = 1 self.switch_controller(req) - # Move to the initial configuration - self.moveit.goto([0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79]) - - # Detect target - bbox = AABBox([0.4, -0.1, 0.0], [0.5, 0.1, 0.1]) - return ResetResponse(to_bbox_msg(bbox)) + def draw_bbox(self, event): + _, bbox = self.load_config() + self.vis.bbox("panda_link0", bbox) def main(): diff --git a/scripts/run.py b/scripts/run.py old mode 100644 new mode 100755 index 6a2dbc6..c854ca8 --- a/scripts/run.py +++ b/scripts/run.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import argparse from datetime import datetime import pandas as pd @@ -33,7 +35,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=100) + parser.add_argument("--runs", type=int, default=1) parser.add_argument("--logdir", type=Path, default="logs") parser.add_argument("--seed", type=int, default=1) return parser diff --git a/src/active_grasp/controller.py b/src/active_grasp/controller.py index 7185881..9f8850c 100644 --- a/src/active_grasp/controller.py +++ b/src/active_grasp/controller.py @@ -59,12 +59,14 @@ class GraspController: req = SwitchControllerRequest() req.start_controllers = ["cartesian_velocity_controller"] req.stop_controllers = ["position_joint_trajectory_controller"] + req.strictness = 1 self.switch_controller(req) def switch_to_joint_trajectory_control(self): req = SwitchControllerRequest() req.start_controllers = ["position_joint_trajectory_controller"] req.stop_controllers = ["cartesian_velocity_controller"] + req.strictness = 1 self.switch_controller(req) def init_camera_stream(self): @@ -85,6 +87,7 @@ class GraspController: res = self.execute_grasp(grasp) else: res = "aborted" + self.gripper.move(0.04) return self.collect_info(res) def reset(self): @@ -110,7 +113,7 @@ class GraspController: def get_state(self): q, _ = self.arm.get_state() msg = copy.deepcopy(self.latest_depth_msg) - img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32) + img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32) * 0.001 pose = tf.lookup(self.base_frame, self.cam_frame, msg.header.stamp) return img, pose, q diff --git a/src/active_grasp/simulation.py b/src/active_grasp/simulation.py index 44b5d00..fad78dc 100644 --- a/src/active_grasp/simulation.py +++ b/src/active_grasp/simulation.py @@ -158,7 +158,7 @@ class Scene: class YamlScene(Scene): def __init__(self, config_name): super().__init__() - self.config_path = pkg_root / "cfg" / config_name + self.config_path = pkg_root / "cfg/sim" / config_name def load_config(self): self.scene = load_cfg(self.config_path)