From e04361dd8c74e3bddff976dfab57073c8364daed Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Wed, 2 Jun 2021 10:54:52 +0200 Subject: [PATCH] Simulate joint trajectory and move gripper interfaces --- bt_sim_node.py | 122 ++++++++++++++++++++++++++++++--------- launch/simulation.launch | 3 + policies.py | 45 +++++++++++---- simulation.py | 5 +- 4 files changed, 137 insertions(+), 38 deletions(-) diff --git a/bt_sim_node.py b/bt_sim_node.py index 2c551e2..8a291bd 100755 --- a/bt_sim_node.py +++ b/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() - parser.add_argument("--gui", action="store_true") - args, _ = parser.parse_known_args() - main(args) + main() diff --git a/launch/simulation.launch b/launch/simulation.launch index 1444117..b61c993 100644 --- a/launch/simulation.launch +++ b/launch/simulation.launch @@ -11,6 +11,9 @@ + + + diff --git a/policies.py b/policies.py index f9ef74f..fdade02 100644 --- a/policies.py +++ b/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.target_pose_pub.publish(to_pose_msg(target)) - rospy.sleep(2.0) + self.send_pose_command(target) + rospy.sleep(3.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): - pass + 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 diff --git a/simulation.py b/simulation.py index da979e5..ed61f21 100644 --- a/simulation.py +++ b/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"]