update ROS/BulletROS + add simulator examples

examples/README.rst

@@ -6,15 +6,16 @@ In this folder, you will find different examples on how to use the framework.
 Warning: this folder is currently being updated; few files might still have some bugs or not 
 implemented completely. Some other folders will be added in the upcoming days.
 
-You can check the following folders:
+You can check the following folders on:
 
+- ``simulators``: how to use a particular simulator. Currently, the Bullet simulator is the one fully operational.
 - ``worlds``: how to create a world in the simulator, load various objects inside and interact with them, use the camera, and load or generate terrains.
-- ``robots``: check how to load a specific robot (biped, quadruped, wheeled, etc) into the world.
+- ``robots``: how to load a specific robot (biped, quadruped, wheeled, etc) into the world.
 - ``interfaces``: the various interfaces (game controllers, webcam, etc) and bridges that you can use.
-- ``kinematics``: check how to use forward and inverse kinematics as well as position and velocity control.
-- ``manipulability``: check how to use the velocity and dynamic manipulability ellipsoids.
+- ``kinematics``: how to use forward and inverse kinematics as well as position and velocity control.
+- ``dynamics``: how to use forward and inverse dynamics as well as force control.
+- ``manipulability``: how to use the velocity and dynamic manipulability ellipsoids.
 - ``states``: how to query the states / observations.
 - ``models``: the different learning models that you can use.
 - ``imitation``: how to use imitation learning with the framework.
-- ``gym/cartpole``: policies are trained with different algorithms on the gym Cartpole environment.
-
+- ``gym/cartpole``: policies that are trained with different algorithms on the gym Cartpole environment.

examples/dynamics/README.md

@@ -1,9 +1,9 @@
 ## Robot dynamics
 
-We provide examples on how to perform forward and inverse dynamics, force and impedance control.
+We provide examples on how to perform forward and inverse dynamics, as well as force control.
 
 References:
 - [1] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010
 - [2] "Springer Handbook of Robotics", Siciliano et al., 2008
 - [3] "Rigid Body Dynamics Algorithms", Featherstone, 2008
-- [4] [Lecture on Impedance Control](http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf) by Prof. De Luca, Universita di Roma,
+- [4] [Lecture on Impedance Control](http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf) by Prof. De Luca, Universita di Roma

examples/dynamics/control/README.md

@@ -1,8 +1,20 @@
-### Force and Impedance control
+### Force control
 
 In a nutshell, you have different control modes:
 
-* position control: specify the desired task (or joint) positions
-* velocity control: specify the desired task (or joint) velocities
+* motion control: specify the desired task (or joint) positions / velocities
 * force control: specify the desired task (or joint) forces
-* impedance control: specify the task (or joint) forces based on the desired task (or joint) positions, velocities, and accelerations.
+	* indirect force control:
+		* impedance control
+		* admittance control
+	* direct force control:
+		* hybrid force/position control
+		* parallel force/position control
+
+Here are the few examples that you can find in this folder:
+1. `no_forces.py`: this example loads a RRBot robot and disable the motors. It does not apply any joint torques.
+2. `gravity_compensation.py`: compute the necessary joint torques to compensate for gravity.
+3. `attractor_point.py`: compute the necessary joint torques (using impedance control) such that the end-effector 
+is attracted by a 3D Cartesian point.
+
+For these 3 above examples, try to move the robot's end effector with your mouse and see what happens.

examples/simulators/README.md

