update middleware: add reset_joint_states and switch_mode (+test on real robot)

examples/imitation/demo.py

@@ -10,13 +10,15 @@ import pyrobolearn as prl
 joint_ids = None  # None for all the actuated joints, or you can select which joint you want to move; e.g. [0, 1, 2]
 num_basis = 20
 rate = 30
+use_real_robot = True
 
 # create middleware
-# ros = prl.middlewares.ROS()
+ros = prl.middlewares.ROS(subscribe=True, teleoperate=True)
 
 # create simulator
-sim = prl.simulators.Bullet()  # middleware=ros)
-# sim.disable_middleware()  # disable the middleware (get/set info only from/to simulation)
+sim = prl.simulators.Bullet(middleware=ros)
+if not use_real_robot:
+    sim.disable_middleware()  # disable the middleware (get/set info only from/to simulation)
 
 # create basic world (with gravity and floor)
 world = prl.worlds.BasicWorld(sim)
@@ -55,6 +57,7 @@ task = prl.tasks.ILTask(env, policy, interface=bridge, recorders=recorder)
 print("\nRecording phase: press `ctrl+r` to start/stop the recording. Once finished, press `shift+r`.")
 task.record(signal_from_interface=True)
 print("Recording phase: finished the recording!")
+sim.disable_middleware()  # disable the middleware (get/set info only from/to simulation)
 
 # train policy
 print("Training phase: training the policy...")
@@ -71,7 +74,10 @@ task.test(num_steps=rate*100, signal_from_interface=False)
 print("Reproduction phase: Policy tested!")
 
 # test policy on real robot
-print("Reproduction phase: test policy in reality...")
-# sim.enable_middleware()  # enable the real robot
-# task.test(num_steps=rate*100, signal_from_interface=False)
-print("Reproduction phase: Policy tested!")
+if use_real_robot:
+    input("Press Enter to move to the real robot experiment...")
+    sim.enable_middleware()  # enable the real robot
+    ros.switch_mode(subscribe=False, publish=True, teleoperate=True)
+    print("Reproduction phase: test policy in reality...")
+    task.test(num_steps=rate*100, signal_from_interface=False)
+    print("Reproduction phase: Policy tested!")

pyrobolearn/robots/gripper.py

@@ -74,14 +74,14 @@ class Gripper(Robot):
 
         Args:
             strength (float): scalar describing how much to increases the stiffness (the torques that are applied on
-                the joint fingers). If positive, it closes the gripper fingers. If negative, it opens the gripper
-                fingers.
-            point (np.array[float[3]], list of np.array[float[3]], None): attractor point(s) described in the specified frame.
-                If multiple points are specified, they have to match the number of fingers and will be used in the
-                same order. If None, it will grasp in a "natural" way (which is let to the user that has implemented
-                this method).
+              the joint fingers). If positive, it closes the gripper fingers. If negative, it opens the gripper
+              fingers.
+            point (np.array[float[3]], list of np.array[float[3]], None): attractor point(s) described in the specified
+              frame. If multiple points are specified, they have to match the number of fingers and will be used in the
+              same order. If None, it will grasp in a "natural" way (which is let to the user that has implemented
+              this method).
             frame (int): integer describing if the above given point is described in the world frame
-                (``Simulator.WORLD_FRAME``), or in the link frame of the gripper base (``Simulator.LINK_FRAME``).
+              (``Simulator.WORLD_FRAME``), or in the link frame of the gripper base (``Simulator.LINK_FRAME``).
         """
         pass
 

pyrobolearn/simulators/middlewares/README.rst

@@ -1,9 +1,9 @@
 Middlewares
 ===========
 
-THIS SECTION IS UNDER CONSTRUCTION
-
 This folder provides interfaces to the middlewares that are used in robotics (such as ROS, YARP, etc). All these
 classes inherit from the ``Middleware`` abstract class. Middlewares can be provided to simulators which can then use
 them to send/receive messages. This allows to communicate with real platforms as well.
 
+The Middleware has a list of RobotMiddleware, where each one specifies how to communicate with the robot middleware.
+

pyrobolearn/simulators/middlewares/__init__.py

@@ -11,7 +11,11 @@ try:
     import rosmsg
     import rosservice
     import rostopic
-    import controller_manager.controller_manager_interface as cm_interface
+    try:
+        import controller_manager.controller_manager_interface as cm_interface
+    except ImportError as e:
+        print("ROS control is not installed for this Python version, please install it... For now, disabling the "
+              "ROS control module... Calling methods that use the controller mananger will fail...")
 
     from .ros import ROS
 except ImportError as e:

pyrobolearn/simulators/middlewares/middleware.py

@@ -21,6 +21,23 @@ class Middleware(object):
     r"""Middleware (abstract) class
 
     Middleware can be provided to simulators which can then use them to send/receive messages.
