Reject grasps outside the target bbox
This commit is contained in:
Michel Breyer
parent
37bef05d70
commit
f29d861fd4
@ -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
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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,7 +173,6 @@ Visualization Manager:
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
world:
|
||||
panda_link0:
|
||||
panda_link1:
|
||||
panda_link2:
|
||||
@ -190,6 +189,7 @@ Visualization Manager:
|
||||
{}
|
||||
panda_rightfinger:
|
||||
{}
|
||||
world:
|
||||
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: <Fixed 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
|
||||
|
||||
48
policies.py
48
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
|
||||
|
||||
10
sim_grasp.py
10
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():
|
||||
|
||||
@ -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)
|
||||
|
||||
|
||||
|
||||
25
utils.py
25
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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user