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

Minor

Browse Source
This commit is contained in:
Michel Breyer
2021-10-08 13:53:07 +02:00
parent c1710eb2da
commit 6ec5272394
6 changed files with 413 additions and 533 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
src/active_grasp/policy.py
View File

@@ -4,7 +4,7 @@ from pathlib import Path
import rospy
from .timer import Timer
from .visualization import Visualizer
from .rviz import Visualizer
from robot_helpers.model import KDLModel
from robot_helpers.ros import tf
from robot_helpers.ros.conversions import *
@@ -62,36 +62,33 @@ class Policy:
self.T_task_base = self.T_base_task.inv()
tf.broadcast(self.T_base_task, self.base_frame, self.task_frame)
rospy.sleep(1.0) # Wait for tf tree to be updated
self.vis.workspace(self.task_frame, 0.3)
def update(self, img, x, q):
raise NotImplementedError
def sort_grasps(self, in_grasps, q):
# Transforms grasps into base frame, checks whether they lie on the target, and sorts by their score
grasps, scores = [], []
for grasp in in_grasps:
def sort_grasps(self, grasps, qualities, q):
"""
1. Transform grasp configurations into base_frame
2. Check whether the finger tips lie within the bounding box
3. Remove grasps for which no IK solution was found
4. Sort grasps according to score_fn
"""
filtered_grasps, scores = [], []
for grasp, quality in zip(grasps, qualities):
pose = self.T_base_task * grasp.pose
R, t = pose.rotation, pose.translation
tip = pose.rotation.apply([0, 0, 0.05]) + pose.translation
# Filter out artifacts close to the support
if t[2] < self.bbox.min[2] + 0.05 or tip[2] < self.bbox.min[2] + 0.02:
continue
if self.bbox.is_inside(tip):
grasp.pose = pose
q_grasp = self.ee_model.ik(q, pose * self.T_grasp_ee)
if q_grasp is not None:
grasps.append(grasp)
scores.append(self.score_fn(grasp, q, q_grasp))
filtered_grasps.append(grasp)
scores.append(self.score_fn(grasp, quality, q, q_grasp))
filtered_grasps, scores = np.asarray(filtered_grasps), np.asarray(scores)
i = np.argsort(-scores)
return filtered_grasps[i], qualities[i], scores[i]
grasps, scores = np.asarray(grasps), np.asarray(scores)
indices = np.argsort(-scores)
return grasps[indices], scores[indices]
def score_fn(self, grasp, q, q_grasp):
def score_fn(self, grasp, quality, q, q_grasp):
return -np.linalg.norm(q - q_grasp)
@@ -108,8 +105,8 @@ class SingleViewPolicy(Policy):
out = self.vgn.predict(tsdf_grid)
self.vis.quality(self.task_frame, voxel_size, out.qual, 0.5)
grasps = select_grid(voxel_size, out, threshold=self.qual_threshold)
grasps, _ = self.sort_grasps(grasps, q)
grasps, qualities = select_grid(voxel_size, out, self.qual_threshold)
grasps, _ = self.sort_grasps(grasps, qualities, q)
if len(grasps) > 0:
self.best_grasp = grasps[0]
@@ -143,17 +140,16 @@ class MultiViewPolicy(Policy):
self.qual_hist[t, ...] = out.qual
with Timer("grasp_selection"):
grasps = select_grid(voxel_size, out, threshold=self.qual_threshold)
grasps, _ = self.sort_grasps(grasps, q)
self.vis.clear_grasps()
grasps, qualities = select_grid(voxel_size, out, self.qual_threshold)
grasps, qualities, _ = self.sort_grasps(grasps, qualities, q)
if len(grasps) > 0:
self.best_grasp = grasps[0]
self.vis.grasps(self.base_frame, grasps)
self.vis.best_grasp(self.base_frame, self.best_grasp)
# self.vis.grasps(self.base_frame, grasps, qualities)
self.vis.grasp(self.base_frame, self.best_grasp, qualities[0])
else:
self.best_grasp = None
self.vis.clear_grasp()
def compute_error(x_d, x):

143
src/active_grasp/rviz.py Normal file
View File