+
+    Here are the possible combinations between the different values for subscribe (S), publish (P), teleoperate (T),
+    and command (C):
+
+    - S=1, P=0, T=0: subscribes to the topics, and get the messages when calling the corresponding getter methods.
+    - S=0, P=1, T=0: publishes the various messages to the topics when calling the corresponding setter methods.
+    - S=1, P=0, T=1, C=0/1: get messages by subscribing to the topics that publish some commands/states. The
+      received commands/states are then set in the simulator. Depending on the value of `C`, it will subscribe to
+      topics that publish some commands (C=1) or states (C=0). Example: should we subscribe to joint trajectory
+      commands, or joint states when teleoperating the robot in the simulator? This C value allows to specify
+      which one we are interested in.
+    - S=0, P=1, T=1: when calling the getters methods such as joint positions, velocities, and others, it also
+      publishes the joint states. This is useful if we are moving/teleoperating the robot in the simulator.
+    - S=0, P=0, T=1/0: doesn't do anything.
+    - S=1, P=1, T=0: subscribes to some topics and publish messages to other topics. The messages are can be
+      sent/received by calling the appropriate getter/setter methods.
+    - S=1, P=1, T=1: not allowed, because teleoperating the robot is not a two-way communication process.
     """
 
     def __init__(self, subscribe=False, publish=False, teleoperate=False, command=True):
@@ -33,14 +50,16 @@ class Middleware(object):
             publish (bool): if True, it will publish the given values to the topics associated to the loaded robots.
             teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2
               previous attributes :attr:`subscribe` and :attr:`publish`.
-            command (bool): if True, it will subscribe/publish to some (joint) commands. If False, it will
-              subscribe/publish to some (joint) states.
+            command (bool): if True, it will subscribe to the joint commands. If False, it will subscribe to the
+              joint states.
         """
         # set variables
-        self.is_subscribing = subscribe
-        self.is_publishing = publish
-        self.is_teleoperating = teleoperate
-        self.is_commanding = command
+        # self.is_subscribing = subscribe
+        # self.is_publishing = publish
+        # self.is_teleoperating = teleoperate
+        # self.is_commanding = command
+        self._subscribe, self._publish, self._teleoperate, self._command = False, False, False, False
+        self.switch_mode(subscribe=subscribe, publish=publish, teleoperate=teleoperate, command=command)
 
         self._robots = {}  # {body_id: RobotMiddleware}
 
@@ -52,33 +71,39 @@ class Middleware(object):
     def is_subscribing(self):
         return self._subscribe
 
-    @is_subscribing.setter
-    def is_subscribing(self, subscribe):
-        self._subscribe = bool(subscribe)
+    # @is_subscribing.setter
+    # def is_subscribing(self, subscribe):
+    #     self._subscribe = bool(subscribe)
 
     @property
     def is_publishing(self):
         return self._publish
 
-    @is_publishing.setter
-    def is_publishing(self, publish):
-        self._publish = bool(publish)
+    # @is_publishing.setter
+    # def is_publishing(self, publish):
+    #     self._publish = bool(publish)
 
     @property
     def is_teleoperating(self):
         return self._teleoperate
 
-    @is_teleoperating.setter
-    def is_teleoperating(self, teleoperate):
-        self._teleoperate = bool(teleoperate)
+    # @is_teleoperating.setter
+    # def is_teleoperating(self, teleoperate):
+    #     self._teleoperate = bool(teleoperate)
 
     @property
     def is_commanding(self):
         return self._command
 
-    @is_commanding.setter
-    def is_commanding(self, command):
-        self._command = bool(command)
+    # @is_commanding.setter
+    # def is_commanding(self, command):
+    #     self._command = bool(command)
+
+    # aliases
+    subscribe = is_subscribing
+    publish = is_publishing
+    teleoperate = is_teleoperating
+    command = is_commanding
 
     #############
     # Operators #
@@ -118,6 +143,57 @@ class Middleware(object):
     # Methods #
     ###########
 
