add base pose/vel states + add static desc. methods for simulators

pyrobolearn/robots/robot.py

@@ -1141,11 +1141,9 @@ class Robot(ControllableBody):
             if multiple links:
                 str[N]: link names
         """
-        if isinstance(link_ids, int):
-            return self.sim.get_joint_info(self.id, link_ids)[12]
         if link_ids is None:
             link_ids = self.joints
-        return [self.sim.get_joint_info(self.id, link)[12] for link in link_ids]
+        return self.sim.get_link_names(self.id, link_ids)
 
     def get_link_masses(self, link_ids=None):
         r"""
@@ -1896,10 +1894,10 @@ class Robot(ControllableBody):
                 based on the current joint positions.
 
         Returns:
-            np.array[6,6,N]: CoM Jacobian
+            np.array[6,N]: CoM Jacobian
 
         References:
-            [1] "Whole-body cooperative balancing of humanoid robot using COG Jacobian", Sugihara et al., IROS, 2002
+            - [1] "Whole-body cooperative balancing of humanoid robot using COG Jacobian", Sugihara et al., IROS, 2002
         """
         # Get current joint position
         if q is None:

pyrobolearn/simulators/bullet.py

@@ -13,9 +13,9 @@ Dependencies in PRL:
 * `pyrobolearn.simulators.simulator.Simulator`
 
 References:
-    [1] PyBullet: https://pybullet.org
-    [2] PyBullet Quickstart Guide: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
-    [3] PEP8: https://www.python.org/dev/peps/pep-0008/
+    - [1] PyBullet: https://pybullet.org
+    - [2] PyBullet Quickstart Guide: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
+    - [3] PEP8: https://www.python.org/dev/peps/pep-0008/
 """
 
 # general imports
@@ -74,9 +74,9 @@ class Bullet(Simulator):
         sim = Bullet()
 
     References:
-        [1] "PyBullet, a Python module for physics simulation for games, robotics and machine learning", Erwin Coumans
-            and Yunfei Bai, 2016-2019
-        [1] PyBullet Quickstart Guide: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
+        - [1] "PyBullet, a Python module for physics simulation for games, robotics and machine learning", Erwin
+            Coumans and Yunfei Bai, 2016-2019
+        - [2] PyBullet Quickstart Guide: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
             Erwin Coumans and Yunfei Bai, 2017/2018
     """
 
@@ -228,6 +228,15 @@ class Bullet(Simulator):
         memo[self] = sim
         return sim
 
+    ##################
+    # Static methods #
+    ##################
+
+    @staticmethod
+    def simulate_soft_bodies():
+        """Return True if the simulator can simulate soft bodies."""
+        return True
+
     ###########
     # Methods #
     ###########
@@ -1728,8 +1737,17 @@ class Bullet(Simulator):
                 str[N]: link names
         """
         if isinstance(link_ids, int):
+            if link_ids == -1:
+                return self.get_base_name(body_id)
             return self.sim.getJointInfo(body_id, link_ids)[12]
-        return [self.sim.getJointInfo(body_id, link_id)[12] for link_id in link_ids]
+
+        link_names = []
+        for link_id in link_ids:
+            if link_id == -1:
+                link_names.append(self.get_base_name(body_id))
+            else:
+                link_names.append(self.sim.getJointInfo(body_id, link_id)[12])
+        return link_names
 
     def get_link_masses(self, body_id, link_ids):
         """

pyrobolearn/simulators/bullet_ros.py

@@ -22,7 +22,8 @@ References:
     - [4] PEP8: https://www.python.org/dev/peps/pep-0008/
 """
 
-# TODO
+# TODO: finish this interface and move ROS stuffs to ros.py
+
 import os
 import subprocess
 import psutil
@@ -93,6 +94,33 @@ class BulletROS(Bullet):  # , ROS):
         self.subscribers = {}
         self.publishers = {}
 
