Execute grasps with MoveIt

active_grasp/controller.py

@@ -1,3 +1,4 @@
+from controller_manager_msgs.srv import *
 import copy
 import cv_bridge
 from geometry_msgs.msg import Twist
@@ -11,15 +12,16 @@ from active_grasp.srv import Reset, ResetRequest
 from robot_helpers.ros import tf
 from robot_helpers.ros.conversions import *
 from robot_helpers.ros.panda import PandaGripperClient
+from robot_helpers.ros.moveit import MoveItClient
 from robot_helpers.spatial import Rotation, Transform
 
 
 class GraspController:
     def __init__(self, policy):
         self.policy = policy
-        self.reset_env = rospy.ServiceProxy("reset", Reset)
         self.load_parameters()
         self.lookup_transforms()
+        self.init_service_proxies()
         self.init_robot_connection()
         self.init_camera_stream()
 
@@ -34,9 +36,28 @@ class GraspController:
         tf.init()
         self.T_ee_cam = tf.lookup(self.ee_frame, self.cam_frame)
 
+    def init_service_proxies(self):
+        self.reset_env = rospy.ServiceProxy("reset", Reset)
+        self.switch_controller = rospy.ServiceProxy(
+            "controller_manager/switch_controller", SwitchController
+        )
+
     def init_robot_connection(self):
         self.cartesian_vel_pub = rospy.Publisher("command", Twist, queue_size=10)
         self.gripper = PandaGripperClient()
+        self.moveit = MoveItClient("panda_arm")
+
+    def switch_to_cartesian_velocity_control(self):
+        req = SwitchControllerRequest()
+        req.start_controllers = ["cartesian_velocity_controller"]
+        req.stop_controllers = ["position_joint_trajectory_controller"]
+        self.switch_controller(req)
+
+    def switch_to_joint_trajectory_control(self):
+        req = SwitchControllerRequest()
+        req.start_controllers = ["position_joint_trajectory_controller"]
+        req.stop_controllers = ["cartesian_velocity_controller"]
+        self.switch_controller(req)
 
     def init_camera_stream(self):
         self.cv_bridge = cv_bridge.CvBridge()
@@ -47,9 +68,15 @@ class GraspController:
 
     def run(self):
         bbox = self.reset()
+
+        self.switch_to_cartesian_velocity_control()
         with Timer("search_time"):
             grasp = self.search_grasp(bbox)
-        res = self.execute_grasp(grasp)
+
+        self.switch_to_joint_trajectory_control()
+        with Timer("execution_time"):
+            res = self.execute_grasp(grasp)
+
         return self.collect_info(res)
 
     def reset(self):
@@ -60,14 +87,11 @@ class GraspController:
     def search_grasp(self, bbox):
         self.policy.activate(bbox)
         r = rospy.Rate(self.policy.rate)
-        while True:
+        while not self.policy.done:
             img, pose = self.get_state()
             cmd = self.policy.update(img, pose)
-            if self.policy.done:
-                break
-            else:
-                self.cartesian_vel_pub.publish(to_twist_msg(cmd))
-                r.sleep()
+            self.cartesian_vel_pub.publish(to_twist_msg(cmd))
+            r.sleep()
         return self.policy.best_grasp
 
     def get_state(self):
@@ -77,40 +101,25 @@ class GraspController:
         return img, pose
 
     def execute_grasp(self, grasp):
-        return
         if not grasp:
             return "aborted"
 
         T_base_grasp = self.postprocess(grasp.pose)
+
         self.gripper.move(0.08)
 
-        # Move to an initial pose offset.
-        self.send_cmd(
-            T_base_grasp * Transform.translation([0, 0, -0.05]) * self.T_grasp_ee
-        )
-        rospy.sleep(3.0)  # TODO
-
-        # Approach grasp pose.
-        self.send_cmd(T_base_grasp * self.T_grasp_ee)
-        rospy.sleep(2.0)
-
-        # Close the fingers.
+        self.moveit.goto(T_base_grasp * Transform.t([0, 0, -0.05]) * self.T_grasp_ee)
+        self.moveit.goto(T_base_grasp * self.T_grasp_ee)
         self.gripper.grasp()
+        self.moveit.goto(Transform.t([0, 0, 0.2]) * T_base_grasp * self.T_grasp_ee)
 
-        # Lift the object.
-        target = Transform.translation([0, 0, 0.2]) * T_base_grasp * self.T_grasp_ee
-        self.send_cmd(target)
-        rospy.sleep(2.0)
-
-        # Check whether the object remains in the hand
         success = self.gripper.read() > 0.005
 
         return "succeeded" if success else "failed"
 
     def postprocess(self, T_base_grasp):
-        # Ensure that the camera is pointing forward.
         rot = T_base_grasp.rotation
-        if rot.as_matrix()[:, 0][0] < 0:
+        if rot.as_matrix()[:, 0][0] < 0:  # Ensure that the camera is pointing forward
             T_base_grasp.rotation = rot * Rotation.from_euler("z", np.pi)
         return T_base_grasp