+    def switch_mode(self, body_id=None, subscribe=None, publish=None, teleoperate=None, command=None):
+        """
+        Switch middleware mode.
+
+        Here are the possible combinations between the different values for subscribe (S), publish (P),
+        teleoperate (T), and command (C):
+
+        - S=1, P=0, T=0: subscribes to the topics, and get the messages when calling the corresponding getter methods.
+        - S=0, P=1, T=0: publishes the various messages to the topics when calling the corresponding setter methods.
+        - S=1, P=0, T=1, C=0/1: get messages by subscribing to the topics that publish some commands/states. The
+          received commands/states are then set in the simulator. Depending on the value of `C`, it will subscribe to
+          topics that publish some commands (C=1) or states (C=0). Example: should we subscribe to joint trajectory
+          commands, or joint states when teleoperating the robot in the simulator? This C value allows to specify
+          which one we are interested in.
+        - S=0, P=1, T=1: when calling the getters methods such as joint positions, velocities, and others, it also
+          publishes the joint states. This is useful if we are moving/teleoperating the robot in the simulator.
+        - S=0, P=0, T=1/0: doesn't do anything.
+        - S=1, P=1, T=0: subscribes to some topics and publish messages to other topics. The messages are can be
+          sent/received by calling the appropriate getter/setter methods.
+        - S=1, P=1, T=1: not allowed, because teleoperating the robot is not a two-way communication process.
+
+        Args:
+            body_id (int): unique body id to switch the mode.
+            subscribe (bool): if True, it will subscribe to the topics associated to the loaded robots, and will read
+              the values published on these topics.
+            publish (bool): if True, it will publish the given values to the topics associated to the loaded robots.
+            teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2
+              previous attributes :attr:`subscribe` and :attr:`publish`.
+            command (bool): if True, it will subscribe to the joint commands. If False, it will subscribe to the
+              joint states.
+        """
+        if body_id is None:
+            if subscribe is None:
+                subscribe = self.subscribe
+            if publish is None:
+                publish = self.publish
+            if teleoperate is None:
+                teleoperate = self.teleoperate
+            if command is None:
+                command = self.command
+
+            if teleoperate and publish and subscribe:
+                raise ValueError("The three following arguments 'subscribe', 'publish', and 'teleoperate' can not be "
+                                 "all true at the same time. Select maximum two to be set to True (see method "
+                                 "documentation).")
+
+            self._subscribe = bool(subscribe)
+            self._publish = bool(publish)
+            self._teleoperate = bool(teleoperate)
+            self._command = bool(command)
+
     def close(self):
         """
         Close the middleware.
@@ -185,6 +261,21 @@ class Middleware(object):
         """
         pass
 
