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

Switch to velocity control

Browse Source
This commit is contained in:
Michel Breyer
2021-09-03 22:39:17 +02:00
parent 81588a1075
commit b4f68e78cc
7 changed files with 83 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
active_grasp/baselines.py
View File

@@ -7,48 +7,50 @@ from vgn.utils import look_at
class InitialView(SingleViewPolicy):
def update(self, img, extrinsic):
self.target = extrinsic
super().update(img, extrinsic)
def update(self, img, pose):
self.x_d = pose
super().update(img, pose)
class TopView(SingleViewPolicy):
def activate(self, bbox):
super().activate(bbox)
eye = np.r_[self.center[:2], self.center[2] + 0.3]
eye = np.r_[self.center[:2], self.center[2] + self.min_z_dist]
up = np.r_[1.0, 0.0, 0.0]
self.target = look_at(eye, self.center, up)
self.x_d = look_at(eye, self.center, up)
class TopTrajectory(MultiViewPolicy):
def activate(self, bbox):
super().activate(bbox)
eye = np.r_[self.center[:2], self.center[2] + 0.3]
eye = np.r_[self.center[:2], self.center[2] + self.min_z_dist]
up = np.r_[1.0, 0.0, 0.0]
self.target = look_at(eye, self.center, up)
self.x_d = look_at(eye, self.center, up)
def update(self, img, extrinsic):
self.integrate(img, extrinsic)
if np.linalg.norm(extrinsic.translation - self.target.translation) < 0.01:
def update(self, img, x):
self.integrate(img, x)
linear, angular = self.compute_error(self.x_d, x)
if np.linalg.norm(linear) < 0.01:
self.done = True
else:
return self.target
return self.compute_velocity_cmd(linear, angular)
class CircularTrajectory(MultiViewPolicy):
def __init__(self, rate):
super().__init__(rate)
self.r = 0.1
self.h = 0.3
self.duration = 12.0
self.m = scipy.interpolate.interp1d([0, self.duration], [np.pi, 3.0 * np.pi])
self.h = self.min_z_dist
self.duration = 2.0 * np.pi * self.r / self.linear_vel + 2.0
self.m = scipy.interpolate.interp1d([0.0, self.duration], [np.pi, 3.0 * np.pi])
def activate(self, bbox):
super().activate(bbox)
self.tic = rospy.Time.now()
def update(self, img, extrinsic):
self.integrate(img, extrinsic)
def update(self, img, x):
self.integrate(img, x)
elapsed_time = (rospy.Time.now() - self.tic).to_sec()
if elapsed_time > self.duration:
self.done = True
@@ -56,4 +58,6 @@ class CircularTrajectory(MultiViewPolicy):
t = self.m(elapsed_time)
eye = self.center + np.r_[self.r * np.cos(t), self.r * np.sin(t), self.h]
up = np.r_[1.0, 0.0, 0.0]
return look_at(eye, self.center, up)
x_d = look_at(eye, self.center, up)
linear, angular = self.compute_error(x_d, x)
return self.compute_velocity_cmd(linear, angular)