From f982d110dc491aa64813bf92583d8df5008caf9b Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Fri, 18 Oct 2019 06:46:25 +0200 Subject: [PATCH] update ROS examples: add Kuka and Panda --- CITATION.rst | 2 +- examples/middlewares/README.md | 20 +++++ .../middlewares/bullet_ros_control_gazebo.py | 11 ++- pyrobolearn/robots/urdfs/franka/default.yaml | 13 +++ .../robots/urdfs/franka/franka_gazebo.launch | 48 +++++++++++ .../robots/urdfs/franka/gripper_commands.yaml | 7 ++ .../robots/urdfs/franka/joint_commands.yaml | 33 ++++++++ .../robots/urdfs/franka/joint_states.yaml | 6 ++ .../urdfs/kuka/kuka_iiwa/iiwa_control.launch | 31 +++++++ ...ka_iiwa_control.yaml => iiwa_control.yaml} | 41 ++++++++- .../urdfs/kuka/kuka_iiwa/iiwa_gazebo.launch | 48 +++++++++++ .../robots/urdfs/rrbot/rrbot_control.launch | 22 +++++ .../robots/urdfs/rrbot/rrbot_gazebo.launch | 34 ++++++++ pyrobolearn/simulators/middlewares/ros.py | 83 +++++++++++++------ .../simulators/middlewares/ros_subscriber.py | 42 ++++++++-- 15 files changed, 404 insertions(+), 37 deletions(-) create mode 100644 examples/middlewares/README.md create mode 100644 pyrobolearn/robots/urdfs/franka/default.yaml create mode 100644 pyrobolearn/robots/urdfs/franka/franka_gazebo.launch create mode 100644 pyrobolearn/robots/urdfs/franka/gripper_commands.yaml create mode 100644 pyrobolearn/robots/urdfs/franka/joint_commands.yaml create mode 100644 pyrobolearn/robots/urdfs/franka/joint_states.yaml create mode 100644 pyrobolearn/robots/urdfs/kuka/kuka_iiwa/iiwa_control.launch rename pyrobolearn/robots/urdfs/kuka/kuka_iiwa/{kuka_iiwa_control.yaml => iiwa_control.yaml} (83%) create mode 100644 pyrobolearn/robots/urdfs/kuka/kuka_iiwa/iiwa_gazebo.launch create mode 100644 pyrobolearn/robots/urdfs/rrbot/rrbot_control.launch create mode 100644 pyrobolearn/robots/urdfs/rrbot/rrbot_gazebo.launch diff --git a/CITATION.rst b/CITATION.rst index a0ba661..4674235 100644 --- a/CITATION.rst +++ b/CITATION.rst @@ -4,7 +4,7 @@ Citation .. code-block:: latex @misc{delhaisse2019pyrobolearn, - author = {Delhaisse, Brian and Xin, Songyan and Rozo, Leonel, and Caldwell, Darwin}, + author = {Delhaisse, Brian and Rozo, Leonel, and Caldwell, Darwin}, title = {PyRoboLearn: A Python Framework for Robot Learning Practitioners}, publisher = {GitHub}, journal = {GitHub repository}, diff --git a/examples/middlewares/README.md b/examples/middlewares/README.md new file mode 100644 index 0000000..76c7d70 --- /dev/null +++ b/examples/middlewares/README.md @@ -0,0 +1,20 @@ +## Middleware examples + +In this folder, you will have simple examples on how to use the ROS middleware with PyRoboLearn. Once this middleware +is instantiated, it is passed to the simulator that uses it to publish or subscribe to topics/services to get the +various joint states, sensor values, etc. The ROS middleware can then later be used to launch ROS nodes and other +ROS features. The ROS middleware can also be useful to get the data from a real robotic platform or to send some +joint trajectories to it. + +Here are the few examples that you can find in this folder: +1. `bullet_ros_control_gazebo.py`: After running the corresponding roslaunch file (see file documentation), you will +be able to teleoperate the manipulator (rrbot, kuka, or franka emika panda) in Gazebo by moving the same robot in +PyBullet. The robots that are instantiated in Gazebo use position control (by using `ros_control`). A simple video +demonstrating the results can be found here: https://www.youtube.com/watch?v=OPh-NCfKKK8 +2. `bullet_ros_rqt.py`: this will launch RQT along PRL. +3. `bullet_ros_publisher.py`: example where we use ROS to publish the joint position values that were returned by +the Bullet simulator on the corresponding ROS topic. +4. `bullet_ros_subscriber.py`: example where we use ROS to get the joint values from the ROS topics and change them +in the simulator. This works with the `bullet_ros_publisher.py` code presented above. By moving the robot with your +mouse in the publisher version, you will see the robot in this subscriber version moving accordingly. This can be +useful if you have access to the real platform as well. diff --git a/examples/middlewares/bullet_ros_control_gazebo.py b/examples/middlewares/bullet_ros_control_gazebo.py index aa7a1c4..8eca026 100644 --- a/examples/middlewares/bullet_ros_control_gazebo.py +++ b/examples/middlewares/bullet_ros_control_gazebo.py @@ -20,7 +20,14 @@ $ python bullet_ros_control_gazebo.py And move the `rrbot` robot using your mouse by left-clicking on a part of the robot and moving it. -Here is a video of what it should give: https://www.youtube.com/watch?v=NOybmaRHaBM +Here is a video of what it should give: https://www.youtube.com/watch?v=OPh-NCfKKK8 + +If you want to use 'kuka_iiwa' and 'franka', you will have to follow the same steps as above but this time by cloning: +- https://github.com/IFL-CAMP/iiwa_stack +- https://github.com/mkrizmancic/franka_gazebo + +Then run the corresponding roslaunch files (*_gazebo.launch) that are located in the corresponding +`pyrobolearn/robots/urdfs//` folder using the `roslaunch _gazebo.launch` command. """ import pyrobolearn as prl @@ -32,7 +39,7 @@ sim = prl.simulators.Bullet(middleware=ros) world = prl.worlds.BasicWorld(sim) # load robot -robot = world.load_robot('rrbot') +robot = world.load_robot('rrbot') # 'kuka_iiwa', 'franka' # run simulation for t in prl.count(): diff --git a/pyrobolearn/robots/urdfs/franka/default.yaml b/pyrobolearn/robots/urdfs/franka/default.yaml new file mode 100644 index 0000000..15b5fa9 --- /dev/null +++ b/pyrobolearn/robots/urdfs/franka/default.yaml @@ -0,0 +1,13 @@ +# this file comes from https://github.com/mkrizmancic/franka_gazebo, and is provided here just in case + +gazebo_ros_control: + pid_gains: + panda_joint1: {p: 50, i: 0, d: 20} + panda_joint2: {p: 10000, i: 0, d: 500} + panda_joint3: {p: 100, i: 0, d: 10} + panda_joint4: {p: 1000, i: 0, d: 50} + panda_joint5: {p: 100, i: 0, d: 10} + panda_joint6: {p: 100, i: 0, d: 10} + panda_joint7: {p: 10, i: 0.5, d: 0.1} + panda_finger_joint1: {p: 100, i: 0, d: 50} + panda_finger_joint2: {p: 100, i: 0, d: 50} \ No newline at end of file diff --git a/pyrobolearn/robots/urdfs/franka/franka_gazebo.launch b/pyrobolearn/robots/urdfs/franka/franka_gazebo.launch new file mode 100644 index 0000000..010f55a --- /dev/null +++ b/pyrobolearn/robots/urdfs/franka/franka_gazebo.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/pyrobolearn/robots/urdfs/franka/gripper_commands.yaml b/pyrobolearn/robots/urdfs/franka/gripper_commands.yaml new file mode 100644 index 0000000..0afc97b --- /dev/null +++ b/pyrobolearn/robots/urdfs/franka/gripper_commands.yaml @@ -0,0 +1,7 @@ +# this file comes from https://github.com/mkrizmancic/franka_gazebo, and is provided here just in case + +gripper_position_controller: + type: "position_controllers/JointGroupPositionController" + joints: + - panda_finger_joint1 + - panda_finger_joint2 diff --git a/pyrobolearn/robots/urdfs/franka/joint_commands.yaml b/pyrobolearn/robots/urdfs/franka/joint_commands.yaml new file mode 100644 index 0000000..df8909c --- /dev/null +++ b/pyrobolearn/robots/urdfs/franka/joint_commands.yaml @@ -0,0 +1,33 @@ +# this file comes from https://github.com/mkrizmancic/franka_gazebo, and is provided here just in case + +joint1_position_controller: + type: "position_controllers/JointPositionController" + joint: panda_joint1 + +joint2_position_controller: + type: "position_controllers/JointPositionController" + joint: panda_joint2 + +joint3_position_controller: + type: "position_controllers/JointPositionController" + joint: panda_joint3 + +joint4_position_controller: + type: "position_controllers/JointPositionController" + joint: panda_joint4 + +joint5_position_controller: + type: "position_controllers/JointPositionController" + joint: panda_joint5 + +joint6_position_controller: + type: "position_controllers/JointPositionController" + joint: panda_joint6 + +joint7_position_controller: + type: "position_controllers/JointPositionController" + joint: panda_joint7 + +joint7_effort_controller: + type: "effort_controllers/JointEffortController" + joint: panda_joint7 \ No newline at end of file diff --git a/pyrobolearn/robots/urdfs/franka/joint_states.yaml b/pyrobolearn/robots/urdfs/franka/joint_states.yaml new file mode 100644 index 0000000..2937718 --- /dev/null +++ b/pyrobolearn/robots/urdfs/franka/joint_states.yaml @@ -0,0 +1,6 @@ +# this file comes from https://github.com/mkrizmancic/franka_gazebo, and is provided here just in case + +# The joint state controller handles publishing transforms for any moving joints +joint_state_controller: + type: "joint_state_controller/JointStateController" + publish_rate: 50 \ No newline at end of file diff --git a/pyrobolearn/robots/urdfs/kuka/kuka_iiwa/iiwa_control.launch b/pyrobolearn/robots/urdfs/kuka/kuka_iiwa/iiwa_control.launch new file mode 100644 index 0000000..e993e12 --- /dev/null +++ b/pyrobolearn/robots/urdfs/kuka/kuka_iiwa/iiwa_control.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/pyrobolearn/robots/urdfs/kuka/kuka_iiwa/kuka_iiwa_control.yaml b/pyrobolearn/robots/urdfs/kuka/kuka_iiwa/iiwa_control.yaml similarity index 83% rename from pyrobolearn/robots/urdfs/kuka/kuka_iiwa/kuka_iiwa_control.yaml rename to pyrobolearn/robots/urdfs/kuka/kuka_iiwa/iiwa_control.yaml index 745256a..70e8f57 100644 --- a/pyrobolearn/robots/urdfs/kuka/kuka_iiwa/kuka_iiwa_control.yaml +++ b/pyrobolearn/robots/urdfs/kuka/kuka_iiwa/iiwa_control.yaml @@ -1,8 +1,47 @@ +# This file comes originally from https://github.com/IFL-CAMP/iiwa_stack, and has been modified by Brian Delhaisse to comply with PRL + #iiwa: # Publish all joint states ----------------------------------- joint_state_controller: type: joint_state_controller/JointStateController - publish_rate: 50 + publish_rate: 50 + + # positions controllers (added by Brian) + + joint1_position_controller: + type: position_controllers/JointPositionController + joint: iiwa14_joint_1 + #pid: {p: 800.0, i: 100, d: 80.0} + + joint2_position_controller: + type: position_controllers/JointPositionController + joint: iiwa14_joint_2 + #pid: {p: 800.0, i: 1000, d: 100.0} + + joint3_position_controller: + type: position_controllers/JointPositionController + joint: iiwa14_joint_3 + #pid: {p: 800.0, i: 10, d: 5.0} + + joint4_position_controller: + type: position_controllers/JointPositionController + joint: iiwa14_joint_4 + #pid: {p: 800.0, i: 10, d: 80.0} + + joint5_position_controller: + type: position_controllers/JointPositionController + joint: iiwa14_joint_5 + #pid: {p: 300.0, i: 5, d: 10.0} + + joint6_position_controller: + type: position_controllers/JointPositionController + joint: iiwa14_joint_6 + #pid: {p: 50.0, i: 1, d: 1.0} + + joint7_position_controller: + type: position_controllers/JointPositionController + joint: iiwa14_joint_7 + #pid: {p: 10.0, i: 0.1, d: 1.0} # Controllers for singular joint ------------------------------------ # diff --git a/pyrobolearn/robots/urdfs/kuka/kuka_iiwa/iiwa_gazebo.launch b/pyrobolearn/robots/urdfs/kuka/kuka_iiwa/iiwa_gazebo.launch new file mode 100644 index 0000000..e55663b --- /dev/null +++ b/pyrobolearn/robots/urdfs/kuka/kuka_iiwa/iiwa_gazebo.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/pyrobolearn/robots/urdfs/rrbot/rrbot_control.launch b/pyrobolearn/robots/urdfs/rrbot/rrbot_control.launch new file mode 100644 index 0000000..10e6221 --- /dev/null +++ b/pyrobolearn/robots/urdfs/rrbot/rrbot_control.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + diff --git a/pyrobolearn/robots/urdfs/rrbot/rrbot_gazebo.launch b/pyrobolearn/robots/urdfs/rrbot/rrbot_gazebo.launch new file mode 100644 index 0000000..ccd2f68 --- /dev/null +++ b/pyrobolearn/robots/urdfs/rrbot/rrbot_gazebo.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/pyrobolearn/simulators/middlewares/ros.py b/pyrobolearn/simulators/middlewares/ros.py index aea5b6d..de8b4d6 100644 --- a/pyrobolearn/simulators/middlewares/ros.py +++ b/pyrobolearn/simulators/middlewares/ros.py @@ -54,6 +54,7 @@ import std_msgs.msg as std_msg import sensor_msgs.msg as sensor_msg import gazebo_msgs.msg as gazebo_msg import geometry_msgs.msg as geometry_msg +import trajectory_msgs.msg as trajectory_msg from pyrobolearn.simulators.middlewares.middleware import MiddleWare from pyrobolearn.simulators.middlewares.ros_publisher import PublisherData, Publisher, RobotPublisher @@ -76,39 +77,51 @@ class Remapper(object): """Remapper from old topic to a new topic. """ - def __init__(self, old_topic, new_topic, data_class, queue_size=10): + def __init__(self, old_topic, new_topic, msg_class, queue_size=10, new_msg_class=None, function=None): """ Initialize the Remapper that subscribes to the given topic. Args: - old_topic (str): old topic name - new_topic (str): new topic name - data_class (class): message class for serialization. + old_topic (str, list[str]): old topic name(s). If multiple topics are provided, it will group them. Note + that you can only group topics that use the same message class. + new_topic (str, list[str]): new topic name(s). If multiple topics are provided, it will group them and send + to each of the new topic the (converted) data. + msg_class (class): message class for serialization. If `new_msg_class` is provided, then `msg_class` + represents the message class used in the old topic(s). queue_size (int): The queue size used for asynchronously publishing messages from different threads. A size of zero means an infinite queue, which can be dangerous. When None is passed all publishing will happen synchronously and a warning message will be printed. + new_msg_class (class, None): optional new message class for serialization. This is the message associated + with the new topic(s). + function (callable, None): optional function that should accept the old message object and process it + and/or convert it into the new one. """ - # self.subscriber = rospy.Subscriber(topic, data_class, callback=self.callback) - # # set the message attributes to be part of this class attributes - # self.attributes = set([attr for attr in [attr for attr in dir(data_class) if not attr.startswith('_')] - # if not callable(getattr(data_class, attr))]) - # self.subscriber_data = data_class() - rospy.init_node("remapper", anonymous=True) + # create subscriber and publisher self.old_topic = old_topic self.new_topic = new_topic - self.subscriber = rospy.Subscriber(old_topic, data_class, callback=self.callback) - self.publisher = rospy.Publisher(new_topic, data_class, queue_size=queue_size) - self.msg = data_class() + self.subscriber = rospy.Subscriber(old_topic, msg_class, callback=self.callback) + if new_msg_class is None: + new_msg_class = msg_class + self.publisher = rospy.Publisher(new_topic, new_msg_class, queue_size=queue_size) + # self.msg = msg_class() + + # check processing function + if function is not None and not callable(function): + raise TypeError("Expecting the given 'function' to be callable...") + self.function = function + # TODO: should I test the function to make sure it returns the correct object type? def callback(self, data): """ Callback function that publishes the received the data from the old topic to the new topic. Args: - data (object): message class instance. + data (object, list[object]): message class instance. """ + if self.function is not None: + data = self.function(data) self.publisher.publish(data) def unregister(self): @@ -164,26 +177,30 @@ class RobotMiddleWare(object): self.is_publishing = publish self.is_teleoperating = teleoperate + if self.is_teleoperating and self.is_publishing and self.is_subscribing: + raise ValueError("The three following arguments 'subscribe', 'publish', and 'teleoperate' can not be all " + "true at the same time. Select maximum two.") + print("\n Creating RobotMiddleware") - # subscriber and publisher associated with the given robot - if self.is_subscribing: - print("Creating Robot Subscriber") - self.subscriber = RobotSubscriber(name='robot' + str(robot_id)) - if self.is_publishing: - print("Creating Robot Publisher") - self.publisher = RobotPublisher(name='robot' + str(robot_id)) - - # 1. check if topics and services related to the loaded robot (such as joint_states, joint_commands, etc) are - # already advertised, if yes it will create the publishers/subscribers to these topics which would allow - # the `Simulator` instance to write/read the values on/from these topics - # get path to the URDF folder path = os.path.abspath(urdf) # /path/to/pyrobolearn/robots/urdfs//robot.urdf dirname = str(os.path.dirname(path)) # /path/to/pyrobolearn/robots/urdfs// basename = str(os.path.basename(path).split('.')[-2]) # robot name without extension config_file = dirname + basename + ".yaml" + # subscriber and publisher associated with the given robot + if self.is_subscribing: + print("Creating Robot Subscriber") + self.subscriber = RobotSubscriber(name=basename) + if self.is_publishing: + print("Creating Robot Publisher") + self.publisher = RobotPublisher(name=basename) + + # 1. check if topics and services related to the loaded robot (such as joint_states, joint_commands, etc) are + # already advertised, if yes it will create the publishers/subscribers to these topics which would allow + # the `Simulator` instance to write/read the values on/from these topics + # check for YAML control configuration file if os.path.isfile(config_file): # if it exists, import it @@ -1159,6 +1176,18 @@ class ROS(MiddleWare): # Robots # ########## + def get_robot_middleware(self, body_id): + """ + Return the robot middleware associated with the given body id. + + Args: + body_id (int): unique body id. + + Returns: + RobotMiddleware, None: robot middleware corresponding to the given body id. None if it could not find it. + """ + return self._robots.get(body_id) + def load_urdf(self, urdf): """Load the given URDF file. @@ -1177,7 +1206,7 @@ class ROS(MiddleWare): # check URDF path; check if it is a valid robot directory. If not a robot, just skip path = os.path.dirname(os.path.abspath(urdf)) # /path/to/pyrobolearn/robots/urdfs// robot_path = '/'.join(path.split('/')[-4:-1]) - if robot_path == 'pyrobolearn/robots/urdfs': + if 'pyrobolearn/robots/urdfs' in path: id_ = self.count_id self.count_id += 1 diff --git a/pyrobolearn/simulators/middlewares/ros_subscriber.py b/pyrobolearn/simulators/middlewares/ros_subscriber.py index a99da02..459cb3b 100644 --- a/pyrobolearn/simulators/middlewares/ros_subscriber.py +++ b/pyrobolearn/simulators/middlewares/ros_subscriber.py @@ -219,22 +219,52 @@ class RobotSubscriber(Subscriber): This Robot Subscriber class is the class from which all the robot subscribers inherit from. """ - def __init__(self, name, id_=None): + def __init__(self, name, id_=None, joint_state_topics=None, joint_state_msg_class=None): r""" Initialize the robot subscriber. Args: name (str): name of the robot. This will be used to create the topics. id_ (int, None): robot id which is used when initializing the node. If None, a name will be - auto-generated for the name using name as the base. See the documentation for the :attr:`anonymous` - parameter in `rospy.init_node`. + auto-generated for the name using name as the base. See the documentation for the :attr:`anonymous` + parameter in `rospy.init_node`. + joint_state_topics (str, list[str]): joint state topic(s). If not provided the joint state topic will be + set to '//joint_states'. + joint_state_msg_class (class): message serialization class used for the provided joint state topic. By + default, it will be set to 'sensor_msg.JointState'. """ super(RobotSubscriber, self).__init__(subscriber_id=id_) self.name = name.lower() - # create Joint states (automatically) - self.joint_states = self.create_subscriber('joint_states', '/' + self.name + '/joint_states', - sensor_msg.JointState) + # create Joint states subscriber (automatically) + if joint_state_topics is None: + joint_state_topics = '/' + self.name + '/joint_states' + if joint_state_msg_class is None: + joint_state_msg_class = sensor_msg.JointState + self._joint_states = self.create_subscriber('joint_states', joint_state_topics, joint_state_msg_class) + + ############## + # Properties # + ############## + + @property + def joint_states(self): + """Get the joint states subscriber.""" + return self._joint_states + + @joint_states.setter + def joint_states(self, subscriber): + """Set the joint states subscriber.""" + if not isinstance(subscriber, SubscriberData): + raise TypeError("Expecting the given 'joint_states' subscriber to be an instance of `SubscriberData` but " + "got instead: {}".format(type(subscriber))) + if self._joint_states is not None: + self._joint_states.unregister() + self._joint_states = subscriber + + ########### + # Methods # + ########### def get_joint_positions(self, q_indices=None): """