From 65bdb5422d011a427169803a90218c0b39b8cb5f Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Sat, 11 Sep 2021 20:49:55 +0200 Subject: [PATCH] Move on a half sphere --- active_grasp/baselines.py | 20 +++++------- active_grasp/bbox.py | 1 + active_grasp/controller.py | 65 +++++++++++++++++++++++++++++++------ active_grasp/nbv.py | 66 ++++++++++++-------------------------- active_grasp/policy.py | 55 ++++--------------------------- cfg/active_grasp.yaml | 13 ++++---- scripts/run.py | 7 ++-- 7 files changed, 101 insertions(+), 126 deletions(-) diff --git a/active_grasp/baselines.py b/active_grasp/baselines.py index 77e8501..dffb2d2 100644 --- a/active_grasp/baselines.py +++ b/active_grasp/baselines.py @@ -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) diff --git a/active_grasp/bbox.py b/active_grasp/bbox.py index e991b71..ef447d4 100644 --- a/active_grasp/bbox.py +++ b/active_grasp/bbox.py @@ -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): diff --git a/active_grasp/controller.py b/active_grasp/controller.py index c7f9f01..44c3c19 100644 --- a/active_grasp/controller.py +++ b/active_grasp/controller.py @@ -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 diff --git a/active_grasp/nbv.py b/active_grasp/nbv.py index d5b5bee..33b2f7a 100644 --- a/active_grasp/nbv.py +++ b/active_grasp/nbv.py @@ -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 diff --git a/active_grasp/policy.py b/active_grasp/policy.py index 7e95273..7d52b68 100644 --- a/active_grasp/policy.py +++ b/active_grasp/policy.py @@ -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() diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml index 559e1d5..278dd6c 100644 --- a/cfg/active_grasp.yaml +++ b/cfg/active_grasp.yaml @@ -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 diff --git a/scripts/run.py b/scripts/run.py index 5f6b3b1..4e4de98 100644 --- a/scripts/run.py +++ b/scripts/run.py @@ -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