@@ -0,0 +1,28 @@
+## Simulator examples
+
+In this folder, you will have simple examples on how to use different simulators by loading a simple world (with a 
+floor and gravity enabled), and a simple robot.
+
+The various files can be called by `python <simulator>.py`. Note that they are mostly identical, and only the line 
+
+```python
+sim = Simulator(args)  # Bullet, BulletROS, ROS, etc
+```
+
+need to be changed. The basic idea is that your code should work without depending on which simulator you use. Note 
+that currently, the `Bullet` simulator is the only fully functional API, while the others are partially implemented 
+or still need to be implemented.
+
+Here are the few examples that you can find in this folder:
+1. `bullet.py`: simple example where we use the `Bullet` simulator, load a basic world (with a floor and gravity 
+enabled) and the RRBot robot in it.
+2. `bullet_ros_publisher.py`: example where we use the `BulletROS(publish=True)` simulator which publishes the 
+joint position values that were returned by the Bullet simulator on the corresponding ROS topic.
+3. `bullet_ros_subscriber.py`: example where we use the `BulletROS(subscribe=True)` simulator which gets 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 moves in accordance with. This can be useful if you have access to the real platform as well.
+
+Later, a `ROS`/`ROS_RBDL` "simulator" (without passing by a real simulator like `Bullet`) will allow you to make 
+your code works on a real platform using ROS without changing any other lines of code. This is one of the big 
+TODOs but is not currently my priority.

examples/simulators/bullet.py

@@ -0,0 +1,34 @@
+#!/usr/bin/env python
+"""Example on how to use the Bullet simulator in pyrobolearn.
+
+Simple example where we use the bullet simulator, load a basic world (with a floor and gravity enabled) and the RRBot
+robot in it.
+
+You can move inside the world using your mouse and keyboard:
+- `ctrl + left click`: rotate the camera
+- `scroll wheel` or `ctrl + right click`: zoom in/out
+- `ctrl + middle click`: move the camera
+- `left click` on an object: if the object has a mass and a collision shape, you can interact with it with the mouse
+- `w`: wireframe (see collision shapes)
+- `g`: show/hide menu
+- `esc`: quit the simulator
+"""
+
+from itertools import count
+import pyrobolearn as prl
+
+
+# create simulator
+sim = prl.simulators.Bullet(render=True)
+
+# create basic world (i.e. with a floor and gravity enabled)
+world = prl.worlds.BasicWorld(sim)
+
+# load rrbot
+robot = prl.robots.RRBot(sim)
+
+
+# run simulator
+for t in count():
+    # perform a step in the simulator and sleep for `sim.dt` (which is 1./240 in this case)
+    sim.step(sim.dt)

examples/simulators/bullet_ros_publisher.py

@@ -0,0 +1,66 @@
+#!/usr/bin/env python
+"""Example on how to use the Bullet-ROS simulator (the publisher version) in PRL.
+
+The publisher version publish the various data on the corresponding topics every time the joints are set. If the
+:attr:`teleoperate` is set to True, it will also publish every time we call a `get_*()` method.
+
+Before running this file, run in the terminal:
+```bash
+$ roscore               # note that roscore is launched automatically by this file if has not already been launched
+$ rostopic list         # to show the list of published topics
+```
+
+The last command should print the following:
+```bash
+/rosout
+/rosout_agg
+```
+
+Now, run this file and check the published topics again:
+```bash
+$ rostopic list
+```
+
+This time, you should get as well:
+```bash
+/rosout
+/rosout_agg
+/rrbot/joint_states
+```
+
+You can print the output of the topic with:
+```bash
+$ rostopic echo /rrbot/joint_states
+```
+
+Try to move the robot with the mouse, and see that the published joint position values changed as well.
+
+You can run this code in parallel with `bullet_ros_subscriber.py`, which implements the subscriber version, that is,
+it listens to the topics and set them in the bullet simulator. So by moving the robot in this simulator, you should
+see that it also moves in the other simulator.
+
+Note: this code also works with other robots.
+"""
+
+from itertools import count
+import pyrobolearn as prl
+
+
+# create simulator (ros core will automatically be launched if it has not already been launched)
+sim = prl.simulators.BulletROS(subscribe=False, publish=True, teleoperate=True)
+
+# load world
+world = prl.worlds.BasicWorld(sim)
+
+# load rrbot
+robot = prl.robots.RRBot(sim)
+
+
+# run simulation
+for t in count():
+    # get the joint positions from the Bullet simulator (because :attr:`teleoperate` has been set to True,
+    # it will publish these read positions on the corresponding topic)
+    q = robot.get_joint_positions()
+
+    # perform a step in the simulator (and sleep for `sim.dt`)
+    world.step(sim.dt)

