hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add test scenes

Browse Source
This commit is contained in:
Michel Breyer
2021-09-10 23:29:15 +02:00
parent 15f120f207
commit 6514872b74
12 changed files with 87 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
active_grasp/__init__.py
View File

@@ -5,5 +5,4 @@ from .nbv import NextBestView
register("initial-view", InitialView)
register("top-view", TopView)
register("top-trajectory", TopTrajectory)
register("circular-trajectory", CircularTrajectory)
register("nbv", NextBestView)

33
active_grasp/baselines.py
View File

@@ -1,6 +1,4 @@
import numpy as np
import rospy
import scipy.interpolate
from .policy import SingleViewPolicy, MultiViewPolicy
from vgn.utils import look_at
@@ -16,7 +14,7 @@ class InitialView(SingleViewPolicy):
class TopView(SingleViewPolicy):
def activate(self, bbox):
super().activate(bbox)
eye = np.r_[self.center[:2], self.center[2] + 0.3]
eye = np.r_[self.center[:2], self.bbox.max[2] + self.min_z_dist]
up = np.r_[1.0, 0.0, 0.0]
self.x_d = look_at(eye, self.center, up)
@@ -24,7 +22,7 @@ class TopView(SingleViewPolicy):
class TopTrajectory(MultiViewPolicy):
def activate(self, bbox):
super().activate(bbox)
eye = np.r_[self.center[:2], self.center[2] + 0.3]
eye = np.r_[self.center[:2], self.bbox.max[2] + self.min_z_dist]
up = np.r_[1.0, 0.0, 0.0]
self.x_d = look_at(eye, self.center, up)
@@ -36,30 +34,3 @@ class TopTrajectory(MultiViewPolicy):
return np.zeros(6)
else:
return self.compute_velocity_cmd(linear, angular)
class CircularTrajectory(MultiViewPolicy):
def __init__(self, rate):
super().__init__(rate)
self.r = 0.08
self.h = 0.3
self.duration = 2.0 * np.pi * self.r / self.linear_vel
self.m = scipy.interpolate.interp1d([0.0, self.duration], [np.pi, 3.0 * np.pi])
def activate(self, bbox):
super().activate(bbox)
self.tic = rospy.Time.now()
def update(self, img, x):
self.integrate(img, x)
elapsed_time = (rospy.Time.now() - self.tic).to_sec()
if elapsed_time > self.duration:
self.done = True
return np.zeros(6)
else:
t = self.m(elapsed_time)
eye = self.center + np.r_[self.r * np.cos(t), self.r * np.sin(t), self.h]
up = np.r_[1.0, 0.0, 0.0]
x_d = look_at(eye, self.center, up)
linear, angular = self.compute_error(x_d, x)
return self.compute_velocity_cmd(linear, angular)

15
active_grasp/policy.py
View File

@@ -33,9 +33,10 @@ class Policy:
def activate(self, bbox):
self.bbox = bbox
self.calibrate_task_frame()
self.vis.clear()
self.calibrate_task_frame()
self.vis.bbox(self.base_frame, bbox)
self.vis.workspace(self.task_frame, 0.3)
self.tsdf = UniformTSDFVolume(0.3, 40)
self.vgn = VGN(Path(rospy.get_param("vgn/model")))
self.views = []
@@ -47,7 +48,7 @@ class Policy:
self.T_base_task = Transform.translation(self.center - np.full(3, 0.15))
self.T_task_base = self.T_base_task.inv()
tf.broadcast(self.T_base_task, self.base_frame, self.task_frame)
rospy.sleep(0.1)
rospy.sleep(0.5)
def compute_error(self, x_d, x):
linear = x_d.translation - x.translation
@@ -67,6 +68,13 @@ class Policy:
for grasp in in_grasps:
pose = self.T_base_task * grasp.pose
R, t = pose.rotation, pose.translation
# Filter out artifacts close to the support
if t[2] < self.bbox.min[2] + 0.04:
continue
tip = pose.rotation.apply([0, 0, 0.05]) + pose.translation
if self.bbox.is_inside(tip):
grasp.pose = pose
@@ -97,7 +105,7 @@ class SingleViewPolicy(Policy):
self.vis.map_cloud(self.task_frame, self.tsdf.get_map_cloud())
out = self.vgn.predict(tsdf_grid)
self.vis.quality(self.task_frame, voxel_size, out.qual)
self.vis.quality(self.task_frame, voxel_size, out.qual, 0.5)
grasps = select_grid(voxel_size, out, threshold=self.qual_threshold)
grasps, scores = self.sort_grasps(grasps)
@@ -122,6 +130,7 @@ class MultiViewPolicy(Policy):
tsdf_grid, voxel_size = self.tsdf.get_grid(), self.tsdf.voxel_size
out = self.vgn.predict(tsdf_grid)
self.vis.quality(self.task_frame, self.tsdf.voxel_size, out.qual, 0.5)
grasps = select_grid(voxel_size, out, threshold=self.qual_threshold)
grasps, scores = self.sort_grasps(grasps)

