Simulate joint trajectory and move gripper interfaces

bt_sim_node.py

@@ -6,6 +6,8 @@ import cv_bridge
 import numpy as np
 import rospy
 
+import control_msgs.msg
+import controller_manager_msgs.srv
 import franka_gripper.msg
 from geometry_msgs.msg import Pose
 from sensor_msgs.msg import JointState, Image, CameraInfo
@@ -13,25 +15,25 @@ import std_srvs.srv
 
 from robot_tools.controllers import CartesianPoseController
 from robot_tools.ros.conversions import *
-from robot_tools.ros.tf import TransformTree
+from robot_tools.ros.tf import TF2Client
 from simulation import Simulation
 
 
 class BtSimNode:
     def __init__(self, gui):
         self.sim = Simulation(gui=gui, sleep=False)
-        self.controller = CartesianPoseController(self.sim.arm)
 
         self.controller_update_rate = 60
         self.joint_state_publish_rate = 60
         self.camera_publish_rate = 5
 
         self.cv_bridge = cv_bridge.CvBridge()
-        self.tf_tree = TransformTree()
+        self.tf_tree = TF2Client()
 
         self.setup_robot_topics()
         self.setup_camera_topics()
-        self.setup_gripper_actions()
+        self.setup_controllers()
+        self.setup_gripper_interface()
         self.broadcast_transforms()
 
         rospy.Service("reset", std_srvs.srv.Trigger, self.reset)
@@ -40,10 +42,18 @@ class BtSimNode:
 
     def setup_robot_topics(self):
         self.joint_pub = rospy.Publisher("joint_states", JointState, queue_size=10)
-        rospy.Subscriber("cmd", Pose, self.target_pose_cb)
+
+    def setup_controllers(self):
+        self.cartesian_pose_controller = CartesianPoseController(
+            self.sim.arm, stopped=False
+        )
+        rospy.Subscriber("command", Pose, self.target_pose_cb)
 
     def target_pose_cb(self, msg):
-        self.controller.set_target(from_pose_msg(msg))
+        self.cartesian_pose_controller.set_target(from_pose_msg(msg))
+
+    def setup_gripper_interface(self):
+        self.gripper_interface = GripperInterface(self.sim.gripper)
 
     def setup_camera_topics(self):
         self.cam_info_msg = to_camera_info_msg(self.sim.camera.intrinsic)
@@ -55,19 +65,6 @@ class BtSimNode:
         )
         self.depth_pub = rospy.Publisher("/cam/depth/image_raw", Image, queue_size=10)
 
-    def setup_gripper_actions(self):
-        self.move_server = actionlib.SimpleActionServer(
-            "/franka_gripper/move",
-            franka_gripper.msg.MoveAction,
-            self.move,
-            False,
-        )
-        self.move_server.start()
-
-    def move(self, goal):
-        self.sim.gripper.move(goal.width)
-        self.move_server.set_succeeded()
-
     def broadcast_transforms(self):
         msgs = []
         msg = geometry_msgs.msg.TransformStamped()
@@ -91,7 +88,7 @@ class BtSimNode:
         self.reset_requested = True
         rospy.sleep(1.0)  # wait for the latest sim step to finish
         self.sim.reset()
-        self.controller.set_target(self.sim.arm.pose())
+        self.cartesian_pose_controller.set_target(self.sim.arm.pose())
         self.step_cnt = 0
         self.reset_requested = False
         return std_srvs.srv.TriggerResponse(success=True)
@@ -107,7 +104,8 @@ class BtSimNode:
 
     def handle_updates(self):
         if self.step_cnt % int(self.sim.rate / self.controller_update_rate) == 0:
-            self.controller.update()
+            self.cartesian_pose_controller.update()
+            self.gripper_interface.update(1.0 / 60.0)
         if self.step_cnt % int(self.sim.rate / self.joint_state_publish_rate) == 0:
             self.publish_joint_state()
         if self.step_cnt % int(self.sim.rate / self.camera_publish_rate) == 0:
@@ -138,14 +136,86 @@ class BtSimNode:
         self.depth_pub.publish(depth_msg)
 
 
-def main(args):
+class GripperInterface:
+    def __init__(self, gripper):
+        self.gripper = gripper
+        self.move_server = actionlib.SimpleActionServer(
+            "/franka_gripper/move",
+            franka_gripper.msg.MoveAction,
+            auto_start=False,
+        )
+        self.move_server.register_goal_callback(self.goal_cb)
+        self.move_server.start()
+
+    def goal_cb(self):
+        goal = self.move_server.accept_new_goal()
+        self.gripper.move(goal.width)
+        self.elapsed_time = 0.0
+
+    def update(self, dt):
+        if self.move_server.is_active():
+            self.elapsed_time += dt
+            if self.elapsed_time > 1.0:
+                self.move_server.set_succeeded()
+
+
+class JointTrajectoryController:
+    def __init__(self, arm, stopped=False):
+        self.arm = arm
+        self.action_server = actionlib.SimpleActionServer(
+            "position_joint_trajectory_controller/follow_joint_trajectory",
+            control_msgs.msg.FollowJointTrajectoryAction,
+            auto_start=False,
+        )
+        self.action_server.register_goal_callback(self.goal_cb)
+        self.action_server.start()
+
+        self.is_running = False
+        if not stopped:
+            self.start()
+
+    def 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 start(self):
+        self.is_running = True
+
+    def stop(self):
+        self.is_running = False
+
+    def update(self, dt):
+        if not (self.is_running and self.action_server.is_active()):
+            return
+
+        self.elapsed_time += 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)
+
+
+def create_parser():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--gui", action="store_true")
+    return parser
+
+
+def main():
     rospy.init_node("bt_sim")