+    def reset_joint_states(self, body_id, joint_ids, positions, velocities=None):
+        """
+        Reset the joint states. It is best only to do this at the start, while not running the simulation:
+        `reset_joint_state` overrides all physics simulation.
+
+        Args:
+            body_id (int): unique body id.
+            joint_ids (int, list[int]): joint indices where each joint index is between [0..num_joints(body_id)]
+            positions (float, list[float], np.array[float]): the joint position(s) (angle in radians [rad] or
+              position [m])
+            velocities (float, list[float], np.array[float]): the joint velocity(ies) (angular [rad/s] or linear
+              velocity [m/s])
+        """
+        pass
+
     def get_joint_positions(self, body_id, joint_ids):
         """
         Get the position of the given joint(s).

pyrobolearn/simulators/middlewares/robot_middleware.py

@@ -48,7 +48,7 @@ class RobotMiddleware(object):
     - S=0, P=0, T=1/0: doesn't do anything.
     - S=1, P=1, T=0: subscribes to some topics and publish messages to other topics. The messages are can be
       sent/received by calling the appropriate getter/setter methods.
-    - S=1, P=1, T=1: not allowed.
+    - S=1, P=1, T=1: not allowed, because teleoperating the robot is not a two-way communication process.
     """
 
     def __init__(self, robot_id, urdf=None, subscribe=False, publish=False, teleoperate=False, command=True,
@@ -64,22 +64,63 @@ class RobotMiddleware(object):
             publish (bool): if True, it will publish the given values to the topics associated to the loaded robot.
             teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2
               previous attributes :attr:`subscribe` and :attr:`publish`.
-            command (bool): if True, it will subscribe/publish to some (joint) commands. If False, it will
-              subscribe/publish to some (joint) states.
+            command (bool): if True, it will subscribe to the joint commands. If False, it will subscribe to the
+              joint states.
             control_file (str, None): path to the YAML control file.
         """
         # set variables
         self.id = robot_id
         self.urdf = urdf
         self.control_file = control_file
-        self.is_subscribing = subscribe
-        self.is_publishing = publish
-        self.is_teleoperating = teleoperate
-        self.command = command
 
-        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.")
+        self._subscribe, self._publish, self._teleoperate, self._command = False, False, False, False
+        self.switch_mode(subscribe=subscribe, publish=publish, teleoperate=teleoperate, command=command)
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def is_subscribing(self):
+        return self._subscribe
+
+    # @is_subscribing.setter
+    # def is_subscribing(self, subscribe):
+    #     self._subscribe = bool(subscribe)
+
+    @property
+    def is_publishing(self):
+        return self._publish
+
+    # @is_publishing.setter
+    # def is_publishing(self, publish):
+    #     self._publish = bool(publish)
+
+    @property
+    def is_teleoperating(self):
+        return self._teleoperate
+
+    # @is_teleoperating.setter
+    # def is_teleoperating(self, teleoperate):
+    #     self._teleoperate = bool(teleoperate)
+
+    @property
+    def is_commanding(self):
+        return self._command
+
+    # @is_commanding.setter
+    # def is_commanding(self, command):
+    #     self._command = bool(command)
+
+    # aliases
+    subscribe = is_subscribing
+    publish = is_publishing
+    teleoperate = is_teleoperating
+    command = is_commanding
+
+    #############
+    # Operators #
+    #############
 
     def __del__(self):
         """
@@ -87,6 +128,10 @@ class RobotMiddleware(object):
         """
         self.close()
 
+    ###########
+    # Methods #
+    ###########
+
     def unregister(self):
         """
         Unsubscribe from a topic. Topic instance is no longer valid after this call. Additional calls to `unregister()`
@@ -100,6 +145,68 @@ class RobotMiddleware(object):
         """
         self.unregister()
 
+    def switch_mode(self, subscribe=None, publish=None, teleoperate=None, command=None):
+        """
+        Switch middleware mode.
+
+        Here are the possible combinations between the different values for subscribe (S), publish (P),
+        teleoperate (T), and command (C):
+
+        - S=1, P=0, T=0: subscribes to the topics, and get the messages when calling the corresponding getter methods.
+        - S=0, P=1, T=0: publishes the various messages to the topics when calling the corresponding setter methods.
+        - S=1, P=0, T=1, C=0/1: get messages by subscribing to the topics that publish some commands/states. The
+          received commands/states are then set in the simulator. Depending on the value of `C`, it will subscribe to
+          topics that publish some commands (C=1) or states (C=0). Example: should we subscribe to joint trajectory
+          commands, or joint states when teleoperating the robot in the simulator? This C value allows to specify
+          which one we are interested in.
+        - S=0, P=1, T=1: when calling the getters methods such as joint positions, velocities, and others, it also
+          publishes the joint states. This is useful if we are moving/teleoperating the robot in the simulator.
+        - S=0, P=0, T=1/0: doesn't do anything.
+        - S=1, P=1, T=0: subscribes to some topics and publish messages to other topics. The messages are can be
+          sent/received by calling the appropriate getter/setter methods.
+        - S=1, P=1, T=1: not allowed, because teleoperating the robot is not a two-way communication process.
+
+        Args:
+            subscribe (bool): if True, it will subscribe to the topics associated to the loaded robots, and will read
+              the values published on these topics.
+            publish (bool): if True, it will publish the given values to the topics associated to the loaded robots.
+            teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2
+              previous attributes :attr:`subscribe` and :attr:`publish`.
+            command (bool): if True, it will subscribe to the joint commands. If False, it will subscribe to the
+              joint states.
+        """
+        if subscribe is None:
+            subscribe = self.subscribe
+        if publish is None:
+            publish = self.publish
+        if teleoperate is None:
+            teleoperate = self.teleoperate
+        if command is None:
+            command = self.command
+
+        if teleoperate and publish and subscribe:
+            raise ValueError("The three following arguments 'subscribe', 'publish', and 'teleoperate' can not be all "
+                             "true at the same time. Select maximum two to be set to True (see method documentation).")
+
+        self._subscribe = bool(subscribe)
+        self._publish = bool(publish)
+        self._teleoperate = bool(teleoperate)
+        self._command = bool(command)
+
+    def reset_joint_states(self, positions, joint_ids=None, velocities=None):
+        """
+        Reset the joint states. It is best only to do this at the start, while not running the simulation:
+        `reset_joint_state` overrides all physics simulation.
+
+        Args:
+            positions (float, list[float], np.array[float]): the joint position(s) (angle in radians [rad] or
+              position [m])
+            joint_ids (int, list[int]): joint indices where each joint index is between [0..num_joints(body_id)]
+            velocities (float, list[float], np.array[float]): the joint velocity(ies) (angular [rad/s] or linear
+              velocity [m/s])
+        """
+        pass
+
     def get_joint_positions(self, joint_ids):
         """
         Get the position of the given joint(s).

pyrobolearn/simulators/middlewares/robots/franka.py

@@ -14,12 +14,20 @@ The topics for the joint states and joint commands (=joint trajectories) are:
 - /panda_hand_controller/command
 """
 
+import time
 import numpy as np
 import rospy
 
-# import ROS messages
+# import ROS messages / services
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
-from sensor_msgs.msg import JointState
+# from sensor_msgs.msg import JointState
+
+MoveJoints = None
+try:
+    from panda_arm.srv import MoveJoints
+except ImportError as e:
+    print("The service `MoveJoints` is not available... Please compile it using `catkin_make` in order to use it "
+          "when resetting the joint states.\n" + str(e))
 
 from pyrobolearn.simulators.middlewares.ros import ROSRobotMiddleware
 
@@ -72,8 +80,6 @@ class FrankaROSMiddleware(ROSRobotMiddleware):
         # self.publisher.init_set_joint_velocities(pub, msg_attribute_name='points')
         # self.publisher.init_set_joint_torques(pub, msg_attribute_name='points')
         #
-        # self.subscriber.
-        #
         # # joint names in the messages
         # self.msg_joint_names = ['panda_finger_joint1', 'panda_finger_joint2'] + \
         #                        ['panda_joint' + str(i+1) for i in range(7)]
@@ -89,15 +95,16 @@ class FrankaROSMiddleware(ROSRobotMiddleware):
         # joint trajectory point instance
         self.arm_point = JointTrajectoryPoint()
         self.arm_point.positions = np.zeros(7)
-        # self.arm_point.velocities = 0.1 * np.ones(7)
+        self.arm_point.velocities = 0.1 * np.ones(7)
         # self.arm_point.effort = 0.1 * np.ones(7)
         self.hand_point = JointTrajectoryPoint()
         self.hand_point.positions = np.zeros(2)
-        # self.hand_point.velocities = 0.1 * np.ones(2)
+        self.hand_point.velocities = 0.1 * np.ones(2)
         # self.hand_point.effort = 0.1 * np.ones(2)
 
         # update publisher
         arm_topic = '/panda_arm_controller/command'
+        # arm_topic = '/position_joint_trajectory_controller/command'
         self.arm_publisher = self.publisher.create_publisher(name='panda_arm_trajectory', topic=arm_topic,
                                                              msg_class=JointTrajectory)
         hand_topic = '/panda_hand_controller/command'
@@ -110,6 +117,63 @@ class FrankaROSMiddleware(ROSRobotMiddleware):
         self.hand_publisher.msg.joint_names = self.msg_joint_names[:2]
         self.hand_publisher.msg.points = [self.hand_point]
 
+        # create reset joint state service
+        self.reset_joint_service = None
+        if MoveJoints is not None:
+            self.reset_joint_service_name = '/arm/move_joint_absolute'
+            self.reset_joint_service = rospy.ServiceProxy(self.reset_joint_service_name, MoveJoints)
+
+    def reset_joint_states(self, positions, joint_ids=None, velocities=None):
+        """
+        Reset the joint states. It is best only to do this at the start, while not running the simulation:
+        `reset_joint_state` overrides all physics simulation.
+
+        Args:
+            positions (float, list[float], np.array[float]): the joint position(s) (angle in radians [rad] or
+              position [m])
+            joint_ids (int, list[int]): joint indices where each joint index is between [0..num_joints(body_id)]
+            velocities (float, list[float], np.array[float]): the joint velocity(ies) (angular [rad/s] or linear
+              velocity [m/s])
+        """
+        if self.reset_joint_service is not None:
+
+            # call rosservice to reset the joints
+            rospy.wait_for_service(self.reset_joint_service_name)
+            try:
+                print("Reset joint state on the real platform...")
+                # keep only joint arm indices/positions
+                q_indices = None if joint_ids is None else self.q_indices[joint_ids]
+                positions = positions[:7]
+                q_indices = q_indices[q_indices <= 6]
+                positions = positions[q_indices]
+                args = np.array(['T' + str(i+1) for i in range(6)])
+                kwargs = dict(zip(args[q_indices], positions))
+                velocity_scale = 0.1  # 1 = max velocity, 0 = don't move
+                duration_time = 10  # 10 secs
+                response = self.reset_joint_service(**kwargs, scale=velocity_scale, time=duration_time)
+
+                # blocking call
+                request = 'test'
+                while request != '':
+                    request = input("Once the robot has been reset to the desired joint configuration, please press "
+                                    "Enter to move on with the code.")
+
+                    # double check that the robot is at the specified joint configuration and if not, ask the user
+                    # to confirm to proceed with the code
+
+                    # q_curr = self.get_joint_positions(joint_ids=joint_ids)
+                    # positions = np.asarray(positions)
+                    # if q_curr is not None:
+                    #     while True:
+                    #         if len(q_curr) > 0:
+                    #             if np.linalg.norm((positions[:7] - )):
+                    #                 break
+                    #         q_curr = self.get_joint_positions(joint_ids=joint_ids)
+                    # print("Reset joint state was a success.")
+
+            except rospy.ServiceException as e:
+                print(self.reset_joint_service_name + " service call failed: " + str(e))
+
     def get_joint_positions(self, joint_ids=None):
         """
         Get the position of the given joint(s).
@@ -142,7 +206,7 @@ class FrankaROSMiddleware(ROSRobotMiddleware):
         if self.is_publishing:
             q = self.subscriber.get_joint_positions()
             dq = self.subscriber.get_joint_velocities()
-            tau = self.subscriber.get_joint_torques()
+            # tau = self.subscriber.get_joint_torques()
 
             if len(q) > 0:
                 q_indices = None if joint_ids is None else self.q_indices[joint_ids]
@@ -152,11 +216,11 @@ class FrankaROSMiddleware(ROSRobotMiddleware):
                         dq[q_indices] = velocities
 
                 self.arm_point.positions = q[:7]
-                # self.arm_point.velocities = dq[:7]
+                self.arm_point.velocities = dq[:7]
                 # self.arm_point.effort = tau[:7]
 
                 self.hand_point.positions = q[7:]
-                # self.hand_point.velocities = dq[7:]
+                self.hand_point.velocities = dq[7:]
                 # self.hand_point.effort = tau[7:]
 
                 # set time duration

pyrobolearn/simulators/middlewares/robots/services/panda_arm/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(panda_arm)
+
+find_package(catkin REQUIRED COMPONENTS rospy roscpp std_msgs genmsg message_generation)
+
+add_service_files(
+  DIRECTORY srv
+  FILES MoveJoints.srv
+)
+
+generate_messages(
+  DEPENDENCIES std_msgs
+)
+
+catkin_package()
+
+include_directories(include ${catkin_INCLUDE_DIRS})

pyrobolearn/simulators/middlewares/robots/services/panda_arm/package.xml

@@ -0,0 +1,66 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>panda_arm</name>
+  <version>0.0.0</version>
+  <description>The panda arm package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="arrfou@todo.todo">arrfou</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/learning_actionlib</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>  
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

pyrobolearn/simulators/middlewares/robots/services/panda_arm/srv/MoveJoints.srv

@@ -0,0 +1,21 @@
+float64 T1
+
+float64 T2
+
+float64 T3
+
+float64 T4
+
+float64 T5
+
+float64 T6
+
+float64 T7
+
+float64 scale
+
+int32 time
+
+---
+
+std_msgs/String output

pyrobolearn/simulators/middlewares/ros.py

@@ -172,7 +172,7 @@ class ROSRobotMiddleware(RobotMiddleware):
     - S=0, P=0, T=1/0: doesn't do anything.
     - S=1, P=1, T=0: subscribes to some topics and publish messages to other topics. The messages are can be
       sent/received by calling the appropriate getter/setter methods.
-    - S=1, P=1, T=1: not allowed.
+    - S=1, P=1, T=1: not allowed, because teleoperating the robot is not a two-way communication process.
     """
 
     def __init__(self, robot_id, urdf=None, subscribe=False, publish=False, teleoperate=False, command=True,
@@ -188,8 +188,8 @@ class ROSRobotMiddleware(RobotMiddleware):
             publish (bool): if True, it will publish the given values to the topics associated to the loaded robot.
             teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2
               previous attributes :attr:`subscribe` and :attr:`publish`.
-            command (bool): if True, it will subscribe/publish to some (joint) commands. If False, it will
-              subscribe/publish to some (joint) states.
+            command (bool): if True, it will subscribe to the joint commands. If False, it will subscribe to the
+              joint states.
             control_file (str, None): path to the YAML control file. If provided, it will be parsed.
             launch_file (str, None): path to the ROS launch file. If provided, it will be parsed.
             joint_state_topics (str, list[str]): joint state topic(s). If not provided the joint state topic will be
@@ -229,9 +229,9 @@ class ROSRobotMiddleware(RobotMiddleware):
         print("Creating Robot Subscriber")
         self.subscriber = RobotSubscriber(name=basename, joint_state_topics=joint_state_topics,
                                           joint_state_msg_class=joint_state_msg_class)
-        if self.is_publishing:
-            print("Creating Robot Publisher")
-            self.publisher = RobotPublisher(name=basename)
+        # if self.is_publishing:
+        print("Creating Robot Publisher")
+        self.publisher = RobotPublisher(name=basename)
 
     def unregister(self):
         """
@@ -322,10 +322,10 @@ class DefaultROSRobotMiddleware(ROSRobotMiddleware):
     - S=0, P=0, T=1/0: doesn't do anything.
     - S=1, P=1, T=0: subscribes to some topics and publish messages to other topics. The messages are can be
       sent/received by calling the appropriate getter/setter methods.
-    - S=1, P=1, T=1: not allowed.
+    - S=1, P=1, T=1: not allowed, because teleoperating the robot is not a two-way communication process.
     """
 
-    def __init__(self, robot_id, urdf=None, subscribe=False, publish=False, teleoperate=False, command=True,
+    def __init__(self, robot_id, urdf=None, subscribe=False, publish=False, teleoperate=False, command=False,
                  control_file=None, launch_file=None):
         """
         Initialize the robot middleware interface.
@@ -338,8 +338,8 @@ class DefaultROSRobotMiddleware(ROSRobotMiddleware):
             publish (bool): if True, it will publish the given values to the topics associated to the loaded robot.
             teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2
               previous attributes :attr:`subscribe` and :attr:`publish`.
-            command (bool): if True, it will subscribe/publish to some (joint) commands. If False, it will
-              subscribe/publish to some (joint) states.
+            command (bool): if True, it will subscribe to the joint commands. If False, it will subscribe to the
+              joint states.
             control_file (str, None): path to the YAML control file.
             launch_file (str, None): path to the ROS launch file. If provided, it will be parsed.
         """
@@ -597,6 +597,23 @@ class ROS(Middleware):
         """
         Initialize the ROS middleware.
 
+        Here are the possible combinations between the different values for subscribe (S), publish (P),
+        teleoperate (T), and command (C):
+
+        - S=1, P=0, T=0: subscribes to the topics, and get the messages when calling the corresponding getter methods.
+        - S=0, P=1, T=0: publishes the various messages to the topics when calling the corresponding setter methods.
+        - S=1, P=0, T=1, C=0/1: get messages by subscribing to the topics that publish some commands/states. The
+          received commands/states are then set in the simulator. Depending on the value of `C`, it will subscribe to
+          topics that publish some commands (C=1) or states (C=0). Example: should we subscribe to joint trajectory
+          commands, or joint states when teleoperating the robot in the simulator? This C value allows to specify
+          which one we are interested in.
+        - S=0, P=1, T=1: when calling the getters methods such as joint positions, velocities, and others, it also
+          publishes the joint states. This is useful if we are moving/teleoperating the robot in the simulator.
+        - S=0, P=0, T=1/0: doesn't do anything.
+        - S=1, P=1, T=0: subscribes to some topics and publish messages to other topics. The messages are can be
+          sent/received by calling the appropriate getter/setter methods.
+        - S=1, P=1, T=1: not allowed, because teleoperating the robot is not a two-way communication process.
+
         Args:
             subscribe (bool): if True, it will subscribe to the topics associated to the loaded robots, and will read
               the values published on these topics.
@@ -1404,6 +1421,23 @@ class ROS(Middleware):
 
         return -1
 
+    def reset_joint_states(self, body_id, joint_ids, positions, velocities=None):
+        """
+        Reset the joint states. It is best only to do this at the start, while not running the simulation:
+        `reset_joint_state` overrides all physics simulation.
+
+        Args:
+            body_id (int): unique body id.
+            joint_ids (int, list[int]): joint indices where each joint index is between [0..num_joints(body_id)]
+            positions (float, list[float], np.array[float]): the joint position(s) (angle in radians [rad] or
+              position [m])
+            velocities (float, list[float], np.array[float]): the joint velocity(ies) (angular [rad/s] or linear
+              velocity [m/s])
+        """
+        robot = self._robots.get(body_id)
+        if robot is not None:
+            return robot.reset_joint_states(positions, joint_ids=joint_ids, velocities=velocities)
+
     def get_joint_positions(self, body_id, joint_ids):
         """
         Get the position of the given joint(s).
@@ -1644,6 +1678,43 @@ class ROS(Middleware):
             return False
         return robot.publish
 
+    def switch_mode(self, body_id=None, subscribe=None, publish=None, teleoperate=None, command=None):
+        """
+        Switch middleware mode.
+
+        Here are the possible combinations between the different values for subscribe (S), publish (P),
+        teleoperate (T), and command (C):
+
+        - S=1, P=0, T=0: subscribes to the topics, and get the messages when calling the corresponding getter methods.
+        - S=0, P=1, T=0: publishes the various messages to the topics when calling the corresponding setter methods.
+        - S=1, P=0, T=1, C=0/1: get messages by subscribing to the topics that publish some commands/states. The
+          received commands/states are then set in the simulator. Depending on the value of `C`, it will subscribe to
+          topics that publish some commands (C=1) or states (C=0). Example: should we subscribe to joint trajectory
+          commands, or joint states when teleoperating the robot in the simulator? This C value allows to specify
+          which one we are interested in.
+        - S=0, P=1, T=1: when calling the getters methods such as joint positions, velocities, and others, it also
+          publishes the joint states. This is useful if we are moving/teleoperating the robot in the simulator.
+        - S=0, P=0, T=1/0: doesn't do anything.
+        - S=1, P=1, T=0: subscribes to some topics and publish messages to other topics. The messages are can be
+          sent/received by calling the appropriate getter/setter methods.
+        - S=1, P=1, T=1: not allowed, because teleoperating the robot is not a two-way communication process.
+
+        Args:
+            body_id (int): unique body id to switch the mode.
+            subscribe (bool): if True, it will subscribe to the topics associated to the loaded robots, and will read
+              the values published on these topics.
+            publish (bool): if True, it will publish the given values to the topics associated to the loaded robots.
+            teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2
+              previous attributes :attr:`subscribe` and :attr:`publish`.
+            command (bool): if True, it will subscribe to the joint commands. If False, it will subscribe to the
+              joint states.
+        """
+        super(ROS, self).switch_mode(body_id=body_id, subscribe=subscribe, publish=publish, teleoperate=teleoperate,
+                                     command=command)
+        if body_id is not None:
+            robot = self._robots.get(body_id, None)
+            robot.switch_mode(subscribe=subscribe, publish=publish, teleoperate=teleoperate, command=command)
+
 
 # Tests
 if __name__ == '__main__':

pyrobolearn/simulators/middlewares/ros_service.py

@@ -0,0 +1,88 @@
+# -*- coding: utf-8 -*-
+#!/usr/bin/env python
+"""Define the abstract ROS service.
+"""
+
+import collections
+import rospy
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class ROSService(object):
+    r"""ROS service.
+
+    This just group the various services into one common data structure.
+    """
+
+    def __init__(self, name, service_class):
+        """
+        Initialize the ROS service.
+
+        Args:
+            name (str, list[str]): service name(s).
+            service_class (class, list[class]): service class(es) for serialization.
+        """
+        if isinstance(name, str):
+            name = [name]
+        elif not isinstance(name, collections.Iterable):
+            raise TypeError("Expecting the given 'name' to be a string, list of string, but got instead: "
+                            "{}".format(type(name)))
+        self.names = name
+
+        if not isinstance(service_class, collections.Iterable):
+            service_class = [service_class]
+        self.service_classes = service_class
+
+        self.services = dict()
+        for name, service_class in zip(self.names, self.service_classes):
+            service = rospy.ServiceProxy(name, service_class)
+            self.services[name] = service
+
+    def call(self, name, *args, **kwargs):
+        """
+        Calls the given service, and returns the possible response.
+
+        Args:
+            name (str): service name.
+            *args (list): list of parameters to be given to the service.
+            **kwargs (dict): dictionary of parameters to be given to the service.
+        """
+        rospy.wait_for_service(name)
+        try:
+            service = self.services[name]
+            response = service(*args, **kwargs)
+            return response
+        except rospy.ServiceException as e:
+            print(name + " service call failed...\n" + str(e))
+
+    def create_service(self, names, service_classes):
+        """
+        Create a ros service. This will be added to the list of inner ROS services.
+
+        Args:
+            names (str, list[str]): service name(s).
+            service_classes (class, list[class]): service class(es) for serialization.
+
+        Returns:
+            rospy.ServiceProxy, list[rospy.ServiceProxy]: created ros services.
+        """
+        if isinstance(names, str):
+            names = [names]
+        elif not isinstance(names, collections.Iterable):
+            raise TypeError("Expecting the given 'names' to be a string, list of string, but got instead: "
+                            "{}".format(type(names)))
+        if not isinstance(service_classes, collections.Iterable):
+            service_classes = [service_classes]
+
+        for name, service_class in zip(names, service_classes):
+            service = rospy.ServiceProxy(name, service_class)
+            self.services[name] = service

pyrobolearn/simulators/middlewares/ros_subscriber.py

@@ -356,9 +356,11 @@ class RobotSubscriber(Subscriber):
         Returns:
             float, np.array[float]: joint positions.
         """
-        if q_indices is None:
-            return np.asarray(self.joint_states.msg.position)
-        return np.asarray(self.joint_states.msg.position)[q_indices]
+        positions = self.joint_states.msg.position
+        if positions:  # because it can be empty at the beginning
+            if q_indices is None:
+                return np.asarray(positions)
+            return np.asarray(positions)[q_indices]
 
     def get_joint_velocities(self, q_indices=None):
         """
@@ -371,9 +373,11 @@ class RobotSubscriber(Subscriber):
         Returns:
             float, np.array[float]: joint velocities.
         """
-        if q_indices is None:
-            return np.asarray(self.joint_states.msg.velocity)
-        return np.asarray(self.joint_states.msg.velocity)[q_indices]
+        velocities = self.joint_states.msg.velocity
+        if velocities:  # because it can be empty at the beginning
+            if q_indices is None:
+                return np.asarray(velocities)
+            return np.asarray(velocities)[q_indices]
 
     def get_joint_torques(self, q_indices=None):
         """
@@ -386,9 +390,11 @@ class RobotSubscriber(Subscriber):
         Returns:
             float, np.array[float]: joint torques.
         """
-        if q_indices is None:
-            return np.asarray(self.joint_states.msg.effort)
-        return np.asarray(self.joint_states.msg.effort)[q_indices]
+        torques = self.joint_states.msg.effort
+        if torques:
+            if q_indices is None:
+                return np.asarray(torques)
+            return np.asarray(torques)[q_indices]
 
     def get_pid(self, q_indices=None):
         """

pyrobolearn/simulators/simulator.py

@@ -1320,6 +1320,45 @@ class Simulator(object):
         """
         pass
 
+    def reset_joint_states(self, body_id, joint_ids, positions, velocities=None):
+        """
+        Reset the joint states. It is best only to do this at the start, while not running the simulation:
+        `reset_joint_state` overrides all physics simulation.
+
+        Args:
+            body_id (int): unique body id.
+            joint_ids (int, list[int]): joint indices where each joint index is between [0..num_joints(body_id)]
+            positions (float, list[float], np.array[float]): the joint position(s) (angle in radians [rad] or
+              position [m])
+            velocities (float, list[float], np.array[float]): the joint velocity(ies) (angular [rad/s] or linear
+              velocity [m/s])
+        """
+        # reset the joint states in the simulator
+        self._reset_joint_states(body_id, joint_ids, positions, velocities)
+
+        # publish the joint positions through the middleware
+        if self.middleware is not None and self._middleware_enabled:
+            self.middleware.reset_joint_states(body_id, joint_ids, positions, velocities)
+
+    def _reset_joint_states(self, body_id, joint_ids, positions, velocities=None):
+        """
+        Reset the joint states. It is best only to do this at the start, while not running the simulation:
+        `reset_joint_state` overrides all physics simulation.
+
+        Args:
+            body_id (int): unique body id.
+            joint_ids (int, list[int]): joint indices where each joint index is between [0..num_joints(body_id)]
+            positions (float, list[float], np.array[float]): the joint position(s) (angle in radians [rad] or
+              position [m])
+            velocities (float, list[float], np.array[float]): the joint velocity(ies) (angular [rad/s] or linear
+              velocity [m/s])
+        """
+        # reset the joint states
+        for i, joint_id in enumerate(joint_ids):
+            position = positions[i]
+            velocity = None if velocities is None else velocities[i]
+            self.reset_joint_state(body_id, joint_id, position, velocity)
+
     def enable_joint_force_torque_sensor(self, body_id, joint_ids, enable=True):
         """
         You can enable or disable a joint force/torque sensor in each joint.