9
active_grasp/simulation.py
View File

@@ -40,15 +40,15 @@ 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, 0.96, 0.2, 1.0, self.arm.uid, 11)
self.camera = BtCamera(320, 240, 0.96, 0.01, 1.0, self.arm.uid, 11)
def seed(self, seed):
self.rng = np.random.default_rng(seed)
def reset(self):
self.scene.reset(rng=self.rng)
q = self.scene.sample_initial_configuration(self.rng)
self.set_arm_configuration(q)
self.scene.reset(rng=self.rng)
uid = self.select_target()
bbox = self.get_target_bbox(uid)
return bbox
@@ -179,10 +179,13 @@ class CustomScene(Scene):
self.get_ycb_urdf_path(object["object_id"]),
Rotation.from_euler("xyz", object["rpy"], degrees=True),
self.center + np.asarray(object["xyz"]),
object.get("scale", 1),
)
for _ in range(60):
p.stepSimulation()
def sample_initial_configuration(self, rng):
return self.scene["configuration"]
return self.scene["q"]
def get_scene(scene_id):

26
active_grasp/visualization.py
View File

@@ -137,8 +137,8 @@ class Visualizer:
)
self.draw([marker])
def quality(self, frame, voxel_size, quality):
points, values = grid_to_map_cloud(voxel_size, quality, threshold=0.9)
def quality(self, frame, voxel_size, quality, threshold=0.9):
points, values = grid_to_map_cloud(voxel_size, quality, threshold)
values = (values - 0.9) / (1.0 - 0.9) # to increase contrast
msg = to_cloud_msg(frame, points, intensities=values)
self.quality_pub.publish(msg)
@@ -169,6 +169,28 @@ class Visualizer:
markers.append(marker)
self.draw(markers)
def workspace(self, frame, size):
scale = size * 0.005
pose = Transform.identity()
scale = [scale, 0.0, 0.0]
color = [0.5, 0.5, 0.5]
lines = [
([0.0, 0.0, 0.0], [size, 0.0, 0.0]),
([size, 0.0, 0.0], [size, size, 0.0]),
([size, size, 0.0], [0.0, size, 0.0]),
([0.0, size, 0.0], [0.0, 0.0, 0.0]),
([0.0, 0.0, size], [size, 0.0, size]),
([size, 0.0, size], [size, size, size]),
([size, size, size], [0.0, size, size]),
([0.0, size, size], [0.0, 0.0, size]),
([0.0, 0.0, 0.0], [0.0, 0.0, size]),
([size, 0.0, 0.0], [size, 0.0, size]),
([size, size, 0.0], [size, size, size]),
([0.0, size, 0.0], [0.0, size, size]),
]
msg = create_line_list_marker(frame, pose, scale, color, lines, ns="workspace")
self.draw([msg])
def create_cam_view_marker(
frame, pose, scale, color, intrinsic, near, far, ns="", id=0