+
+    parser = create_parser()
+    args, _ = parser.parse_known_args()
+
     server = BtSimNode(args.gui)
     server.run()
 
 
 if __name__ == "__main__":
-    parser = argparse.ArgumentParser()
+    main()
-    parser.add_argument("--gui", action="store_true")
-    args, _ = parser.parse_known_args()
-    main(args)

launch/simulation.launch

@@ -11,6 +11,9 @@
   <node pkg="active_grasp" type="bt_sim_node.py" name="simulation" args="--gui" output="screen" />
   <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
 
+  <!-- Launch MoveIt -->
+  <!-- <include file="$(find panda_moveit_config)/launch/move_group.launch" /> -->
+
   <!-- Visualize the robot. -->
   <node pkg="rviz" type="rviz" name="rviz" args="-d $(find active_grasp)/launch/panda_visualization.rviz" if="$(arg launch_rviz)" />
 </launch>

policies.py

@@ -10,8 +10,9 @@ import std_srvs.srv
 
 from robot_tools.spatial import Rotation, Transform
 from robot_tools.ros.conversions import *
+from robot_tools.ros.control import ControllerManagerClient
 from robot_tools.ros.panda import PandaGripperClient
-from robot_tools.ros.tf import TransformTree
+from robot_tools.ros.tf import TF2Client
 from robot_tools.perception import *
 from vgn import vis
 from vgn.detection import VGN, compute_grasps
@@ -34,7 +35,7 @@ class BaseController:
         self.length = rospy.get_param("~length")
 
         self.cv_bridge = cv_bridge.CvBridge()
-        self.tf = TransformTree()
+        self.tf = TF2Client()
 
         self.reset_client = rospy.ServiceProxy("/reset", std_srvs.srv.Trigger)
 
@@ -49,9 +50,12 @@ class BaseController:
         self.base_frame_id = rospy.get_param("~base_frame_id")
         self.ee_frame_id = rospy.get_param("~ee_frame_id")
         self.ee_grasp_offset = Transform.from_list(rospy.get_param("~ee_grasp_offset"))
-        self.target_pose_pub = rospy.Publisher("/cmd", Pose, queue_size=10)
+        self.target_pose_pub = rospy.Publisher("/command", Pose, queue_size=10)
         self.gripper = PandaGripperClient()
 
+    def send_pose_command(self, target):
+        self.target_pose_pub.publish(to_pose_msg(target))
+
     def setup_camera_connection(self):
         self.cam_frame_id = rospy.get_param("~camera/frame_id")
         info_topic = rospy.get_param("~camera/info_topic")
@@ -78,6 +82,7 @@ class BaseController:
         self.execute_grasp()
 
     def reset(self):
+        vis.clear()
         req = std_srvs.srv.TriggerRequest()
         self.reset_client(req)
         rospy.sleep(1.0)  # wait for states to be updated
@@ -104,13 +109,11 @@ class BaseController:
         target = self.T_B_O * grasp.pose * self.ee_grasp_offset.inv()
 
         self.gripper.move(0.08)
-        rospy.sleep(1.0)  # TODO properly implement the simulated move server
+        self.send_pose_command(target)
-        self.target_pose_pub.publish(to_pose_msg(target))
+        rospy.sleep(3.0)
-        rospy.sleep(2.0)
         self.gripper.move(0.0)
-        rospy.sleep(1.0)  # TODO
         target.translation[2] += 0.3
-        self.target_pose_pub.publish(to_pose_msg(target))
+        self.send_pose_command(target)
         rospy.sleep(2.0)
 
     def predict_best_grasp(self):
@@ -176,8 +179,30 @@ class FixedTrajectoryBaseline(BaseController):
                 self.center
                 + np.r_[self.radius * np.cos(t), self.radius * np.sin(t), 0.0]
             )
-            self.target_pose_pub.publish(to_pose_msg(self.target))
+            self.send_pose_command(self.target)
+
+
+class Map:
+    def __init__(self):
+        pass
+
+    def update(self):
+        pass
 
 
 class MultiViewPicking(BaseController):
+    def __init__(self):
+        super().__init__()
+        self.rate = 5
+        self.grid = np.zeros((40, 40, 40))
+
+    def reset(self):
+        super().reset()
+        self.tic = rospy.Time.now()
+        timeout = rospy.Duration(0.1)
+        x0 = self.tf.lookup(self.base_frame_id, self.ee_frame_id, self.tic, timeout)
+        self.center = np.r_[x0.translation[0] + self.radius, x0.translation[1:]]
+        self.target = x0
+
+    def update(self):
         pass

simulation.py

@@ -27,7 +27,7 @@ class Simulation(BtManipulationSim):
         ori = Rotation.from_rotvec(np.array([0, 0, np.pi / 2])).as_quat()
         p.loadURDF("table/table.urdf", baseOrientation=ori, useFixedBase=True)
         self.length = 0.3
-        self.origin = [-0.2, -0.5 * self.length, 0.5]
+        self.origin = [-0.3, -0.5 * self.length, 0.5]
 
     def add_robot(self):
         self.T_W_B = Transform(Rotation.identity(), np.r_[-0.6, 0.0, 0.5])
@@ -39,7 +39,8 @@ class Simulation(BtManipulationSim):
         self.remove_all_objects()
         self.set_initial_arm_configuration()
         urdfs = np.random.choice(self.urdfs, 4)
-        self.add_random_arrangement(urdfs, np.r_[self.origin[:2], 0.625], self.length)
+        origin = np.r_[self.origin[:2], 0.625]
+        self.add_random_arrangement(urdfs, origin, self.length, 0.8)
 
     def set_initial_arm_configuration(self):
         q = self.arm.configurations["ready"]