Execute grasps with MoveIt

active_grasp/baselines.py

@@ -30,8 +30,9 @@ class TopTrajectory(MultiViewPolicy):
     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:
+        if np.linalg.norm(linear) < 0.02:
             self.done = True
+            return np.zeros(6)
         else:
             return self.compute_velocity_cmd(linear, angular)
 
@@ -50,10 +51,10 @@ class CircularTrajectory(MultiViewPolicy):
 
     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
+            return np.zeros(6)
         else:
             t = self.m(elapsed_time)
             eye = self.center + np.r_[self.r * np.cos(t), self.r * np.sin(t), self.h]