From 81588a10751803139ede58f35a0a6eebeddc5280 Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Fri, 3 Sep 2021 17:10:36 +0200 Subject: [PATCH] Move towards best grasp candidate --- active_grasp/controller.py | 2 +- active_grasp/nbv.py | 55 +++++++++++++++++++++++--------------- active_grasp/policy.py | 1 + active_grasp/simulation.py | 2 +- scripts/run.py | 2 +- 5 files changed, 37 insertions(+), 25 deletions(-) diff --git a/active_grasp/controller.py b/active_grasp/controller.py index 42a7dc1..a81032f 100644 --- a/active_grasp/controller.py +++ b/active_grasp/controller.py @@ -90,7 +90,7 @@ class GraspController: self.send_cmd( T_base_grasp * Transform.translation([0, 0, -0.05]) * self.T_grasp_ee ) - rospy.sleep(4.0) # TODO + rospy.sleep(3.0) # TODO # Approach grasp pose. self.send_cmd(T_base_grasp * self.T_grasp_ee) diff --git a/active_grasp/nbv.py b/active_grasp/nbv.py index 5ae7895..3d7c973 100644 --- a/active_grasp/nbv.py +++ b/active_grasp/nbv.py @@ -1,5 +1,7 @@ import itertools import numpy as np +from numpy.lib.twodim_base import eye +from scipy.ndimage.measurements import center_of_mass from .policy import MultiViewPolicy from vgn.utils import look_at, spherical_to_cartesian @@ -12,22 +14,39 @@ class NextBestView(MultiViewPolicy): self.min_ig = 10.0 def update(self, img, extrinsic): - self.integrate(img, extrinsic) - views = self.generate_views() - 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] - - if self.check_stopping_criteria(ig): - self.vis.clear_views(len(views)) + if len(self.views) > self.max_views: self.done = True + return + + T_base_cam = extrinsic.inv() + + self.integrate(img, extrinsic) + + if self.best_grasp: + R, t = self.best_grasp.pose.rotation, self.best_grasp.pose.translation + d = np.linalg.norm(T_base_cam.translation - t) + if d < 0.21: + self.done = True + return + center = t + eye = R.apply([0.0, 0.0, -0.2]) + t + up = np.r_[1.0, 0.0, 0.0] + cmd = look_at(eye, center, up) else: - return nbv.inv() # the controller expects T_cam_base + # Explore occluded parts of the object. + views = self.generate_views() + 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] + if ig < self.min_ig: + self.done = True + return + cmd = nbv.inv() + + return cmd def generate_views(self): r, h = 0.14, 0.2 @@ -47,14 +66,6 @@ class NextBestView(MultiViewPolicy): def compute_movement_costs(self, views): return [self.cost_fn(v) for v in views] - def check_stopping_criteria(self, ig): - if len(self.views) > self.max_views: - return True - if ig < self.min_ig: - return True - # TODO check whether a "good enough" grasp has been found - return False - def ig_fn(self, view, downsample=20): fx = self.intrinsic.fx / downsample fy = self.intrinsic.fy / downsample diff --git a/active_grasp/policy.py b/active_grasp/policy.py index 74fc7ec..5aa2ec0 100644 --- a/active_grasp/policy.py +++ b/active_grasp/policy.py @@ -51,6 +51,7 @@ class Policy: rospy.sleep(0.1) def sort_grasps(self, in_grasps): + # Transforms grasps into base frame, checks whether they lie on the target, and sorts by their score grasps, scores = [], [] for grasp in in_grasps: diff --git a/active_grasp/simulation.py b/active_grasp/simulation.py index 93d7f6b..a63a983 100644 --- a/active_grasp/simulation.py +++ b/active_grasp/simulation.py @@ -147,7 +147,7 @@ class CartesianPoseController: self.frame = frame self.kp = np.ones(6) * 4.0 - self.max_linear_vel = 0.1 + self.max_linear_vel = 0.05 self.max_angular_vel = 1.57 self.x_d = x0 diff --git a/scripts/run.py b/scripts/run.py index f230bf8..b75e93d 100644 --- a/scripts/run.py +++ b/scripts/run.py @@ -30,7 +30,7 @@ def main(): def create_parser(): parser = argparse.ArgumentParser() parser.add_argument("policy", type=str, choices=registry.keys()) - parser.add_argument("--runs", type=int, default=10) + 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=12)