examples/simulators/bullet_ros_subscriber.py

@@ -0,0 +1,33 @@
+#!/usr/bin/env python
+"""Example on how to use the Bullet-ROS simulator (the subscriber version) in PRL.
+
+The subscriber version subscribe to various topics related to the loaded robot.
+
+This code works in parallel with the `bullet_ros_publisher.py`, which implements the publisher version, that is,
+it publish to the topics the various data. By moving the robot in the publisher version of the simulator, you should
+see that the robot in this simulator should move in accordance.
+
+Note: this code also works with other robots.
+"""
+
+from itertools import count
+import pyrobolearn as prl
+
+
+# create simulator (ros core will automatically be launched if it has not already been launched)
+sim = prl.simulators.BulletROS(subscribe=True, publish=False)
+
+# load world
+world = prl.worlds.BasicWorld(sim)
+
+# load rrbot
+robot = prl.robots.RRBot(sim)
+
+
+# run simulation
+for t in count():
+    # get the joint positions from the ROS subscribers (if possible)
+    robot.get_joint_positions()
+
+    # perform a step in the world, and sleep for `sim.dt`
+    world.step(sim.dt)

pyrobolearn/__init__.py

@@ -4,6 +4,7 @@
 name = "pyrobolearn"
 
 import sys
+import signal
 
 # logging
 import logging
@@ -86,13 +87,22 @@ from . import algos
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "GNU GPLv3"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
+# Capture a SIGINT in Python: make sure we quit PRL when Ctrl+C is pressed
+def signal_handler(sig, frame):
+    print('You pressed Ctrl+C!')
+    sys.exit(0)
+
+
+signal.signal(signal.SIGINT, signal_handler)
+
+
 # https://stackoverflow.com/questions/30483246/how-to-check-if-a-python-module-has-been-imported
 # https://stackoverflow.com/questions/14050281/how-to-check-if-a-python-module-exists-without-importing-it/25045228
 def module_imported(module_name):  # TODO: improve this method

pyrobolearn/robots/ros/old/rrbot/publisher.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+"""Define the RRBot publisher.
+"""
+
+from pyrobolearn.robots.ros.publisher import RobotPublisher
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class RRBotPublisher(RobotPublisher):
+
+    def __init__(self, id_=None):
+        """
+        Initialize the RRBot Publisher.
+
+        Args:
+            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`.
+        """
+        super(RRBotPublisher, self).__init__(name='rrbot', id_=id_)

pyrobolearn/robots/ros/publisher.py

@@ -40,6 +40,9 @@ class PublisherData(object):
         if key in self.attributes:
             setattr(self.publisher_data, key, value)
 
