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

Switch nbv policy to velocity commands

Browse Source
This commit is contained in:
Michel Breyer 2021-09-06 10:11:07 +02:00
parent 1d00ed0f02
commit 6c0a39b175
1 changed files with 24 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
active_grasp/nbv.py
View File

@ -12,41 +12,43 @@ class NextBestView(MultiViewPolicy):
super().__init__(rate) super().__init__(rate)
self.max_views = 20 self.max_views = 20
self.min_ig = 10.0 self.min_ig = 10.0
self.cost_factor = 10.0
def update(self, img, extrinsic): def update(self, img, x):
if len(self.views) > self.max_views: if len(self.views) > self.max_views:
self.done = True self.done = True
return return np.zeros(6)
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: else:
# Explore occluded parts of the object. self.integrate(img, x)
views = self.generate_views() views = self.generate_views()
gains = self.compute_expected_information_gains(views) gains = self.compute_expected_information_gains(views)
costs = self.compute_movement_costs(views) costs = self.compute_movement_costs(views)
utilities = gains / np.sum(gains) - costs / np.sum(costs) utilities = gains / np.sum(gains) - costs / np.sum(costs)
self.vis.views(self.base_frame, self.intrinsic, views, utilities) self.vis.views(self.base_frame, self.intrinsic, views, utilities)
i = np.argmax(utilities) i = np.argmax(utilities)
nbv, ig = views[i], gains[i] nbv, ig = views[i], gains[i]
if ig < self.min_ig: if ig < self.min_ig:
self.done = True self.done = True
return return np.zeros(6)
cmd = nbv.inv()
return cmd cmd = self.compute_velocity_cmd(*self.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(*self.compute_error(x_d, x))
return cmd
def generate_views(self): def generate_views(self):
r, h = 0.14, 0.2 r, h = 0.14, 0.2
@ -57,7 +59,7 @@ class NextBestView(MultiViewPolicy):
eye = self.center + np.r_[0, 0, h] + spherical_to_cartesian(r, theta, phi) eye = self.center + np.r_[0, 0, h] + spherical_to_cartesian(r, theta, phi)
target = self.center target = self.center
up = np.r_[1.0, 0.0, 0.0] up = np.r_[1.0, 0.0, 0.0]
views.append(look_at(eye, target, up).inv()) views.append(look_at(eye, target, up))
return views return views
def compute_expected_information_gains(self, views): def compute_expected_information_gains(self, views):