Grasp using velocity control

scripts/bt_sim_node.py

@@ -253,7 +253,7 @@ class GraspActionPlugin(Plugin):
     def action_goal_cb(self):
         self.elapsed_time = 0.0
         goal = self.action_server.accept_new_goal()
-        self.gripper.set_desired_width(goal.width)
+        self.gripper.set_desired_speed(-0.1)
 
     def update(self):
         if self.action_server.is_active():

src/active_grasp/simulation.py

@@ -63,6 +63,7 @@ class Simulation:
             p.resetJointState(self.arm.uid, i, q_i, 0)
         p.resetJointState(self.arm.uid, 9, 0.04, 0)
         p.resetJointState(self.arm.uid, 10, 0.04, 0)
+        self.gripper.set_desired_width(0.4)
 
     def select_target(self):
         _, _, mask = self.camera.get_image()