+    def __getattr__(self, key):
+        return getattr(self.publisher_data, key)
+
 
 class Publisher(object):
     r"""Publisher class
@@ -89,7 +92,7 @@ class Publisher(object):
             for publisher in self.publishers.values():
                 publisher.publish()
         elif name is not None:
-            self.name.publish(data)
+            self.__dict__[name].publish(data)
 
     # def __getattr__(self, name):
     #     return self.publishers[name]
@@ -120,7 +123,8 @@ class RobotPublisher(Publisher):
         self.publishers['joint_states'] = self.joint_states
 
     def set_joint_positions(self, joint_ids, positions):
-        self.joint_states.position[joint_ids] = positions
+        # self.joint_states.position[joint_ids] = positions
+        self.joint_states.position = positions
 
     def set_joint_velocities(self, joint_ids, velocities):
         self.joint_states.velocity[joint_ids] = velocities
@@ -133,15 +137,16 @@ class RobotPublisher(Publisher):
 if __name__ == '__main__':
     # NOTE: run roscore before hand
     import numpy as np
+    from itertools import count
     import time
 
     publisher = RobotPublisher('walter')
+    print("Published topics: {}".format(rospy.get_published_topics()))
     print("Robot joint state attributes: {}".format(publisher.joint_states.attributes))
 
     publisher.joint_states.position = np.array(range(3))
 
-    for t in range(20):
+    for t in count():
+        print(t)
         publisher.publish()
         time.sleep(0.1)
-
-    print("Published topics: {}".format(rospy.get_published_topics()))

pyrobolearn/robots/ros/subscriber.py

@@ -29,14 +29,13 @@ class SubscriberData(object):
         self.subscriber = rospy.Subscriber(topic, data_class, callback=self.callback)
         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 = None
+        self.subscriber_data = data_class()
 
     def callback(self, data):
         self.subscriber_data = data
 
     def __getattr__(self, name):
-        if self.subscriber_data is not None:
-            return getattr(self.subscriber_data, name, None)
+        return getattr(self.subscriber_data, name)
 
     def unregister(self):
         self.subscriber.unregister()
@@ -124,13 +123,19 @@ class RobotSubscriber(Subscriber):
         self.create_subscriber('joint_states', self.name + '/joint_states', sensor_msg.JointState)
 
     def get_joint_positions(self, joint_ids):
-        return np.asarray(self.joint_states.position)[joint_ids]
+        if len(self.joint_states.position) >= len(joint_ids):
+            return np.asarray(self.joint_states.position)  # [joint_ids]
+        return np.asarray(self.joint_states.position)
 
     def get_joint_velocities(self, joint_ids):
-        return np.asarray(self.joint_states.velocity)[joint_ids]
+        if len(self.joint_states.velocity) >= len(joint_ids):
+            return np.asarray(self.joint_states.velocity)[joint_ids]
+        return np.asarray(self.joint_states.velocity)
 
     def get_joint_torques(self, joint_ids):
-        return np.asarray(self.joint_states.effort)[joint_ids]
+        if len(self.joint_states.effort) >= len(joint_ids):
+            return np.asarray(self.joint_states.effort)[joint_ids]
+        return np.asarray(self.joint_states.effort)
 
 
 # Tests
@@ -143,7 +148,7 @@ if __name__ == '__main__':
     print("Published topics: {}".format(rospy.get_published_topics()))
     print("Robot joint state attributes: {}".format(subscriber.joint_states.attributes))
 
-    for t in range(100):
+    for t in count():
         print(t)
         print("Joint position data: {}".format(subscriber.joint_states.position))
         time.sleep(0.1)

pyrobolearn/robots/rrbot.py

@@ -51,7 +51,7 @@ class RRBot(Manipulator):
         #     self.sim.step()
 
         # disable each motor joint
-        self.disable_motor()
+        # self.disable_motor()
         # self.sim.setJointMotorControlArray(self.id, self.joints, self.sim.VELOCITY_CONTROL, forces=forces)
 
         # enable F/T sensor at the end effector
@@ -78,6 +78,7 @@ if __name__ == "__main__":
 
     # load robot
     robot = RRBot(sim)
+    robot.disable_motor()
     # robot.add_joint_slider()
 
     print("Robot: {}".format(robot))

pyrobolearn/simulators/__init__.py

@@ -8,6 +8,9 @@ from .simulator import Simulator
 from .bullet import Bullet
 from .bullet import Bullet as BulletSim  # alias  # TODO: remove that alias
 
+# bullet ros simulator
+from .bullet_ros import BulletROS
+
 # dart simulator
 # from .dart import Dart
 

pyrobolearn/simulators/bullet_ros.py

@@ -27,7 +27,8 @@ import os
 import subprocess
 import psutil
 import signal
-import rospy
+import importlib
+import inspect
 
 # from pyrobolearn.simulators.simulator import Simulator
 from pyrobolearn.simulators.bullet import Bullet
@@ -54,7 +55,7 @@ class BulletROS(Bullet):  # , ROS):
     sensors, actuators, and forces, to map the real world to the simulated one, etc.
     """
 
