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

Move on a half sphere

Browse Source
This commit is contained in:
Michel Breyer 2021-09-11 20:49:55 +02:00
parent 4ebd587553
commit 65bdb5422d
7 changed files with 101 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
active_grasp/baselines.py
View File

@ -6,28 +6,24 @@ from .policy import SingleViewPolicy, MultiViewPolicy, compute_error
class InitialView(SingleViewPolicy):
def update(self, img, pose):
self.x_d = pose
cmd = super().update(img, pose)
return cmd
super().update(img, pose)
class TopView(SingleViewPolicy):
def activate(self, bbox):
super().activate(bbox)
def activate(self, bbox, view_sphere):
super().activate(bbox, view_sphere)
self.x_d = self.view_sphere.get_view(0.0, 0.0)
self.done = False if self.is_view_feasible(self.x_d) else True
self.done = False if self.view_sphere.is_feasible(self.x_d) else True
class TopTrajectory(MultiViewPolicy):
def activate(self, bbox):
super().activate(bbox)
def activate(self, bbox, view_sphere):
super().activate(bbox, view_sphere)
self.x_d = self.view_sphere.get_view(0.0, 0.0)
self.done = False if self.is_view_feasible(self.x_d) else True
self.done = False if self.view_sphere.is_feasible(self.x_d) else True
def update(self, img, x):
self.integrate(img, x)
linear, angular = compute_error(self.x_d, x)
linear, _ = compute_error(self.x_d, x)
if np.linalg.norm(linear) < 0.02:
self.done = True
return np.zeros(6)
else:
return self.compute_velocity_cmd(linear, angular)

1
active_grasp/bbox.py
View File

@ -10,6 +10,7 @@ class AABBox:
self.min = np.asarray(bbox_min)
self.max = np.asarray(bbox_max)
self.center = 0.5 * (self.min + self.max)
self.size = self.max - self.min
@property
def corners(self):

65
active_grasp/controller.py
View File