@@ -0,0 +1,143 @@
import numpy as np
from robot_helpers.ros.rviz import *
from robot_helpers.spatial import Transform
import vgn.rviz
cm = lambda s: tuple([float(1 - s), float(s), float(0)])
red = np.r_[1.0, 0.0, 0.0]
blue = np.r_[0, 0.6, 1.0]
class Visualizer(vgn.rviz.Visualizer):
def bbox(self, frame, bbox):
pose = Transform.identity()
scale = [0.004, 0.0, 0.0]
color = red
corners = bbox.corners
edges = [
(0, 1),
(1, 3),
(3, 2),
(2, 0),
(4, 5),
(5, 7),
(7, 6),
(6, 4),
(0, 4),
(1, 5),
(3, 7),
(2, 6),
]
lines = [(corners[s], corners[e]) for s, e in edges]
marker = create_line_list_marker(frame, pose, scale, color, lines, "bbox")
self.draw([marker])
def rays(self, frame, origin, directions, t_max=1.0):
lines = [[origin, origin + t_max * direction] for direction in directions]
marker = create_line_list_marker(
frame,
Transform.identity(),
[0.001, 0.0, 0.0],
[0.9, 0.9, 0.9],
lines,
"rays",
)
self.draw([marker])
def path(self, frame, poses):
color = blue
points = [p.translation for p in poses]
spheres = create_sphere_list_marker(
frame,
Transform.identity(),
np.full(3, 0.01),
color,
points,
"path",
0,
)
markers = [spheres]
if len(poses) > 1:
lines = create_line_strip_marker(
frame,
Transform.identity(),
[0.005, 0.0, 0.0],
color,
points,
"path",
1,
)
markers.append(lines)
self.draw(markers)
def point(self, frame, point):
marker = create_sphere_marker(
frame,
Transform.translation(point),
np.full(3, 0.01),
[0, 0, 1],
"point",
)
self.draw([marker])
def views(self, frame, intrinsic, views, values):
vmin, vmax = min(values), max(values)
scale = [0.002, 0.0, 0.0]
near, far = 0.0, 0.02
markers = []
for i, (view, value) in enumerate(zip(views, values)):
color = cm((value - vmin) / (vmax - vmin))
marker = create_cam_view_marker(
frame,
view,
scale,
color,
intrinsic,
near,
far,
ns="views",
id=i,
)
markers.append(marker)
self.draw(markers)
def create_cam_view_marker(
frame, pose, scale, color, intrinsic, near, far, ns="", id=0
):
marker = create_marker(Marker.LINE_LIST, frame, pose, scale, color, ns, id)
x_n = near * intrinsic.width / (2.0 * intrinsic.fx)
y_n = near * intrinsic.height / (2.0 * intrinsic.fy)
z_n = near
x_f = far * intrinsic.width / (2.0 * intrinsic.fx)
y_f = far * intrinsic.height / (2.0 * intrinsic.fy)
z_f = far
points = [
[x_n, y_n, z_n],
[-x_n, y_n, z_n],
[-x_n, y_n, z_n],
[-x_n, -y_n, z_n],
[-x_n, -y_n, z_n],
[x_n, -y_n, z_n],
[x_n, -y_n, z_n],
[x_n, y_n, z_n],
[x_f, y_f, z_f],
[-x_f, y_f, z_f],
[-x_f, y_f, z_f],
[-x_f, -y_f, z_f],
[-x_f, -y_f, z_f],
[x_f, -y_f, z_f],
[x_f, -y_f, z_f],
[x_f, y_f, z_f],
[x_n, y_n, z_n],
[x_f, y_f, z_f],
[-x_n, y_n, z_n],
[-x_f, y_f, z_f],
[-x_n, -y_n, z_n],
[-x_f, -y_f, z_f],
[x_n, -y_n, z_n],
[x_f, -y_f, z_f],
]
marker.points = [to_point_msg(p) for p in points]
return marker

244
src/active_grasp/visualization.py
View File