-    def __init__(self, render=True, subscribe=False, publish=False, ros_master_uri=11311, **kwargs):
+    def __init__(self, render=True, subscribe=False, publish=False, teleoperate=False, ros_master_uri=11311, **kwargs):
         """
         Initialize the Bullet-ROS simulator.
 
@@ -80,10 +81,13 @@ class BulletROS(Bullet):  # , ROS):
             # subprocess.Popen("roscore", env=self.env)
             self.roscore = subprocess.Popen(["roscore", "-p", str(ros_master_uri)], env=self.env,
                                             preexec_fn=os.setsid)  # , shell=True)
+        else:
+            print('ROS core has already been initialized.')
 
         # set variables
         self.subscribe = subscribe
         self.publish = publish
+        self.teleoperate = teleoperate
 
         # remember each publisher/subscriber
         self.subscribers = {}
@@ -98,7 +102,11 @@ class BulletROS(Bullet):  # , ROS):
         # delete each publishers
 
         # delete ROS
-        os.killpg(os.getpgid(self.roscore.pid), signal.SIGTERM)
+        if self.roscore is not None:
+            os.killpg(os.getpgid(self.roscore.pid), signal.SIGTERM)
+
+        # call parent destructor
+        super(BulletROS, self).close()
 
     @property
     def is_subscribing(self):
@@ -160,19 +168,42 @@ class BulletROS(Bullet):  # , ROS):
                                                use_maximal_coordinates=use_maximal_coordinates,
                                                use_fixed_base=use_fixed_base, flags=flags, scale=scale)
         # get path to directory of urdf
-        path = os.path.dirname(filename)
-        robot_directory_name = path.split('/')[-1]
-        path = path + '/../../ros/' + robot_directory_name + '/'
+        path = os.path.dirname(os.path.abspath(filename))               # /path/to/pyrobolearn/robots/urdfs/<robot>/
+        robot_directory_name = path.split('/')[-1]                      # <robot>
+        # path = path + '/../../ros/' + robot_directory_name + '/'        # /path/to/pyrobolearn/robots/ros/<robot>/
 
         # check if valid robot directory
+        # if os.path.isdir(path):
+        robot_path = '/'.join(path.split('/')[-5:-2])
+        if robot_path == 'pyrobolearn/robots/urdfs':
 
-        # load subscriber in simulator
-        if self.subscribe:
-            pass
+            # get corresponding subscriber/publisher
+            def check_ros(name, dictionary, id_):
+                # TODO: do I really need a class for each robot? Can I not just use RobotPublisher?
+                # if os.path.isfile(path + name + '.py'):
+                #     module = importlib.import_module('pyrobolearn.robots.ros.' + robot_directory_name + '.' + name)
+                #     classes = inspect.getmembers(module, inspect.isclass)  # list of (name, class)
+                #     length = len(name)
+                #     robot_name = ''.join(robot_directory_name.split('_'))
+                #
+                #     # go through each class and get the :attr:`name` corresponding to the robot and add it to the
+                #     # given :attr:`dictionary`
+                #     for name, cls in classes:
+                #         if name[:-length].lower() == robot_name:
+                #             dictionary[id_] = cls(id_=id_)
+                #             break
+                module = importlib.import_module('pyrobolearn.robots.ros.' + name)
+                classes = dict(inspect.getmembers(module, inspect.isclass))
+                cls = classes['Robot' + name.capitalize()]
+                dictionary[id_] = cls(name=robot_directory_name, id_=id_)
 
-        # load publisher in simulator
-        if self.publish:
-            pass
+            # load subscriber in simulator
+            if self.subscribe:
+                check_ros('subscriber', self.subscribers, id_)
+
+            # load publisher in simulator
+            if self.publish:
+                check_ros('publisher', self.publishers, id_)
 
         return id_
 
@@ -191,10 +222,16 @@ class BulletROS(Bullet):  # , ROS):
                 np.float[N]: joint positions [rad]
         """
         if body_id in self.subscribers:
-            q = self.subscribers[body_id].get_joint_positions[joint_ids]
-            super(BulletROS, self).set_joint_positions(body_id=body_id, joint_ids=joint_ids, positions=q)  # or reset?
+            q = self.subscribers[body_id].get_joint_positions(joint_ids)
+            if len(q) == len(joint_ids):
+                super(BulletROS, self).set_joint_positions(body_id=body_id, joint_ids=joint_ids, positions=q)  # reset?
+            else:  # if failed to get the joint positions from the subscriber
+                q = super(BulletROS, self).get_joint_positions(body_id=body_id, joint_ids=joint_ids)
         else:
             q = super(BulletROS, self).get_joint_positions(body_id=body_id, joint_ids=joint_ids)