@ -14,6 +14,7 @@ from robot_helpers.ros.conversions import *
from robot_helpers.ros.panda import PandaGripperClient
from robot_helpers.ros.moveit import MoveItClient
from robot_helpers.spatial import Rotation, Transform
from vgn.utils import look_at, cartesian_to_spherical, spherical_to_cartesian
class GraspController:
@ -27,10 +28,13 @@ class GraspController:
def load_parameters(self):
self.base_frame = rospy.get_param("~base_frame_id")
self.ee_frame = rospy.get_param("~ee_frame_id")
self.T_grasp_ee = Transform.from_list(rospy.get_param("~ee_grasp_offset")).inv()
self.cam_frame = rospy.get_param("~camera/frame_id")
self.depth_topic = rospy.get_param("~camera/depth_topic")
self.T_grasp_ee = Transform.from_list(rospy.get_param("~ee_grasp_offset")).inv()
self.min_z_dist = rospy.get_param("~camera/min_z_dist")
self.control_rate = rospy.get_param("~control_rate")
self.linear_vel = rospy.get_param("~linear_vel")
self.policy_rate = rospy.get_param("~policy_rate")
def init_service_proxies(self):
self.reset_env = rospy.ServiceProxy("reset", Reset)
@ -44,10 +48,9 @@ class GraspController:
def init_moveit(self):
self.moveit = MoveItClient("panda_arm")
rospy.sleep(1.0) # wait for connections to be established
rospy.sleep(1.0) # Wait for connections to be established.
# msg = to_pose_stamped_msg(Transform.t([0.4, 0, 0.4]), self.base_frame)
# self.moveit.scene.add_box("table", msg, size=(0.5, 0.5, 0.02))
self.policy.moveit = self.moveit
def switch_to_cartesian_velocity_control(self):
req = SwitchControllerRequest()
@ -83,17 +86,20 @@ class GraspController:
def reset(self):
res = self.reset_env(ResetRequest())
rospy.sleep(1.0) # wait for states to be updated
rospy.sleep(1.0) # Wait for the TF tree to be updated.
return from_bbox_msg(res.bbox)
def search_grasp(self, bbox):
self.policy.activate(bbox)
r = rospy.Rate(self.policy.rate)
self.view_sphere = ViewHalfSphere(bbox, self.min_z_dist, self.moveit)
self.policy.activate(bbox, self.view_sphere)
timer = rospy.Timer(rospy.Duration(1.0 / self.control_rate), self.send_vel_cmd)
r = rospy.Rate(self.policy_rate)
while not self.policy.done:
img, pose = self.get_state()
cmd = self.policy.update(img, pose)
self.cartesian_vel_pub.publish(to_twist_msg(cmd))
self.policy.update(img, pose)
r.sleep()
rospy.sleep(0.1) # Wait for a zero command to be sent to the robot.
timer.shutdown()
return self.policy.best_grasp
def get_state(self):
@ -102,6 +108,26 @@ class GraspController:
pose = tf.lookup(self.base_frame, self.cam_frame, msg.header.stamp)
return img, pose
def send_vel_cmd(self, event):
if self.policy.x_d is None or self.policy.done:
cmd = np.zeros(6)
else:
x = tf.lookup(self.base_frame, self.cam_frame)
cmd = self.compute_velocity_cmd(self.policy.x_d, x)
self.cartesian_vel_pub.publish(to_twist_msg(cmd))
def compute_velocity_cmd(self, x_d, x):
# TODO verify that the frames are handled correctly
r, theta, phi = cartesian_to_spherical(x.translation - self.view_sphere.center)
e_t = x_d.translation - x.translation
e_n = (self.view_sphere.center - x.translation) * (r - self.view_sphere.r) / r
linear = 1.0 * e_t + 10.0 * e_n
scale = np.linalg.norm(linear)
linear *= np.clip(scale, 0.0, self.linear_vel) / scale
angular = self.view_sphere.get_view(theta, phi).rotation * x.rotation.inv()
angular = 0.5 * angular.as_rotvec()
return np.r_[linear, angular]
def execute_grasp(self, grasp):
if not grasp:
return "aborted"
@ -131,3 +157,24 @@ class GraspController:
}
info.update(Timer.timers)
return info
class ViewHalfSphere:
# TODO move feasibility check to a robot interface module
def __init__(self, bbox, min_z_dist, moveit):
self.center = bbox.center
self.r = 0.5 * bbox.size[2] + min_z_dist
self.moveit = moveit
self.T_cam_ee = tf.lookup("camera_depth_optical_frame", "panda_link8")
def get_view(self, theta, phi):
eye = self.center + spherical_to_cartesian(self.r, theta, phi)
up = np.r_[1.0, 0.0, 0.0]
return look_at(eye, self.center, up)
def sample_view(self):
raise NotImplementedError
def is_feasible(self, view):
success, _ = self.moveit.plan(view * self.T_cam_ee)
return success

66
active_grasp/nbv.py
View File