@@ -1,244 +0,0 @@
from geometry_msgs.msg import PoseStamped
import matplotlib.colors
import numpy as np
import rospy
from robot_helpers.ros.rviz import *
from robot_helpers.spatial import Transform
from vgn.utils import *
cmap = matplotlib.colors.LinearSegmentedColormap.from_list("RedGreen", ["r", "g"])
red = np.r_[1.0, 0.0, 0.0]
blue = np.r_[0, 0.6, 1.0]
class Visualizer:
def __init__(self, topic="visualization_marker_array"):
self.marker_pub = rospy.Publisher(topic, MarkerArray, queue_size=1)
self.scene_cloud_pub = rospy.Publisher(
"scene_cloud",
PointCloud2,
latch=True,
queue_size=1,
)
self.map_cloud_pub = rospy.Publisher(
"map_cloud",
PointCloud2,
latch=True,
queue_size=1,
)
self.pose_pub = rospy.Publisher("pose", PoseStamped, queue_size=1)
self.quality_pub = rospy.Publisher("quality", PointCloud2, queue_size=1)
self.grasp_marker_count = 0
def clear(self):
self.clear_markers()
msg = to_cloud_msg("panda_link0", np.array([]))
self.scene_cloud_pub.publish(msg)
self.map_cloud_pub.publish(msg)
self.quality_pub.publish(msg)
rospy.sleep(0.1)
def clear_markers(self):
self.draw([Marker(action=Marker.DELETEALL)])
def draw(self, markers):
self.marker_pub.publish(MarkerArray(markers=markers))
def bbox(self, frame, bbox):
pose = Transform.identity()
scale = [0.004, 0.0, 0.0]
color = red
corners = bbox.corners
edges = [
(0, 1),
(1, 3),
(3, 2),
(2, 0),
(4, 5),
(5, 7),
(7, 6),
(6, 4),
(0, 4),
(1, 5),
(3, 7),
(2, 6),
]
lines = [(corners[s], corners[e]) for s, e in edges]
marker = create_line_list_marker(frame, pose, scale, color, lines, ns="bbox")
self.draw([marker])
def best_grasp(self, frame, grasp, qmin=0.5, qmax=1.0):
color = cmap((grasp.quality - qmin) / (qmax - qmin))
self.draw(create_grasp_markers(frame, grasp, color, "best_grasp", radius=0.006))
def grasps(self, frame, grasps, qmin=0.5, qmax=1.0):
markers = []
for i, grasp in enumerate(grasps):
color = cmap((grasp.quality - qmin) / (qmax - qmin))
markers += create_grasp_markers(frame, grasp, color, "grasps", 4 * i)
self.grasp_marker_count = len(markers)
self.draw(markers)
def clear_grasps(self):
if self.grasp_marker_count > 0:
markers = [
Marker(action=Marker.DELETE, ns="grasps", id=i)
for i in range(self.grasp_marker_count)
]
markers += [
Marker(action=Marker.DELETE, ns="best_grasp", id=i) for i in range(4)
]
self.draw(markers)
self.grasp_marker_count = 0
def rays(self, frame, origin, directions, t_max=1.0):
lines = [[origin, origin + t_max * direction] for direction in directions]
marker = create_line_list_marker(
frame,
Transform.identity(),
[0.001, 0.0, 0.0],
[0.9, 0.9, 0.9],
lines,
"rays",
)
self.draw([marker])
def map_cloud(self, frame, cloud):
points = np.asarray(cloud.points)
distances = np.expand_dims(np.asarray(cloud.colors)[:, 0], 1)
msg = to_cloud_msg(frame, points, distances=distances)
self.map_cloud_pub.publish(msg)
def path(self, frame, poses):
color = blue
points = [p.translation for p in poses]
spheres = create_sphere_list_marker(
frame,
Transform.identity(),
np.full(3, 0.01),
color,
points,
"path",
0,
)
markers = [spheres]
if len(poses) > 1:
lines = create_line_strip_marker(
frame,
Transform.identity(),
[0.005, 0.0, 0.0],
color,
points,
"path",
1,
)
markers.append(lines)
self.draw(markers)
def point(self, frame, point):
marker = create_sphere_marker(
frame,
Transform.translation(point),
np.full(3, 0.01),
[0, 0, 1],
"point",
)
self.draw([marker])
def pose(self, frame, pose):
msg = to_pose_stamped_msg(pose, frame)
self.pose_pub.publish(msg)
def quality(self, frame, voxel_size, quality, threshold=0.9):
points, values = grid_to_map_cloud(voxel_size, quality, threshold)
msg = to_cloud_msg(frame, points, intensities=values)
self.quality_pub.publish(msg)
def scene_cloud(self, frame, cloud):
msg = to_cloud_msg(frame, np.asarray(cloud.points))
self.scene_cloud_pub.publish(msg)
def views(self, frame, intrinsic, views, values):
vmin, vmax = min(values), max(values)
scale = [0.002, 0.0, 0.0]
near, far = 0.0, 0.02
markers = []
for i, (view, value) in enumerate(zip(views, values)):
color = cmap((value - vmin) / (vmax - vmin))
marker = create_cam_view_marker(
frame,
view,
scale,
color,
intrinsic,
near,
far,
ns="views",
id=i,
)
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
):
marker = create_marker(Marker.LINE_LIST, frame, pose, scale, color, ns, id)
x_n = near * intrinsic.width / (2.0 * intrinsic.fx)
y_n = near * intrinsic.height / (2.0 * intrinsic.fy)
z_n = near
x_f = far * intrinsic.width / (2.0 * intrinsic.fx)
y_f = far * intrinsic.height / (2.0 * intrinsic.fy)
z_f = far
points = [
[x_n, y_n, z_n],
[-x_n, y_n, z_n],
[-x_n, y_n, z_n],
[-x_n, -y_n, z_n],
[-x_n, -y_n, z_n],
[x_n, -y_n, z_n],
[x_n, -y_n, z_n],
[x_n, y_n, z_n],
[x_f, y_f, z_f],
[-x_f, y_f, z_f],
[-x_f, y_f, z_f],
[-x_f, -y_f, z_f],
[-x_f, -y_f, z_f],
[x_f, -y_f, z_f],
[x_f, -y_f, z_f],
[x_f, y_f, z_f],
[x_n, y_n, z_n],
[x_f, y_f, z_f],
[-x_n, y_n, z_n],
[-x_f, y_f, z_f],
[-x_n, -y_n, z_n],
[-x_f, -y_f, z_f],
[x_n, -y_n, z_n],
[x_f, -y_f, z_f],
]
marker.points = [to_point_msg(p) for p in points]
return marker