+            if self.teleoperate and body_id in self.publishers:
+                self.publishers[body_id].set_joint_positions(joint_ids, q)
+                self.publishers[body_id].publish('joint_states')
         return q
 
     def set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None):
@@ -213,6 +250,7 @@ class BulletROS(Bullet):  # , ROS):
         super(BulletROS, self).set_joint_positions(body_id, joint_ids, positions, velocities, kps, kds, forces)
         if body_id in self.publishers:
             self.publishers[body_id].set_joint_positions(joint_ids, positions)
+            self.publishers[body_id].publish('joint_states')
 
     def get_joint_velocities(self, body_id, joint_ids):
         """
@@ -228,7 +266,18 @@ class BulletROS(Bullet):  # , ROS):
             if multiple joints:
                 np.float[N]: joint velocities [rad/s]
         """
-        pass
+        if body_id in self.subscribers:
+            dq = self.subscribers[body_id].get_joint_velocities(joint_ids)
+            if len(dq) == len(joint_ids):
+                super(BulletROS, self).set_joint_velocities(body_id=body_id, joint_ids=joint_ids, velocities=dq)
+            else:  # if failed to get the joint velocities from the subscriber
+                dq = super(BulletROS, self).get_joint_velocities(body_id=body_id, joint_ids=joint_ids)
+        else:
+            dq = super(BulletROS, self).get_joint_velocities(body_id=body_id, joint_ids=joint_ids)
+            if self.teleoperate and body_id in self.publishers:
+                self.publishers[body_id].set_joint_velocities(joint_ids, dq)
+                self.publishers[body_id].publish('joint_states')
+        return dq
 
     def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None):
         """
@@ -240,7 +289,10 @@ class BulletROS(Bullet):  # , ROS):
             velocities (float, np.float[N]): desired velocity, or list of desired velocities [rad/s]
             max_force (None, float, np.float[N]): maximum motor forces/torques
         """
-        pass
+        super(BulletROS, self).set_joint_velocities(body_id, joint_ids, velocities, max_force)
+        if body_id in self.publishers:
+            self.publishers[body_id].set_joint_velocities(joint_ids, velocities)
+            self.publishers[body_id].publish('joint_states')
 
     def get_joint_torques(self, body_id, joint_ids):
         """
@@ -258,7 +310,18 @@ class BulletROS(Bullet):  # , ROS):
             if multiple joints:
                 np.float[N]: torques associated to the given joints [Nm]
         """
-        pass
+        if body_id in self.subscribers:
+            torques = self.subscribers[body_id].get_joint_torques(joint_ids)
+            if len(torques) == len(joint_ids):
+                super(BulletROS, self).set_joint_torques(body_id=body_id, joint_ids=joint_ids, torques=torques)
+            else:  # if failed to get the joint torques from the subscriber
+                torques = super(BulletROS, self).get_joint_torques(body_id=body_id, joint_ids=joint_ids)
+        else:
+            torques = super(BulletROS, self).get_joint_torques(body_id=body_id, joint_ids=joint_ids)
+            if self.teleoperate and body_id in self.publishers:
+                self.publishers[body_id].set_joint_torques(joint_ids, torques)
+                self.publishers[body_id].publish('joint_states')
+        return torques
 
     def set_joint_torques(self, body_id, joint_ids, torques):
         """
@@ -269,7 +332,10 @@ class BulletROS(Bullet):  # , ROS):
             joint_ids (int, list of int): joint id, or list of joint ids.
             torques (float, list of float): desired torque(s) to apply to the joint(s) [N].
         """
-        pass
+        super(BulletROS, self).set_joint_torques(body_id, joint_ids, torques)
+        if body_id in self.publishers:
+            self.publishers[body_id].set_joint_torques(joint_ids, torques)
+            self.publishers[body_id].publish('joint_states')
 
 
 # Test

pyrobolearn/simulators/ros.py