@ -2,59 +2,40 @@ import itertools
import numpy as np
import rospy
from .policy import MultiViewPolicy, compute_error
from vgn.utils import look_at
from .policy import MultiViewPolicy
class NextBestView(MultiViewPolicy):
def __init__(self, rate):
super().__init__(rate)
self.max_views = 20
self.min_ig = 10.0
self.cost_factor = 10.0
def __init__(self):
super().__init__()
self.max_views = 40
def activate(self, bbox):
super().activate(bbox)
def activate(self, bbox, view_sphere):
super().activate(bbox, view_sphere)
self.generate_view_candidates()
def update(self, img, x):
if len(self.views) > self.max_views:
self.done = True
return np.zeros(6)
else:
self.integrate(img, x)
views = self.view_candidates
gains = self.compute_expected_information_gains(views)
costs = self.compute_movement_costs(views)
utilities = gains / np.sum(gains) - costs / np.sum(costs)
self.vis.views(self.base_frame, self.intrinsic, views, utilities)
i = np.argmax(utilities)
nbv, ig = views[i], gains[i]
cmd = self.compute_velocity_cmd(*compute_error(nbv, x))
if self.best_grasp:
R, t = self.best_grasp.pose.rotation, self.best_grasp.pose.translation
if np.linalg.norm(t - x.translation) < self.min_z_dist:
self.done = True
return np.zeros(6)
center = t
eye = R.apply([0.0, 0.0, -0.2]) + t
up = np.r_[1.0, 0.0, 0.0]
x_d = look_at(eye, center, up)
cmd = self.compute_velocity_cmd(*compute_error(x_d, x))
return cmd
def generate_view_candidates(self):
thetas = np.arange(1, 4) * np.deg2rad(30)
phis = np.arange(1, 6) * np.deg2rad(60)
self.view_candidates = []
for theta, phi in itertools.product(thetas, phis):
view = self.view_sphere.get_view(theta, phi)
if self.is_view_feasible(view):
if self.view_sphere.is_feasible(view):
self.view_candidates.append(view)
def compute_expected_information_gains(self, views):
return [self.ig_fn(v) for v in views]
def update(self, img, x):
if len(self.views) > self.max_views:
self.done = True
else:
self.integrate(img, x)
views = self.view_candidates
gains = [self.ig_fn(v) for v in views]
costs = [self.cost_fn(v) for v in views]
utilities = gains / np.sum(gains) - costs / np.sum(costs)
self.vis.views(self.base_frame, self.intrinsic, views, utilities)
i = np.argmax(utilities)
nbv, _ = views[i], gains[i]
self.x_d = nbv
def ig_fn(self, view, downsample=20):
fx = self.intrinsic.fx / downsample
@ -88,18 +69,14 @@ class NextBestView(MultiViewPolicy):
origin = view.translation
direction = np.r_[(u - cx) / fx, (v - cy) / fy, 1.0]
direction = view.rotation.apply(direction / np.linalg.norm(direction))
# self.vis.rays(self.task_frame, origin, [direction])
# rospy.sleep(0.01)
t, tsdf_prev = t_min, -1.0
while t < t_max:
p = origin + t * direction
t += t_step
# self.vis.point(self.task_frame, p)
# rospy.sleep(0.01)
index = get_voxel_at(p)
if index is not None:
i, j, k = index
@ -117,8 +94,5 @@ class NextBestView(MultiViewPolicy):
return ig
def compute_movement_costs(self, views):
return [self.cost_fn(v) for v in views]
def cost_fn(self, view):
return 1.0

55
active_grasp/policy.py
View File

