diff --git a/bt_sim_node.py b/bt_sim_node.py index 6e88648..f65e81f 100755 --- a/bt_sim_node.py +++ b/bt_sim_node.py @@ -14,6 +14,7 @@ import tf2_ros import active_grasp.srv from robot_utils.ros.conversions import * from simulation import Simulation +from utils import * class BtSimNode: @@ -46,7 +47,7 @@ class BtSimNode: self.reset_requested = True rospy.sleep(1.0) # wait for the latest sim step to finish bbox = self.sim.reset() - res = active_grasp.srv.ResetResponse(bbox.to_msg()) + res = active_grasp.srv.ResetResponse(to_bbox_msg(bbox)) self.step_cnt = 0 self.reset_requested = False return res diff --git a/controller.py b/controller.py index deeed8f..d1d7a19 100644 --- a/controller.py +++ b/controller.py @@ -18,16 +18,16 @@ class GraspController: self.T_G_EE = Transform.from_list(rospy.get_param("~ee_grasp_offset")).inv() def run(self): - self.reset() - grasp = self.explore() + bbox = self.reset() + grasp = self.explore(bbox) if grasp: self.execute_grasp(grasp) def reset(self): raise NotImplementedError - def explore(self): - self.policy.activate() + def explore(self, bbox): + self.policy.activate(bbox) r = rospy.Rate(self.policy.rate) while True: cmd = self.policy.update() diff --git a/launch/active_grasp.rviz b/launch/active_grasp.rviz index 4776f8c..55638e3 100644 --- a/launch/active_grasp.rviz +++ b/launch/active_grasp.rviz @@ -6,7 +6,7 @@ Panels: Expanded: - /TF1/Tree1 Splitter Ratio: 0.5 - Tree Height: 547 + Tree Height: 595 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -173,23 +173,23 @@ Visualization Manager: Show Axes: true Show Names: true Tree: + panda_link0: + panda_link1: + panda_link2: + panda_link3: + panda_link4: + panda_link5: + panda_link6: + panda_link7: + panda_link8: + panda_hand: + cam_optical_frame: + {} + panda_leftfinger: + {} + panda_rightfinger: + {} world: - panda_link0: - panda_link1: - panda_link2: - panda_link3: - panda_link4: - panda_link5: - panda_link6: - panda_link7: - panda_link8: - panda_hand: - cam_optical_frame: - {} - panda_leftfinger: - {} - panda_rightfinger: - {} task: {} Update Interval: 0 @@ -247,7 +247,7 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Spheres - Topic: /points + Topic: /cloud Unreliable: false Use Fixed Frame: true Use rainbow: true @@ -268,6 +268,14 @@ Visualization Manager: Topic: /command Unreliable: false Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /bbox + Name: BBox + Namespaces: + {} + Queue Size: 100 + Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /path @@ -279,7 +287,7 @@ Visualization Manager: - Class: rviz/MarkerArray Enabled: true Marker Topic: /grasps - Name: PredictedGrasps + Name: Grasps Namespaces: {} Queue Size: 100 @@ -312,7 +320,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.5376757383346558 + Distance: 1.1227775812149048 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -320,25 +328,25 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0.192186176776886 - Y: 0.14037109911441803 - Z: 0.3879348933696747 + X: 0.2026568502187729 + Y: 0.13089965283870697 + Z: 0.3290979564189911 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.21479728817939758 + Pitch: 0.3197976350784302 Target Frame: - Yaw: 5.193514823913574 + Yaw: 5.398510456085205 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 844 + Height: 892 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000002aefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ae000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000397000002ae00000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000156000002defc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002de000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fb0000003efc0100000002fb0000000800540069006d00650100000000000004fb000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000039f000002de00000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -347,6 +355,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1267 - X: 100 - Y: 457 + Width: 1275 + X: 61 + Y: 200 diff --git a/policies.py b/policies.py index 3044538..43eb1e2 100644 --- a/policies.py +++ b/policies.py @@ -2,6 +2,7 @@ import cv_bridge import numpy as np from pathlib import Path import rospy +from rospy import Publisher import sensor_msgs.msg from visualization_msgs.msg import Marker, MarkerArray @@ -12,7 +13,7 @@ from robot_utils.ros.rviz import * from robot_utils.spatial import Transform from vgn.detection import VGN, compute_grasps from vgn.perception import UniformTSDFVolume -import vgn.vis +from vgn.utils import * def get_policy(name): @@ -25,8 +26,8 @@ def get_policy(name): class BasePolicy: def __init__(self): self.cv_bridge = cv_bridge.CvBridge() - self.tsdf = UniformTSDFVolume(0.3, 40) self.vgn = VGN(Path(rospy.get_param("vgn/model"))) + self.finger_depth = 0.05 self.load_parameters() self.lookup_transforms() @@ -64,9 +65,16 @@ class BasePolicy: ) def connect_to_rviz(self): - self.path_pub = rospy.Publisher("path", MarkerArray, queue_size=1, latch=True) + self.bbox_pub = Publisher("bbox", Marker, queue_size=1, latch=True) + self.cloud_pub = Publisher("cloud", PointCloud2, queue_size=1, latch=True) + self.path_pub = Publisher("path", MarkerArray, queue_size=1, latch=True) + self.grasps_pub = Publisher("grasps", MarkerArray, queue_size=1, latch=True) - def activate(self): + def activate(self, bbox): + self.clear_grasps() + self.bbox = bbox + self.draw_bbox() + self.tsdf = UniformTSDFVolume(0.3, 40) self.viewpoints = [] self.done = False self.best_grasp = None # grasp pose defined w.r.t. the robot's base frame @@ -86,13 +94,38 @@ class BasePolicy: tsdf_grid = self.tsdf.get_grid() out = self.vgn.predict(tsdf_grid) score_fn = lambda g: g.pose.translation[2] - grasps = compute_grasps(self.tsdf.voxel_size, out, score_fn) - vgn.vis.draw_grasps(grasps, 0.05) + grasps = compute_grasps(self.tsdf.voxel_size, out, score_fn, max_filter_size=3) + grasps = self.filter_grasps_on_target_object(grasps) + self.draw_grasps(grasps) return self.T_B_task * grasps[0].pose if len(grasps) > 0 else None + def filter_grasps_on_target_object(self, grasps): + return [ + g + for g in grasps + if self.bbox.is_inside( + g.pose.rotation.apply([0, 0, 0.05]) + g.pose.translation + ) + ] + + def clear_grasps(self): + self.grasps_pub.publish(DELETE_MARKER_ARRAY_MSG) + + def draw_bbox(self): + pose = Transform.translation((self.bbox.min + self.bbox.max) / 2.0) + scale = self.bbox.max - self.bbox.min + color = np.r_[0.8, 0.2, 0.2, 0.6] + msg = create_marker(Marker.CUBE, self.task_frame, pose, scale, color) + self.bbox_pub.publish(msg) + def draw_scene_cloud(self): cloud = self.tsdf.get_scene_cloud() - vgn.vis.draw_points(np.asarray(cloud.points)) + msg = to_cloud_msg(self.task_frame, np.asarray(cloud.points)) + self.cloud_pub.publish(msg) + + def draw_grasps(self, grasps): + msg = create_grasp_marker_array(self.task_frame, grasps, self.finger_depth) + self.grasps_pub.publish(msg) def draw_camera_path(self): identity = Transform.identity() @@ -129,6 +162,5 @@ class SingleViewBaseline(BasePolicy): def update(self): self.integrate_latest_image() self.draw_scene_cloud() - self.draw_camera_path() self.best_grasp = self.predict_best_grasp() self.done = True diff --git a/sim_grasp.py b/sim_grasp.py index 89b07ef..4dc7cb9 100644 --- a/sim_grasp.py +++ b/sim_grasp.py @@ -1,21 +1,23 @@ import argparse import rospy -import std_srvs.srv as std_srvs +from active_grasp.srv import Reset, ResetRequest from controller import GraspController from policies import get_policy +from utils import * class SimGraspController(GraspController): def __init__(self, policy): super().__init__(policy) - self.reset_sim = rospy.ServiceProxy("/reset", std_srvs.Trigger) + self.reset_sim = rospy.ServiceProxy("reset", Reset) def reset(self): - req = std_srvs.TriggerRequest() - self.reset_sim(req) + req = ResetRequest() + res = self.reset_sim(req) rospy.sleep(1.0) # wait for states to be updated + return from_bbox_msg(res.bbox) def create_parser(): diff --git a/simulation.py b/simulation.py index e3d460b..682d29d 100644 --- a/simulation.py +++ b/simulation.py @@ -5,13 +5,13 @@ import rospkg from robot_utils.bullet import * from robot_utils.controllers import CartesianPoseController from robot_utils.spatial import Rotation, Transform -from utils import AABBox +from utils import * class Simulation(BtSim): def __init__(self, gui=True): super().__init__(gui=gui, sleep=False) - p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) + # p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) p.resetDebugVisualizerCamera(1.4, 50, -35, [0.0, 0.0, 0.6]) self.object_uids = [] @@ -111,6 +111,8 @@ class Simulation(BtSim): def get_target_bbox(self, uid): aabb_min, aabb_max = p.getAABB(uid) + aabb_min = np.array(aabb_min) - self.origin + aabb_max = np.array(aabb_max) - self.origin return AABBox(aabb_min, aabb_max) diff --git a/utils.py b/utils.py index 319d13e..ac6e5fd 100644 --- a/utils.py +++ b/utils.py @@ -19,17 +19,18 @@ class AABBox: self.min = bbox_min self.max = bbox_max - @classmethod - def from_msg(cls, msg): - aabb_min = from_point_msg(msg.min) - aabb_max = from_point_msg(msg.max) - return cls(aabb_min, aabb_max) - - def to_msg(self): - msg = active_grasp.msg.AABBox() - msg.min = to_point_msg(self.min) - msg.max = to_point_msg(self.max) - return msg - def is_inside(self, p): return np.all(p > self.min) and np.all(p < self.max) + + +def from_bbox_msg(msg): + aabb_min = from_point_msg(msg.min) + aabb_max = from_point_msg(msg.max) + return AABBox(aabb_min, aabb_max) + + +def to_bbox_msg(bbox): + msg = active_grasp.msg.AABBox() + msg.min = to_point_msg(bbox.min) + msg.max = to_point_msg(bbox.max) + return msg