@@ -18,11 +18,16 @@ References:
 """
 
 # TODO
-
-import rospy
+import os
+import subprocess
+import psutil
+import signal
+import importlib
+import inspect
 
 from pyrobolearn.simulators.simulator import Simulator
 
+
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
@@ -50,12 +55,12 @@ class ROS(Simulator):
     r"""ROS Interface
     """
 
-    def __init__(self, subscribe=False, publish=False, master_uri=11311, **kwargs):
+    def __init__(self, subscribe=False, publish=False, teleoperate=False, master_uri=11311, **kwargs):
         super(ROS, self).__init__(render=False)
 
         # Environment variable
         self.env = os.environ.copy()
-        self.env["ROS_MASTER_URI"] = "http://localhost:" + str(ros_master_uri)
+        self.env["ROS_MASTER_URI"] = "http://localhost:" + str(master_uri)
 
         # this is for the rospy methods such as: wait_for_service(), init_node(), ...
         os.environ['ROS_MASTER_URI'] = self.env['ROS_MASTER_URI']
@@ -64,12 +69,13 @@ class ROS(Simulator):
         self.roscore = None
         if "roscore" not in [p.name() for p in psutil.process_iter()]:
             # subprocess.Popen("roscore", env=self.env)
-            self.roscore = subprocess.Popen(["roscore", "-p", str(ros_master_uri)], env=self.env,
+            self.roscore = subprocess.Popen(["roscore", "-p", str(master_uri)], env=self.env,
                                             preexec_fn=os.setsid)  # , shell=True)
 
         # set variables
         self.subscribe = subscribe
         self.publish = publish
+        self.teleoperate = teleoperate
 
         # remember each publisher/subscriber
         self.subscribers = {}
@@ -78,6 +84,28 @@ class ROS(Simulator):
 
         self.count_id = -1
 
+    def close(self):
+        """
+        Close everything
+        """
+        # delete each subscribers
+
+        # delete each publishers
+
+        # delete ROS
+        if self.roscore is not None:
+            os.killpg(os.getpgid(self.roscore.pid), signal.SIGTERM)
+
+    @property
+    def is_subscribing(self):
+        """Return True if we are subscribing to topics."""
+        return self.subscribe
+
+    @property
+    def is_publishing(self):
+        """Return True if we are publishing to topics."""
+        return self.publish
+
     def load_urdf(self, filename, position=None, orientation=None, use_maximal_coordinates=None,
                   use_fixed_base=None, flags=None, scale=None):
         """Load the given URDF file.
@@ -88,16 +116,6 @@ class ROS(Simulator):
         Many robots have public URDF files, you can find a description and tutorial here:
         http://wiki.ros.org/urdf/Tutorials
 
-        Important note:
-            most joints (slider, revolute, continuous) have motors enabled by default that prevent free
-            motion. This is similar to a robot joint with a very high-friction harmonic drive. You should set the joint
-            motor control mode and target settings using `pybullet.setJointMotorControl2`. See the
-            `setJointMotorControl2` API for more information.
-
-        Warning:
-            by default, PyBullet will cache some files to speed up loading. You can disable file caching using
-            `setPhysicsEngineParameter(enableFileCaching=0)`.
-
         Args:
             filename (str): a relative or absolute path to the URDF file on the file system of the physics server.
             position (vec3): create the base of the object at the specified position in world space coordinates [x,y,z]
