update ROS examples: add Kuka and Panda

2026-09-09 11:31:38 +08:00 · 2019-10-18 06:46:25 +02:00
parent 68629ad9c2
commit f982d110dc
15 changed files with 404 additions and 37 deletions
@@ -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},
@@ -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.
@@ -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/<robot_name>/` folder using the `roslaunch <robot>_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():
@@ -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}
@@ -0,0 +1,48 @@
+<?xml version="1.0"?>
+<!-- This file comes from https://github.com/mkrizmancic/franka_gazebo, and has been modified by Brian Delhaisse. 
+     In order to use it, please first clone the repository above and compile it using catkin_make. -->
+<launch>
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(find franka_gazebo)/robots/panda_arm_hand.urdf.xacro"/>
+
+	<!-- GAZEBO arguments -->
+    <arg name="paused" default="false"/>
+    <arg name="use_sim_time" default="true"/>
+    <arg name="gui" default="true"/>
+    <arg name="headless" default="false"/>
+    <arg name="debug" default="false"/>
+    <arg name="load_gripper" default="false"/>
+
+    <!--launch GAZEBO with own world configuration -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->
+        <arg name="debug" value="$(arg debug)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="paused" value="$(arg paused)"/>
+        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+        <arg name="headless" value="$(arg headless)"/>
+    </include>
+
+	<node name="spawn" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda"/>
+
+	<rosparam command="load" file="$(find franka_gazebo)/config/joint_states.yaml" ns="panda"/>
+	<rosparam command="load" file="$(find franka_gazebo)/config/joint_commands.yaml" ns="panda"/>
+	<rosparam command="load" file="$(find franka_gazebo)/config/gripper_commands.yaml" ns="panda"/>
+	<rosparam command="load" file="$(find franka_gazebo)/config/default.yaml"/>
+
+	<!-- ROS control -->
+	<node name="franka_controller_spawner" pkg="controller_manager" type="spawner" args=
+		"panda/joint_state_controller
+		panda/joint1_position_controller
+		panda/joint2_position_controller
+		panda/joint3_position_controller
+		panda/joint4_position_controller
+		panda/joint5_position_controller
+		panda/joint6_position_controller
+		panda/joint7_position_controller
+		panda/gripper_position_controller"/>
+
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub"/> 
+
+  <node pkg="franka_gazebo" type="gripper_publisher.py" name="gripper_publisher"/>
+  <node pkg="franka_gazebo" type="example_joint_publisher.py" name="example_joint_publisher" output="screen"/>
+</launch>
@@ -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
@@ -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
@@ -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
@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<!-- This file comes from https://github.com/IFL-CAMP/iiwa_stack, and has been modified by Brian Delhaisse. 
+     In order to use it, please first clone the repository above and compile it using catkin_make. -->
+<launch>
+    
+    <!-- Launches the controllers according to the hardware interface selected -->
+    <!-- Everythings is spawned under a namespace with the same name as the robot's. -->
+    
+    <arg name="hardware_interface" default="PositionJointInterface"/>
+    <arg name="controllers" default="joint_state_controller pos_joint_trajectory_controller"/>
+    <arg name="robot_name" default="iiwa" />
+    <arg name="model" default="iiwa7" />
+    <arg name="joint_state_frequency" default="100" />
+    <arg name="robot_state_frequency" default="100" />
+    
+    <!-- Loads joint controller configurations from YAML file to parameter server -->
+    <rosparam file="iiwa_control.yaml" command="load" />
+    <param name="/$(arg robot_name)/joint_state_controller/publish_rate" value="$(arg joint_state_frequency)" />
+    
+    <!-- Loads the controllers -->
+    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+          output="screen" args="$(arg controllers)" />
+    
+    <!-- Converts joint states to TF transforms for rviz, etc -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
+          respawn="false" output="screen">
+        <remap from="joint_states" to="/$(arg robot_name)/joint_states" />
+        <param name="publish_frequency" value="$(arg robot_state_frequency)" />
+    </node>
+    
+</launch>
@@ -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 ------------------------------------
  # 
