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]

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)
+
+        self.switch_to_joint_trajectory_control()
+        with Timer("execution_time"):
             res = self.execute_grasp(grasp)
+
         return self.collect_info(res)
 
     def reset(self):
@@ -60,12 +87,9 @@ 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()
         return self.policy.best_grasp
@@ -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.moveit.goto(T_base_grasp * Transform.t([0, 0, -0.05]) * self.T_grasp_ee)
-        self.send_cmd(
+        self.moveit.goto(T_base_grasp * self.T_grasp_ee)
-            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.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
 

active_grasp/policy.py

@@ -55,7 +55,10 @@ class Policy:
         return linear, angular
 
     def compute_velocity_cmd(self, linear, angular):
-        linear = linear / np.linalg.norm(linear) * self.linear_vel
+        kp = 4.0
+        linear = kp * linear
+        scale = np.linalg.norm(linear)
+        linear *= np.clip(scale, 0.0, self.linear_vel) / scale
         return np.r_[linear, angular]
 
     def sort_grasps(self, in_grasps):
@@ -85,7 +88,7 @@ class SingleViewPolicy(Policy):
     def update(self, 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.views.append(x)
 
             self.tsdf.integrate(img, self.intrinsic, x.inv() * self.T_base_task)
@@ -103,6 +106,7 @@ class SingleViewPolicy(Policy):
 
             self.best_grasp = grasps[0] if len(grasps) > 0 else None
             self.done = True
+            return np.zeros(6)
         else:
             return self.compute_velocity_cmd(linear, angular)
 

cfg/active_grasp.yaml

@@ -6,8 +6,8 @@ bt_sim:
 grasp_controller:
   base_frame_id: panda_link0
   ee_frame_id: panda_hand
-  ee_grasp_offset: [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.065]
+  ee_grasp_offset: [0.0, 0.0, -0.383, 0.924, 0.0, 0.0, 0.065]  # offset to the moveit ee
-  linear_vel: 0.1
+  linear_vel: 0.05
   qual_threshold: 0.9
   camera:
     frame_id: camera_optical_frame

launch/active_grasp.rviz

@@ -7,7 +7,7 @@ Panels:
         - /TF1/Frames1
         - /Markers1/Namespaces1
       Splitter Ratio: 0.5
-    Tree Height: 543
+    Tree Height: 538
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -26,7 +26,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: SceneCloud
+    SyncSource: Image
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -132,39 +132,222 @@ Visualization Manager:
       Update Interval: 0
       Value: true
       Visual Enabled: true
+    - Acceleration_Scaling_Factor: 0.1
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: false
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Warehouse_Host: 127.0.0.1
+      MoveIt_Warehouse_Port: 33829
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          camera_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Loop Animation: false
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 0.05 s
+        Trail Step Size: 1
+        Trajectory Topic: /move_group/display_planned_path
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0
+        Joint Violation Color: 255; 0; 255
+        Planning Group: panda_arm
+        Query Goal State: true
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: move_group/monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          camera_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: false
+      Velocity_Scaling_Factor: 0.1
     - Class: rviz/TF
-      Enabled: true
+      Enabled: false
       Frame Timeout: 15
       Frames:
         All Enabled: false
-        camera_optical_frame:
-          Value: true
-        panda_hand:
-          Value: true
-        panda_leftfinger:
-          Value: false
-        panda_link0:
-          Value: true
-        panda_link1:
-          Value: false
-        panda_link2:
-          Value: false
-        panda_link3:
-          Value: false
-        panda_link4:
-          Value: false
-        panda_link5:
-          Value: false
-        panda_link6:
-          Value: false
-        panda_link7:
-          Value: false
-        panda_link8:
-          Value: false
-        panda_rightfinger:
-          Value: false
-        task:
-          Value: true
       Marker Alpha: 1
       Marker Scale: 0.5
       Name: TF
@@ -172,37 +355,8 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        panda_link0:
-          panda_link1:
-            panda_link2:
-              panda_link3:
-                panda_link4:
-                  panda_link5:
-                    panda_link6:
-                      panda_link7:
-                        panda_link8:
-                          panda_hand:
-                            camera_optical_frame:
-                              {}
-                            panda_leftfinger:
-                              {}
-                            panda_rightfinger:
-                              {}
-          task:
         {}
       Update Interval: 0
-      Value: true
-    - Class: rviz/Image
-      Enabled: false
-      Image Topic: /camera/depth/image_raw
-      Max Value: 1
-      Median window: 5
-      Min Value: 0
-      Name: Depth
-      Normalize Range: true
-      Queue Size: 2
-      Transport Hint: raw
-      Unreliable: false
       Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
@@ -232,7 +386,7 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
-    - Alpha: 1
+    - Alpha: 0.20000000298023224
       Autocompute Intensity Bounds: false
       Autocompute Value Bounds:
         Max Value: 10
@@ -262,7 +416,7 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: false
-    - Alpha: 0.20000000298023224
+    - Alpha: 1
       Autocompute Intensity Bounds: false
       Autocompute Value Bounds:
         Max Value: 10
@@ -274,7 +428,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: Intensity
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 138; 226; 52
       Max Intensity: 1
@@ -291,36 +445,30 @@ Visualization Manager:
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: false
-      Value: true
+      Value: false
-    - Alpha: 1
-      Axes Length: 0.05000000074505806
-      Axes Radius: 0.009999999776482582
-      Class: rviz/Pose
-      Color: 255; 25; 0
-      Enabled: true
-      Head Length: 0.30000001192092896
-      Head Radius: 0.10000000149011612
-      Name: EETarget
-      Queue Size: 10
-      Shaft Length: 1
-      Shaft Radius: 0.05000000074505806
-      Shape: Axes
-      Topic: /command
-      Unreliable: false
-      Value: true
     - Class: rviz/MarkerArray
       Enabled: true
       Marker Topic: visualization_marker_array
       Name: Markers
       Namespaces:
         bbox: true
+        best_grasp: true
         grasps: true
         path: true
-        point: true
-        rays: true
-        views: true
       Queue Size: 100
       Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /camera/depth/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -349,7 +497,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.4553453922271729
+      Distance: 1.1316771507263184
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -357,27 +505,31 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 0.43946000933647156
+        X: 0.26778873801231384
-        Y: 0.016814040020108223
+        Y: 0.05089891329407692
-        Z: 0.40760287642478943
+        Z: 0.41754576563835144
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.19979602098464966
+      Pitch: 0.27479735016822815
       Target Frame: <Fixed Frame>
-      Yaw: 5.38097620010376
+      Yaw: 5.182796955108643
     Saved: ~
 Window Geometry:
-  Depth:
-    collapsed: false
   Displays:
     collapsed: false
-  Height: 762
+  Height: 1019
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001560000025cfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000025c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0044006500700074006800000001e4000000b50000001600ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004000000003efc0100000002fb0000000800540069006d0065010000000000000400000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002a40000025c00000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Image:
+    collapsed: false
+  MotionPlanning:
+    collapsed: false
+  MotionPlanning - Trajectory Slider:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f300000362fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000257000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a004400650070007400680000000166000000b50000000000000000fb0000000a0049006d006100670065010000029a000001050000001600fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000217000001880000017d00fffffffb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ec00000039fc0100000002fb0000000800540069006d00650100000000000005ec000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003f30000036200000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -386,6 +538,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1024
+  Width: 1516
-  X: 1440
+  X: 905
-  Y: 268
+  Y: 139

launch/simulation.launch

@@ -2,12 +2,18 @@
 <launch>
   <arg name="launch_rviz" default="false" />
 
+  <!-- Load parameters -->
   <rosparam command="load" file="$(find active_grasp)/cfg/active_grasp.yaml" subst_value="true" />
 
+  <!-- Launch simulated robot -->
   <param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/urdfs/franka/panda_arm_hand.urdf.xacro" />
-
   <node pkg="active_grasp" type="bt_sim_node.py" name="bt_sim" output="screen" />
   <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
 
+  <!-- Launch MoveIt -->
+  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster" args="0 0 0 0 0 0 world panda_link0" />
+  <include file="$(find panda_moveit_config)/launch/move_group.launch" />
+
+  <!-- Launch rviz -->
   <node pkg="rviz" type="rviz" name="rviz" args="-d $(find active_grasp)/launch/active_grasp.rviz" if="$(arg launch_rviz)" />
 </launch>

scripts/bt_sim_node.py

@@ -1,6 +1,8 @@
 #!/usr/bin/env python3
 
 from actionlib import SimpleActionServer
+import control_msgs.msg
+from controller_manager_msgs.srv import *
 import cv_bridge
 from franka_gripper.msg import *
 from geometry_msgs.msg import Twist
@@ -27,34 +29,22 @@ class BtSimNode:
         self.plugins = [
             PhysicsPlugin(self.sim),
             JointStatePlugin(self.sim.arm, self.sim.gripper),
-            CartesianVelocityControllerPlugin(self.sim.arm, self.sim.model),
             MoveActionPlugin(self.sim.gripper),
             GraspActionPlugin(self.sim.gripper),
+            GripperActionPlugin(),
             CameraPlugin(self.sim.camera),
         ]
-
+        self.controllers = {
-    def advertise_services(self):
+            "cartesian_velocity_controller": CartesianVelocityControllerPlugin(
-        rospy.Service("seed", Seed, self.seed)
+                self.sim.arm, self.sim.model
-        rospy.Service("reset", Reset, self.reset)
+            ),
-
+            "position_joint_trajectory_controller": JointTrajectoryControllerPlugin(
-    def seed(self, req):
+                self.sim.arm
-        self.sim.seed(req.seed)
+            ),
-        return SeedResponse()
+        }
-
-    def reset(self, req):
-        self.deactivate_plugins()
-        rospy.sleep(1.0)  # TODO replace with a read-write lock
-        bbox = self.sim.reset()
-        self.activate_plugins()
-        return ResetResponse(to_bbox_msg(bbox))
-
-    def run(self):
-        self.start_plugins()
-        self.activate_plugins()
-        rospy.spin()
 
     def start_plugins(self):
-        for plugin in self.plugins:
+        for plugin in self.plugins + list(self.controllers.values()):
             plugin.thread.start()
 
     def activate_plugins(self):
@@ -65,6 +55,43 @@ class BtSimNode:
         for plugin in self.plugins:
             plugin.deactivate()
 
+    def deactivate_controllers(self):
+        for controller in self.controllers.values():
+            controller.deactivate()
+
+    def advertise_services(self):
+        rospy.Service("seed", Seed, self.seed)
+        rospy.Service("reset", Reset, self.reset)
+        rospy.Service(
+            "/controller_manager/switch_controller",
+            SwitchController,
+            self.switch_controller,
+        )
+
+    def seed(self, req):
+        self.sim.seed(req.seed)
+        return SeedResponse()
+
+    def reset(self, req):
+        self.deactivate_plugins()
+        self.deactivate_controllers()
+        rospy.sleep(1.0)  # TODO replace with a read-write lock
+        bbox = self.sim.reset()
+        self.activate_plugins()
+        return ResetResponse(to_bbox_msg(bbox))
+
+    def switch_controller(self, req):
+        for controller in req.stop_controllers:
+            self.controllers[controller].deactivate()
+        for controller in req.start_controllers:
+            self.controllers[controller].activate()
+        return SwitchControllerResponse(ok=True)
+
+    def run(self):
+        self.start_plugins()
+        self.activate_plugins()
+        rospy.spin()
+
 
 class Plugin:
     """A plugin that spins at a constant rate in its own thread."""
@@ -134,6 +161,11 @@ class CartesianVelocityControllerPlugin(Plugin):
         self.dx_d = np.zeros(6)
         self.is_running = True
 
+    def deactivate(self):
+        self.dx_d = np.zeros(6)
+        self.is_running = False
+        self.arm.set_desired_joint_velocities(np.zeros(7))
+
     def update(self):
         q, _ = self.arm.get_state()
         J_pinv = np.linalg.pinv(self.model.jacobian(q))
@@ -141,6 +173,41 @@ class CartesianVelocityControllerPlugin(Plugin):
         self.arm.set_desired_joint_velocities(cmd)
 
 
+class JointTrajectoryControllerPlugin(Plugin):
+    def __init__(self, arm, rate=30):
+        super().__init__(rate)
+        self.arm = arm
+        self.dt = 1.0 / self.rate
+        self.init_action_server()
+
+    def init_action_server(self):
+        name = "position_joint_trajectory_controller/follow_joint_trajectory"
+        self.action_server = SimpleActionServer(
+            name,
+            control_msgs.msg.FollowJointTrajectoryAction,
+            auto_start=False,
+        )
+        self.action_server.register_goal_callback(self.action_goal_cb)
+        self.action_server.start()
+
+    def action_goal_cb(self):
+        goal = self.action_server.accept_new_goal()
+        self.elapsed_time = 0.0
+        self.points = iter(goal.trajectory.points)
+        self.next_point = next(self.points)
+
+    def update(self):
+        if self.action_server.is_active():
+            self.elapsed_time += self.dt
+            if self.elapsed_time > self.next_point.time_from_start.to_sec():
+                try:
+                    self.next_point = next(self.points)
+                except StopIteration:
+                    self.action_server.set_succeeded()
+                    return
+            self.arm.set_desired_joint_positions(self.next_point.positions)
+
+
 class MoveActionPlugin(Plugin):
     def __init__(self, gripper, rate=10):
         super().__init__(rate)
@@ -191,6 +258,29 @@ class GraspActionPlugin(Plugin):
                 self.action_server.set_succeeded()
 
 
+class GripperActionPlugin(Plugin):
+    """Empty action server to make MoveIt happy"""
+
+    def __init__(self, rate=1):
+        super().__init__(rate)
+        self.init_action_server()
+
+    def init_action_server(self):
+        name = "/franka_gripper/gripper_action"
+        self.action_server = SimpleActionServer(
+            name, control_msgs.msg.GripperCommandAction, auto_start=False
+        )
+        self.action_server.register_goal_callback(self.action_goal_cb)
+        self.action_server.start()
+
+    def action_goal_cb(self):
+        self.action_server.accept_new_goal()
+
+    def update(self):
+        if self.action_server.is_active():
+            self.action_server.set_succeeded()
+
+
 class CameraPlugin(Plugin):
     def __init__(self, camera, name="camera", rate=5):
         super().__init__(rate)