@@ -124,24 +142,48 @@ class ROS(Simulator):
         Returns:
             int (non-negative): unique id associated to the load model.
         """
-        # get path to directory of urdf
-        path = os.path.dirname(filename)
-        robot_directory_name = path.split('/')[-1]
-        path = path + '/../../ros/' + robot_directory_name + '/'
-
-        # check if valid robot directory
-
-        # load subscriber in simulator
-        if self.subscribe:
-            pass
-
-        # load publisher in simulator
-        if self.publish:
-            pass
-
+        id_ = self.count_id
         self.count_id += 1
 
-        return self.count_id
+        # get path to directory of urdf
+        path = os.path.dirname(os.path.abspath(filename))           # /path/to/pyrobolearn/robots/urdfs/<robot>/
+        robot_directory_name = path.split('/')[-1]                  # <robot>
+        path = path + '/../../ros/' + robot_directory_name + '/'    # /path/to/pyrobolearn/robots/ros/<robot>/
+
+        # check if valid robot directory
+        # if os.path.isdir(path):
+        robot_path = '/'.join(path.split('/')[-5:-2])
+        if robot_path == 'pyrobolearn/robots/urdfs':
+
+            # get corresponding subscriber/publisher
+            def check_ros(name, dictionary, id_):
+                # TODO: do I really need a class for each robot? Can I not just use RobotPublisher?
+                # if os.path.isfile(path + name + '.py'):
+                #     module = importlib.import_module('pyrobolearn.robots.ros.' + robot_directory_name + '.' + name)
+                #     classes = inspect.getmembers(module, inspect.isclass)  # list of (name, class)
+                #     length = len(name)
+                #     robot_name = ''.join(robot_directory_name.split('_'))
+                #
+                #     # go through each class and get the :attr:`name` corresponding to the robot and add it to the
+                #     # given :attr:`dictionary`
+                #     for name, cls in classes:
+                #         if name[:-length].lower() == robot_name:
+                #             dictionary[id_] = cls(id_=id_)
+                #             break
+                module = importlib.import_module('pyrobolearn.robots.ros.' + name)
+                classes = dict(inspect.getmembers(module, inspect.isclass))
+                cls = classes['Robot' + name.capitalize()]
+                dictionary[id_] = cls(name=robot_directory_name, id_=id_)
+
+            # load subscriber in simulator
+            if self.subscribe:
+                check_ros('subscriber', self.subscribers, id_)
+
+            # load publisher in simulator
+            if self.publish:
+                check_ros('publisher', self.publishers, id_)
+
+        return id_
 
     def get_joint_positions(self, body_id, joint_ids):
         """
@@ -158,7 +200,7 @@ class ROS(Simulator):
                 np.float[N]: joint positions [rad]
         """
         if body_id in self.subscribers:
-            return self.subscribers[body_id].get_joint_positions[joint_ids]
+            return self.subscribers[body_id].get_joint_positions(joint_ids)
 
     def set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None):
         """
@@ -175,6 +217,7 @@ class ROS(Simulator):
         """
         if body_id in self.publishers:
             self.publishers[body_id].set_joint_positions(joint_ids, positions)
+            self.publishers[body_id].publish('joint_states')
 
     def get_joint_velocities(self, body_id, joint_ids):
         """
@@ -190,7 +233,8 @@ class ROS(Simulator):
             if multiple joints:
                 np.float[N]: joint velocities [rad/s]
         """
-        pass
+        if body_id in self.subscribers:
+            return self.subscribers[body_id].get_joint_velocities(joint_ids)
 
     def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None):
         """
@@ -202,7 +246,9 @@ class ROS(Simulator):
             velocities (float, np.float[N]): desired velocity, or list of desired velocities [rad/s]
             max_force (None, float, np.float[N]): maximum motor forces/torques
         """
-        pass
+        if body_id in self.publishers:
+            self.publishers[body_id].set_joint_velocities(joint_ids, velocities)
+            self.publishers[body_id].publish('joint_states')
 
     def get_joint_torques(self, body_id, joint_ids):
         """
@@ -220,7 +266,8 @@ class ROS(Simulator):
             if multiple joints:
                 np.float[N]: torques associated to the given joints [Nm]
         """
-        pass
+        if body_id in self.subscribers:
+            return self.subscribers[body_id].get_joint_torques(joint_ids)
 
     def set_joint_torques(self, body_id, joint_ids, torques):
         """
@@ -231,4 +278,6 @@ class ROS(Simulator):
             joint_ids (int, list of int): joint id, or list of joint ids.
             torques (float, list of float): desired torque(s) to apply to the joint(s) [N].
         """
-        pass
+        if body_id in self.publishers:
+            self.publishers[body_id].set_joint_torques(joint_ids, torques)
+            self.publishers[body_id].publish('joint_states')