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
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)