diff --git a/src/active_grasp/nbv.py b/src/active_grasp/nbv.py index 183dc8c..3ee0f47 100644 --- a/src/active_grasp/nbv.py +++ b/src/active_grasp/nbv.py @@ -70,7 +70,7 @@ class NextBestView(MultiViewPolicy): with Timer("state_update"): self.integrate(img, x, q) with Timer("view_generation"): - views = self.generate_views() + views = self.generate_views(q) with Timer("ig_computation"): gains = [self.ig_fn(v, 10) for v in views] with Timer("cost_computation"): @@ -98,13 +98,13 @@ class NextBestView(MultiViewPolicy): return True return False - def generate_views(self): + def generate_views(self, q): thetas = np.deg2rad([15, 30, 45]) phis = np.arange(6) * np.deg2rad(60) view_candidates = [] for theta, phi in itertools.product(thetas, phis): view = self.view_sphere.get_view(theta, phi) - if self.is_feasible(view): + if self.is_feasible(view, q): view_candidates.append(view) return view_candidates diff --git a/src/active_grasp/policy.py b/src/active_grasp/policy.py index 7f7a966..add82ca 100644 --- a/src/active_grasp/policy.py +++ b/src/active_grasp/policy.py @@ -36,7 +36,8 @@ class Policy: self.vis = Visualizer() def is_feasible(self, view, q_init=None): - q_init = q_init if q_init else [0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79] + if q_init is None: + q_init = [0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79] return self.model.ik(q_init, view) is not None def activate(self, bbox, view_sphere):