@ -8,42 +8,31 @@ from robot_helpers.ros import tf
from robot_helpers.ros.conversions import *
from vgn.detection import *
from vgn.perception import UniformTSDFVolume
from vgn.utils import look_at, spherical_to_cartesian
class Policy:
def __init__(self, rate=5):
self.rate = rate
self.load_parameters()
self.init_visualizer()
self.lookup_transforms()
def load_parameters(self):
self.base_frame = rospy.get_param("~base_frame_id")
self.cam_frame = rospy.get_param("~camera/frame_id")
info_topic = rospy.get_param("~camera/info_topic")
self.linear_vel = rospy.get_param("~linear_vel")
self.min_z_dist = rospy.get_param("~camera/min_z_dist")
self.qual_threshold = rospy.get_param("~qual_threshold")
self.task_frame = "task"
info_topic = rospy.get_param("~camera/info_topic")
msg = rospy.wait_for_message(info_topic, CameraInfo, rospy.Duration(2.0))
self.intrinsic = from_camera_info_msg(msg)
self.qual_threshold = rospy.get_param("vgn/qual_threshold")
def init_visualizer(self):
self.vis = Visualizer()
def lookup_transforms(self):
self.T_cam_ee = tf.lookup(self.cam_frame, "panda_link8")
def activate(self, bbox):
def activate(self, bbox, view_sphere):
self.vis.clear()
self.bbox = bbox
self.view_sphere = view_sphere
self.vis.bbox(self.base_frame, self.bbox)
self.view_sphere = ViewSphere(bbox)
self.calibrate_task_frame()
self.tsdf = UniformTSDFVolume(0.3, 40)
@ -51,6 +40,7 @@ class Policy:
self.views = []
self.best_grasp = None
self.x_d = None
self.done = False
def calibrate_task_frame(self):
@ -89,26 +79,12 @@ class Policy:
def score_fn(self, grasp):
return grasp.quality
def is_view_feasible(self, view):
# Check whether MoveIt can find a trajectory to the given view
success, _ = self.moveit.plan(view * self.T_cam_ee)
return success
def compute_velocity_cmd(self, linear, angular):
kp = 4.0
linear = kp * linear
scale = np.linalg.norm(linear)
linear *= np.clip(scale, 0.0, self.linear_vel) / scale
return np.r_[linear, angular]
class SingleViewPolicy(Policy):
def update(self, img, x):
linear, angular = compute_error(self.x_d, x)
linear, _ = compute_error(self.x_d, x)
if np.linalg.norm(linear) < 0.02:
self.views.append(x)
self.tsdf.integrate(img, self.intrinsic, x.inv() * self.T_base_task)
tsdf_grid, voxel_size = self.tsdf.get_grid(), self.tsdf.voxel_size
self.vis.scene_cloud(self.task_frame, self.tsdf.get_scene_cloud())
@ -119,14 +95,10 @@ class SingleViewPolicy(Policy):
grasps = select_grid(voxel_size, out, threshold=self.qual_threshold)
grasps, scores = self.sort_grasps(grasps)
self.vis.grasps(self.base_frame, grasps, scores)
self.best_grasp = grasps[0] if len(grasps) > 0 else None
self.done = True
return np.zeros(6)
else:
return self.compute_velocity_cmd(linear, angular)
class MultiViewPolicy(Policy):
@ -152,21 +124,6 @@ class MultiViewPolicy(Policy):
self.vis.grasps(self.base_frame, grasps, scores)
class ViewSphere:
# Define sphere for view generation on top of the bbox.
# TODO an ellipse around the bbox's center would be even nicer ;)
def __init__(self, bbox):
self.center = np.r_[bbox.center[:2], bbox.max[2]]
self.r = rospy.get_param("~camera/min_z_dist")
self.target = bbox.center
def get_view(self, theta, phi):
eye = self.center + spherical_to_cartesian(self.r, theta, phi)
up = np.r_[1.0, 0.0, 0.0]
return look_at(eye, self.target, up)
def compute_error(x_d, x):
linear = x_d.translation - x.translation
angular = (x_d.rotation * x.rotation.inv()).as_rotvec()

13
cfg/active_grasp.yaml
View File

@ -1,20 +1,21 @@
bt_sim:
gui: False
gui: True
cam_noise: True
scene: test.yaml
scene: test1.yaml
grasp_controller:
base_frame_id: panda_link0
ee_frame_id: panda_hand
ee_grasp_offset: [0.0, 0.0, -0.383, 0.924, 0.0, 0.0, 0.065] # offset to the moveit ee
control_rate: 30
linear_vel: 0.05
qual_threshold: 0.9
policy_rate: 5
camera:
frame_id: camera_optical_frame
frame_id: camera_depth_optical_frame
info_topic: /camera/depth/camera_info
depth_topic: /camera/depth/image_raw
min_z_dist: 0.26
min_z_dist: 0.3
vgn:
model: $(find vgn)/assets/models/vgn_conv.pth
finger_depth: 0.05
qual_threshold: 0.9

7
scripts/run.py
View File

@ -18,11 +18,12 @@ def main():
parser = create_parser()
args = parser.parse_args()
policy = make(args.policy, args.rate)
policy = make(args.policy)
controller = GraspController(policy)
logger = Logger(args)
seed_simulation(args.seed)
rospy.sleep(1.0) # Prevents a rare race condiion
for _ in tqdm(range(args.runs)):
info = controller.run()
@ -34,7 +35,6 @@ def create_parser():
parser.add_argument("policy", type=str, choices=registry.keys())
parser.add_argument("--runs", type=int, default=100)
parser.add_argument("--logdir", type=Path, default="logs")
parser.add_argument("--rate", type=int, default=5)
parser.add_argument("--seed", type=int, default=1)
return parser
@ -42,10 +42,9 @@ def create_parser():
class Logger:
def __init__(self, args):
stamp = datetime.now().strftime("%y%m%d-%H%M%S")
name = "{}_policy={},rate={},seed={}.csv".format(
name = "{}_policy={},seed={}.csv".format(
stamp,
args.policy,
args.rate,
args.seed,
)
self.path = args.logdir / name