@@ -0,0 +1,48 @@
+<?xml version="1.0"?>
+<!-- This file comes from https://github.com/IFL-CAMP/iiwa_stack, and has been modified by Brian Delhaisse. 
+     In order to use it, please first clone the repository above and compile it using catkin_make. -->
+<launch>
+    
+    <!-- ===================================================================================== -->
+    <!-- |    Lauch file to start Gazebo with an IIWA using various controllers.	         | -->
+    
+    <!-- |    It allows to customize the name of the robot, for each robot                   | -->
+    <!-- |	  its topics will be under a nameespace with the same name as the robot's.       | -->
+    
+    <!-- |	  One can choose to have a joint trajectory controller or                        | -->
+    <!-- |    controllers for the single joints, using the "trajectory" argument.            | -->
+    <!-- ===================================================================================== -->
+    
+    <arg name="hardware_interface" default="PositionJointInterface" />
+    <arg name="robot_name" default="iiwa14" />
+    <arg name="model" default="iiwa14"/>
+    <arg name="trajectory" default="false"/>
+    
+    <!-- Loads the Gazebo world. -->
+    <include file="iiwa_world.launch">
+        <arg name="hardware_interface" value="$(arg hardware_interface)" />
+        <arg name="robot_name" value="$(arg robot_name)" />
+        <arg name="model" value="$(arg model)" />
+    </include>
+    
+    <!-- Spawn controllers - it uses an position controller for each joint -->
+    <group ns="$(arg robot_name)">
+        
+        <include file="iiwa_control.launch">
+            <arg name="hardware_interface" value="$(arg hardware_interface)" />
+            <arg name="controllers" value="joint_state_controller
+                 joint1_position_controller
+                 joint2_position_controller
+                 joint3_position_controller
+                 joint4_position_controller
+                 joint5_position_controller
+                 joint6_position_controller
+                 joint7_position_controller"/>
+            <arg name="robot_name" value="$(arg robot_name)" />
+            <arg name="model" value="$(arg model)" />
+        </include>
+        
+    </group>
+    
+    
+</launch>
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<!-- This file comes from https://github.com/ros-simulation/gazebo_ros_demos, and has been modified by Brian Delhaisse. 
+     In order to use it, please first clone the repository above and compile it using catkin_make. -->
+<launch>
+
+  <!-- Load joint controller configurations from YAML file to parameter server -->
+  <rosparam file="$(find rrbot_control)/config/rrbot_control.yaml" command="load"/>
+
+  <!-- load the controllers -->
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+    output="screen" ns="/rrbot" args="joint_state_controller
+                      joint1_position_controller
+                      joint2_position_controller"/>
+
+  <!-- convert joint states to TF transforms for rviz, etc -->
+  <!--node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
+    respawn="false" output="screen">
+    <remap from="/joint_states" to="/rrbot/joint_states" />
+  </node-->
+
+</launch>
+
@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<!-- This file comes from https://github.com/ros-simulation/gazebo_ros_demos, and has been modified by Brian Delhaisse. 
+     In order to use it, please first clone the repository above and compile it using catkin_make. -->
+<launch>
+
+  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
+  <arg name="paused" default="false"/>
+  <arg name="use_sim_time" default="true"/>
+  <arg name="gui" default="true"/>
+  <arg name="headless" default="false"/>
+  <arg name="debug" default="false"/>
+
+  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find rrbot_gazebo)/worlds/rrbot.world"/>
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+    <arg name="headless" value="$(arg headless)"/>
+  </include>
+
+  <!-- Load the URDF into the ROS Parameter Server -->
+  <param name="robot_description"
+    command="$(find xacro)/xacro --inorder '$(find rrbot_description)/urdf/rrbot.xacro'" />
+
+  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
+  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
+    args="-urdf -model rrbot -param robot_description"/>
+
+  <!-- ros_control rrbot launch file -->
+  <include file="rrbot_control.launch" />
+
+</launch>
@@ -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>/robot.urdf
        dirname = str(os.path.dirname(path))  # /path/to/pyrobolearn/robots/urdfs/<robot>/
        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>/
        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

@@ -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 '/<robot_name>/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):
        """