+    ##############
+    # Properties #
+    ##############
+
+    @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
+
+    ##################
+    # Static methods #
+    ##################
+
+    @staticmethod
+    def has_middleware_communication_layer():
+        """Return True if the simulator has a middleware communication layer (like ROS, YARP, etc)."""
+        return True
+
+    ###########
+    # Methods #
+    ###########
+
     def close(self):
         """
         Close everything
@@ -108,16 +136,6 @@ class BulletROS(Bullet):  # , ROS):
         # call parent destructor
         super(BulletROS, self).close()
 
-    @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.

pyrobolearn/simulators/rbdl_.py

@@ -0,0 +1,134 @@
+#!/usr/bin/env python
+"""Python wrapper around RBDL
+
+The signature of each method defined here are inspired by the `Robot` class in PyRoboLearn and [1] but in accordance
+with the PEP8 style guide [2]. Most of the documentation for the methods have been copied-pasted from [1] for
+completeness purposes.
+
+References:
+    - [1] RBDL:
+        - Webpage (with documentation): https://rbdl.bitbucket.io/
+        - Bitbucket repository: https://bitbucket.org/rbdl/rbdl/
+    - [2] PEP8: https://www.python.org/dev/peps/pep-0008/
+"""
+
+import collections
+import rbdl
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Martin Felis (martin@fysx.org)"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class RBDL(object):
+    r"""RBDL Interface.
+
+    References:
+    - [1] RBDL:
+        - Webpage (with documentation): https://rbdl.bitbucket.io/
+        - Bitbucket repository: https://bitbucket.org/rbdl/rbdl/
+    - [2] PEP8: https://www.python.org/dev/peps/pep-0008/
+    """
+
+    def __init__(self, filename=None, verbose=False, floating_base=False):
+        self.model = None
+        self.load_model(filename, verbose=verbose, floating_base=floating_base)
+
+    def load_model(self, filename, verbose=False, floating_base=False):
+        """
+        Load the given URDF model.
+
+        Args:
+            filename (str): path to the urdf model.
+            verbose (bool): if True, it will output
+            floating_base (bool): if True, the model will be considered to have a floating base, thus 6 more DoFs will
+                be added.
+        """
+        self.model = rbdl.loadModel(filename, verbose=verbose, floating_base=floating_base)
+
+    # alias
+    load_urdf = load_model
+
+    @property
+    def num_dofs(self):
+        """Return the number of degrees of freedom (DoFs); that is, if the base is not fixed, 6 (= 3 degrees for
+        translation + 3 degrees for orientation) + the joints that are not fixed.
+        """
+        return self.model.dof_count
+
+    # alias
+    dof_count = num_dofs
+
+    @property
+    def gravity(self):
+        """Return the Cartesian gravity vector applied on the model."""
+        return self.model.gravity
+
+    @property
+    def num_joints(self):
+        """Return the number of joints: ROOT + Num DoFs"""
+        return len(self.model.mJoints)
+
+    @property
+    def num_fixed_links(self):
+        """Return the number of fixed links."""
+        return len(self.model.mFixedBodies)
+
+    @property
+    def num_links(self):
+        """Return the number of links."""
+        return len(self.model.mBodies)
+
+    @property
+    def num_actuated_joints(self):
+        """Return the number of actuated joints."""
+        return self.model.q_size
+
+    @property
+    def fixed_joints(self):
+        """Return the list of fixed joints"""
+        return []  # TODO
+
+    def get_link_names(self, link_ids):
+        """Return the link names."""
+        if isinstance(link_ids, collections.Iterable):
+            return [self.model.GetBodyName(link_id) for link_id in link_ids]
+        return self.model.GetBodyName(link_ids)
+
+
+# Test
+if __name__ == '__main__':
+    import os
+    import pyrobolearn as prl
+
+    fixed_base = False
+
+    robot = prl.robots.HyQ2Max(prl.simulators.Bullet(render=False), fixed_base=fixed_base)
+    rbdl_ = RBDL(os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/hyq2max/hyq2max.urdf',
+                 floating_base=not fixed_base)
+
+    robots = [robot, rbdl_]
+
+    def print_attr(msg, attr, *args, **kwargs):
+        for rob in robots:
+            a = getattr(rob, attr)
+            if callable(a):
+                print(msg.format(a(*args, **kwargs)))
+            print(msg.format(a))
+
+    print_attr("Num DoFs: {}", 'num_dofs')
+    print_attr("Num of actuated joints: {}", 'num_actuated_joints')
+    print_attr("Num of joints: {}", 'num_joints')
+    print_attr("Num of links: {}", 'num_links')
+
+    print(robot.get_link_names([-1] + range(robot.num_links)))
+    print(rbdl_.get_link_names(range(rbdl_.num_links)))
+
+    print("Num fixed links: {}".format(robot.num_links - robot.num_actuated_joints))
+    print("Num fixed links: {}".format(rbdl_.num_fixed_links))

pyrobolearn/simulators/simulator.py

@@ -262,6 +262,35 @@ class Simulator(object):
         memo[self] = sim
         return sim
 
+    ##################
+    # Static methods #
+    ##################
+
+    @staticmethod
+    def simulate_gas_dynamics():
+        """Return True if the simulator can simulate gases."""
+        return False
+
+    @staticmethod
+    def simulate_liquid_dynamics():
+        """Return True if the simulator can simulate liquids."""
+        return False
+
+    @staticmethod
+    def simulate_fluid_dynamics():
+        """Return True if the simulator can simulate fluids (gases and liquids)."""
+        return Simulator.simulate_gas_dynamics() and Simulator.simulate_liquid_dynamics()
+
+    @staticmethod
+    def simulate_soft_bodies():
+        """Return True if the simulator can simulate soft bodies."""
+        return False
+
+    @staticmethod
+    def has_middleware_communication_layer():
+        """Return True if the simulator has a middleware communication layer (like ROS, YARP, etc)."""
+        return False
+
     ###########
     # Methods #
     ###########

pyrobolearn/states/robot_states/robot_states.py

@@ -182,6 +182,40 @@ class BaseOrientationState(RobotState):
         self.data = self.robot.get_base_orientation()
 
 
+class BasePoseState(RobotState):
+    r"""Base pose state
+
+    This is the state that returns the base position [x,y,z] and orientation (expressed as a quaternion [x,y,z,w])
+    with respect to the world frame.
+    """
+
+    def __init__(self, robot, window_size=1, axis=None, ticks=1):
+        """
+        Initialize the base pose state.
+
+        Args:
+            robot (Robot): instance of Robot which allows to access to the robot state
+            window_size (int): window size of the state. This is the total number of states we should remember. That
+                is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`,
+                the window size is 2. By default, the :attr:`window_size` is one which means we only remember the
+                current state. The window size has to be bigger than 1. If it is below, it will be set automatically
+                to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states,
+                but is given some :attr:`data`.
+            axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with
+                shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting
+                state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack
+                the states in the specified axis. With the example, for axis=0, the resulting state has a shape of
+                (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the
+                state is not a combination of states, but is given some :attr:`data`.
+            ticks (int): number of ticks to sleep before getting the next state data.
+        """
+        super(BasePoseState, self).__init__(robot, window_size=window_size, axis=axis, ticks=ticks)
+
+    def _read(self):
+        """Read the base position state data."""
+        self.data = self.robot.get_base_pose()
+
+
 class BaseLinearVelocityState(RobotState):
     r"""Base linear velocity state
 
@@ -246,3 +280,37 @@ class BaseAngularVelocityState(RobotState):
     def _read(self):
         """Read the base angular velocity state data."""
         self.data = self.robot.get_base_angular_velocity()
+
+
+class BaseVelocityState(RobotState):
+    r"""Base velocity state
+
+    This is the state that returns the base linear [vx, vy, vz] and angular velocity [wx, wy, wz] with respect to the
+    world frame.
+    """
+
+    def __init__(self, robot, window_size=1, axis=None, ticks=1):
+        """
+        Initialize the base velocity state.
+
+        Args:
+            robot (Robot): instance of Robot which allows to access to the robot state
+            window_size (int): window size of the state. This is the total number of states we should remember. That
+                is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`,
+                the window size is 2. By default, the :attr:`window_size` is one which means we only remember the
+                current state. The window size has to be bigger than 1. If it is below, it will be set automatically
+                to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states,
+                but is given some :attr:`data`.
+            axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with
+                shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting
+                state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack
+                the states in the specified axis. With the example, for axis=0, the resulting state has a shape of
+                (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the
+                state is not a combination of states, but is given some :attr:`data`.
+            ticks (int): number of ticks to sleep before getting the next state data.
+        """
+        super(BaseVelocityState, self).__init__(robot, window_size=window_size, axis=axis, ticks=ticks)
+
+    def _read(self):
+        """Read the base linear velocity state data."""
+        self.data = self.robot.get_base_velocity()

pyrobolearn/tools/interfaces/interface.py

@@ -94,6 +94,8 @@ class Interface(object):
         if self.use_thread:
             while True:
                 if self.stop_thread:  # if the thread should stop
+                    if self.verbose:
+                        print("Stopping the thread")
                     break
                 self.run(*args, **kwargs)
                 time.sleep(self.dt)
@@ -112,6 +114,8 @@ class Interface(object):
         Stop and close the interface.
         """
         if self.use_thread:
+            if self.verbose:
+                print("Asking for thread to stop...")
             self.stop_thread = True
 
     #############

pyrobolearn/tools/interfaces/phones/__init__.py

@@ -0,0 +1,3 @@
+
+# import the abstract phone interface
+from .phone import PhoneInterface  # , TabletInterface

pyrobolearn/tools/interfaces/phones/android.py

@@ -0,0 +1,37 @@
+#!/usr/bin/env python
+"""Define the android interface.
+"""
+
+# TODO: finish this interface
+# TODO: implement android application (Kotlin or Java)
+
+from pyrobolearn.tools.interfaces.phones import PhoneInterface
+
+
+__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 AndroidInterface(PhoneInterface):
+    r"""Android phone/tablet interface
+    """
+
+    def __init__(self, use_thread=False, sleep_dt=0, verbose=False):
+        """
+        Initialize the android interface.
+
+        Args:
+            use_thread (bool): If True, it will run the interface in a separate thread than the main one.
+                The interface will update its data automatically.
+            sleep_dt (float): If :attr:`use_thread` is True, it will sleep the specified amount before acquiring /
+                setting the next sample.
+            verbose (bool): If True, it will print information about the state of the interface. This is let to the
+                programmer what he / she wishes to print.
+        """
+        super(AndroidInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose)

pyrobolearn/tools/interfaces/phones/iphone.py

@@ -0,0 +1,37 @@
+#!/usr/bin/env python
+"""Define the iPhone interface.
+"""
+
+# TODO: finish this interface
+# TODO: implement iphone application (Swift)
+
+from pyrobolearn.tools.interfaces.phones import PhoneInterface
+
+
+__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 IPhoneInterface(PhoneInterface):
+    r"""IPhone / IPad interface
+    """
+
+    def __init__(self, use_thread=False, sleep_dt=0, verbose=False):
+        """
+        Initialize the iphone interface.
+
+        Args:
+            use_thread (bool): If True, it will run the interface in a separate thread than the main one.
+                The interface will update its data automatically.
+            sleep_dt (float): If :attr:`use_thread` is True, it will sleep the specified amount before acquiring /
+                setting the next sample.
+            verbose (bool): If True, it will print information about the state of the interface. This is let to the
+                programmer what he / she wishes to print.
+        """
+        super(IPhoneInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose)

pyrobolearn/tools/interfaces/phones/phone.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python
+"""Define the smartphone and tablet interface.
+
+These interfaces allows you to get sensor values sent with your phone or tablet, and values returned by the
+corresponding (Android / IPhone) application.
+"""
+
+from pyrobolearn.tools.interfaces import InputOutputInterface
+
+
+__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"
+
+
+# TODO: define common interface for phone and tablet?
+
+class PhoneInterface(InputOutputInterface):
+    r"""Smart-Phone Interface
+    """
+
+    def __init__(self, use_thread=False, sleep_dt=0, verbose=False):
+        """
+        Initialize the smartphone interface.
+
+        Args:
+            use_thread (bool): If True, it will run the interface in a separate thread than the main one.
+                The interface will update its data automatically.
+            sleep_dt (float): If :attr:`use_thread` is True, it will sleep the specified amount before acquiring /
+                setting the next sample.
+            verbose (bool): If True, it will print information about the state of the interface. This is let to the
+                programmer what he / she wishes to print.
+        """
+        super(PhoneInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose)
+
+
+# class TabletInterface(InputOutputInterface):
+#     r"""Tablet Interface
+#     """
+#
+#     def __init__(self, use_thread=False, sleep_dt=0, verbose=False):
+#         """
+#         Initialize the tablet interface.
+#
+#         Args:
+#             use_thread (bool): If True, it will run the interface in a separate thread than the main one.
+#                 The interface will update its data automatically.
+#             sleep_dt (float): If :attr:`use_thread` is True, it will sleep the specified amount before acquiring /
+#                 setting the next sample.
+#             verbose (bool): If True, it will print information about the state of the interface. This is let to the
+#                 programmer what he / she wishes to print.
+#         """
+#         super(TabletInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose)

pyrobolearn/worlds/world.py

@@ -1335,7 +1335,7 @@ class World(object):
             position (float[3]): position of the box in the Cartesian world space (in meters)
             orientation (float[4]): orientation of the box using quaternion [x,y,z,w].
             mass (float): mass of the box (in kg). If mass = 0, the box won't move even if there is a collision.
-            dimensions (float[3]): dimensions of the box
+            dimensions (float[3]): dimensions of the box (in meter)
             color (int[4], None): color of the box for red, green, blue, and alpha, each in range [0,1]
             return_body (bool): if True, it will return an instance of the `Body`, otherwise, it will return the
                 unique id.