refactor + update simulators and physics randomizers

README.md

@@ -3,6 +3,8 @@
 This repository contains the code for the *PyRoboLearn* (PRL) framework: a Python framework for Robot Learning.
 This framework revolves mainly around 7 axes: simulators, worlds, robots, interfaces, learning tasks (= environment and policy), learning models, and learning algorithms. 
 
+This development of this framework is ongoing.
+
 ## Requirements
 
 The framework has been tested with Python 2.7 and Ubuntu 16.04 and 18.04. We also tested parts of it with Python 3.5 on Ubuntu 16.04 and so far so good, but there might be some errors that escaped me.

pyrobolearn/physics/README.md

@@ -0,0 +1,9 @@
+## Physics randomizer
+
+This folder provides physics randomizers which randomizes the dynamical attributes / properties of an object.
+For instance, it can randomize the mass or inertial matrix of a link, the bounciness of an object, the gravity of 
+the world, the contact friction coefficients of the floor and various links, and so on.
+
+Note that physics randomizer instances have access to the simulator in order to modify the physical properties.
+Also, note that normally the physics randomizer is called at the beginning of an episode, and not at each time
+step. Changing the physical properties at each time step can lead to weird behaviors in the simulator.

pyrobolearn/physics/__init__.py

@@ -0,0 +1,18 @@
+
+# import physics
+from .physics_randomizer import *
+
+# import world physics randomizer
+from .world_physics_randomizer import *
+
+# import body physics randomizer
+from .body_physics_randomizer import *
+
+# import link physics randomizer
+from .link_physics_randomizer import *
+
+# import joint physics randomizer
+from .joint_physics_randomizer import *
+
+# import robot physics randomizer
+from .robot_physics_randomizer import *

pyrobolearn/physics/body_physics_randomizer.py

@@ -0,0 +1,66 @@
+#!/usr/bin/env python
+"""Define the `BodyPhysicsRandomizer` class which randomizes the physical attributes / properties of a body.
+
+Dependencies:
+- `pyrobolearn.physics`
+- `pyrobolearn.robots`
+"""
+
+from pyrobolearn.physics.physics_randomizer import PhysicsRandomizer
+# from pyrobolearn.robots.base import Object  # TODO: change to Body or MultiBody
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class BodyPhysicsRandomizer(PhysicsRandomizer):
+    r"""Body Physics Randomizer
+
+    The body physics randomizer can randomize the physical attributes of a body. It is an abstract class which is
+    inherited notably by `LinkPhysicsRandomizer` and `JointPhysicsRandomizer`.
+    """
+
+    def __init__(self, body):
+        """
+        Initialize the body physics randomizer.
+
+        Args:
+            body (Body): multi-body object.
+        """
+        self.body = body
+        simulator = self.body.sim
+        super(BodyPhysicsRandomizer, self).__init__(simulator)
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def body(self):
+        """Return the body / object instance."""
+        return self._body
+
+    @body.setter
+    def body(self, body):
+        """Set the body / object instance."""
+        # TODO: uncomment the following lines
+        # if not isinstance(body, Object):
+        #     raise TypeError("Expecting the given body to be an instance of `Object`, instead got: "
+        #                     "{}".format(type(body)))
+        self._body = body
+
+    @property
+    def num_links(self):
+        """Return the number of links of the body."""
+        return self.body.num_links
+
+    @property
+    def num_joints(self):
+        """Return the number of joints of the body."""
+        return self.body.num_joints

pyrobolearn/physics/joint_physics_randomizer.py

@@ -0,0 +1,118 @@
+#!/usr/bin/env python
+"""Define the `JointPhysicsRandomizer` class which randomizes the physical attributes / properties of a joint or
+multiple joints of a specific body.
+
+Dependencies:
+- `pyrobolearn.physics`
+"""
+
+import collections
+
+from pyrobolearn.physics.body_physics_randomizer import BodyPhysicsRandomizer
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class JointPhysicsRandomizer(BodyPhysicsRandomizer):
+    r"""Joint Physics Randomizer
+
+    The joint physics randomizer can randomize the physical attributes of a joint. For instance, this can be the
+    joint friction or damping coefficients. Other attributes can be the maximum force or velocity the joint(s) can
+    achieve.
+    """
+
+    def __init__(self, body, joint_ids=None, joint_damping=None, **kwargs):
+        """
+        Initialize the joint physics randomizer.
+
+        Args:
+            body (Body): multi-body object.
+            joint_ids (int, list of int, None): joint id(s).
+            joint_damping (float, list of float, tuple of float, list of tuple of float, None): joint damping
+                coefficient. If None, it will take the default joint damping value associated with the given
+                `joint_ids` of the given `body`. If float, it will set that value to the specified joints and will
+                always return this value when sampling. If list of float, it will set each value to each joint and will
+                always return these values when sampling. If tuple of float, the first item is the lower bound of the
+                joint damping and the second item is its upper bound. It will set these bounds for each joint. If list
+                of tuples of joints, it will have a tuple of lower / upper bound for each joint.
+            **kwargs (dict): range of possible physical properties. If given one value, that property won't be
+                randomized. Each range is a tuple of two values `[lower_bound, upper_bound]`.
+        """
+        super(JointPhysicsRandomizer, self).__init__(body)
+        self.joints = joint_ids
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def joints(self):
+        """Return the list of joint ids."""
+        return self._joints
+
+    @joints.setter
+    def joints(self, joints):
+        """Set the joint id or the list of joint ids."""
+        if joints is None:
+            joints = self.body.joints
+        elif isinstance(joints, int):
+            joints = [joints]
+        elif isinstance(joints, collections.Iterable):
+            for idx, joint in enumerate(joints):
+                if not isinstance(joint, int):
+                    raise TypeError("The {} element of the given list of joints is not an integer, instead got: "
+                                    "{}".format(idx, type(joint)))
+        else:
+            raise TypeError("Expecting the given joints to be an integer or a list of integers, instead got: "
+                            "{}".format(type(joints)))
+        self._joints = joints
+
+    @property
+    def joint_dampings(self):
+        """Return the joint dampings associated with the joints."""
+        return self.body.get_joint_dampings(self.joints)
+
+    @joint_dampings.setter
+    def joint_dampings(self, values):
+        """Set the given joint damping values to each joint."""
+        for joint, value in zip(self.joints, values):
+            self.body.set_joint_damping(joint, value)
+
+    ###########
+    # Methods #
+    ###########
+
+    def names(self):
+        """Return an iterator over the property names."""
+        for name in ['joint_damping']:
+            yield name
+
+    def bounds(self):
+        """Return an iterator over the bounds for each property."""
+        pass
+
+    def get_properties(self):
+        """
+        Get the physics properties.
+
+        Returns:
+            dict: current physic property values.
+        """
+        pass
+
+    def set_properties(self, properties):
+        """
+        Set the given physic property values using the simulator.
+
+        Args:
+            properties (dict): the physic property values to be set in the simulator.
+        """
+        pass

pyrobolearn/physics/link_physics_randomizer.py

@@ -0,0 +1,134 @@
+#!/usr/bin/env python
+"""Define the `LinkPhysicsRandomizer` class which randomizes the physical attributes / properties of a link or
+multiple links of a specific body.
+
+Dependencies:
+- `pyrobolearn.physics`
+"""
+
+import collections
+
+from pyrobolearn.physics.body_physics_randomizer import BodyPhysicsRandomizer
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class LinkPhysicsRandomizer(BodyPhysicsRandomizer):
+    r"""Link Physics Randomizer
+
+    The link physics randomizer can randomize the physical attributes of a link.
+    """
+
+    def __init__(self, body, link_ids=None, masses=None, local_inertia_diagonals=None, local_inertia_positions=None,
+                 local_inertia_orientations=None, lateral_frictions=None, spinning_frictions=None,
+                 rolling_frictions=None, restitutions=None, linear_dampings=None, angular_dampings=None,
+                 contact_stiffnesses=None, contact_dampings=None, **kwargs):
+        """
+        Initialize the link physics randomizer.
+
+        Args:
+            body (Body): multi-body object.
+            link_ids (int, list of int, None): link id(s).
+            **kwargs (dict): range of possible physical properties. If given one value, that property won't be
+                randomized. Each range is a tuple of two values `[lower_bound, upper_bound]`.
+        """
+        super(LinkPhysicsRandomizer, self).__init__(body)
+        self.links = link_ids
+
+        # set the bounds
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def links(self):
+        """Return the list of link ids."""
+        return self._links
+
+    @links.setter
+    def links(self, links):
+        """Set the link id or the list of link ids."""
+        if isinstance(links, int):
+            links = [links]
+        elif isinstance(links, collections.Iterable):
+            for idx, link in enumerate(links):
+                if not isinstance(link, int):
+                    raise TypeError("The {} element of the given list of links is not an integer, instead got: "
+                                    "{}".format(idx, type(link)))
+        else:
+            raise TypeError("Expecting the given links to be an integer or a list of integers, instead got: "
+                            "{}".format(type(links)))
+        self._links = links
+
+    @property
+    def masses(self):
+        """Return the mass of each specified link."""
+        return self.body.get_masses(self.links)
+
+    @masses.setter
+    def masses(self, values):
+        """Set the mass values."""
+        self.body.set_masses(self.links, values)
+
+    @property
+    def mass_bounds(self):
+        """Return the lower and upper bounds of each link mass."""
+        return self._mass_bounds
+
+    @mass_bounds.setter
+    def mass_bounds(self, bounds):
+        """Set the mass bound for each link."""
+        if isinstance(bounds, (float, int)):
+            bounds = [(bounds, bounds) for _ in self.links]
+        elif isinstance(bounds, (list, tuple, np.ndarray)):
+            pass
+        self._mass_bounds = bounds
+
+    @property
+    def dynamics(self):
+        return None
+
+    ###########
+    # Methods #
+    ###########
+
+    def names(self):
+        """Return an iterator over the property names."""
+        for name in ['mass']:
+            yield name
+
+    def bounds(self):
+        """Return an iterator over the bounds for each property."""
+        yield self.mass_bounds
+
+    def get_properties(self):
+        """
+        Get the physics properties.
+
+        Returns:
+            dict: current physic property values.
+        """
+        properties = dict()
+        # properties['mass'] =
+        return properties
+
+    def set_properties(self, properties):
+        """
+        Set the given physic property values using the simulator.
+
+        Args:
+            properties (dict): the physic property values to be set in the simulator.
+        """
+        if not isinstance(properties, dict):
+            raise TypeError("Expecting the given 'properties' to be a dictionary, instead got: "
+                            "{}".format(type(properties)))
+

pyrobolearn/physics/physics_randomizer.py

@@ -0,0 +1,158 @@
+#!/usr/bin/env python
+"""Define the `PhysicsRandomizer` class which randomizes the physical attributes / properties of an object.
+
+This is the main abstract class from which all physics randomizers inherit from.
+
+Dependencies:
+- `pyrobolearn.simulators`
+"""
+
+import numpy as np
+
+from pyrobolearn.simulators import Simulator
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class PhysicsRandomizer(object):
+    r"""Physics Randomizer
+
+    This the main abstract class from which all the physics randomizers inherit from. A physics randomizer randomize
+    the physics properties of a certain object. This can be,  for instance, the world, a robot, or a particular link
+    of that robot.
+
+    It can change for example the dynamics of a particular object such as the mass, inertia, or others. It can also
+    change their physical properties such as the friction, bounciness, etc.
+
+    Note that the physics randomizer instance has access to the simulator in order to modify the physical properties.
+    Also, note that normally the physics randomizer is called at the beginning of an episode, and not at each time
+    step. Changing the physical properties at each time step can lead to weird behaviors.
+
+    It is possible to not randomize some physical properties by specifying a specific value instead of a range (=tuple
+    of 2 values; lower and upper bound).
+    """
+
+    def __init__(self, simulator):
+        """
+        Initialize the physics randomizer.
+
+        Args:
+            simulator (Simulator): simulator instance
+        """
+        self.simulator = simulator
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def simulator(self):
+        """Return the simulator instance."""
+        return self._simulator
+
+    @simulator.setter
+    def simulator(self, simulator):
+        """Set the simulator instance."""
+        # TODO: uncomment the following lines
+        # if not isinstance(simulator, Simulator):
+        #     raise TypeError("Expecting the given simulator to be an instance of `Simulator`, instead got: "
+        #                     "{}".format(type(simulator)))
+        self._simulator = simulator
+
+    ###########
+    # Methods #
+    ###########
+
+    def properties(self):
+        """Return an iterator over the properties."""
+        properties = self.get_properties()
+        for p in properties.values():
+            yield p
+
+    def named_properties(self):
+        """Return an iterator over the properties with their name and value"""
+        properties = self.get_properties()
+        for name, p in properties.items():
+            yield name, p
+
+    def names(self):
+        """Return an iterator over the property names."""
+        pass
+
+    def bounds(self):
+        """Return an iterator over the bounds for each property."""
+        pass
+
+    def named_bounds(self):
+        """Return an iterator over the property bounds with their name and value."""
+        for name, bound in zip(self.names(), self.bounds()):
+            yield name, bound
+
+    def get_properties(self):
+        """
+        Get the physics properties.
+
+        Returns:
+            dict: current physic property values.
+        """
+        pass
+
+    def set_properties(self, properties):
+        """
+        Set the given physic property values using the simulator.
+
+        Args:
+            properties (dict): the physic property values to be set in the simulator.
+        """
+        pass
+
+    def sample(self, seed=None):
+        """
+        Sample a new set of physics properties and returns it. Note that it doesn't set them in the simulator.
+        This sampling can be useful if the user wishes to check more carefully the sampled physic property values.
+        Once satisfied, the user can set them by calling the `set_properties` method.
+
+        Note that it samples uniformly the physics properties between their specified lower and upper bounds.
+
+        Args:
+            seed (int, None): random seed.
+
+        Returns:
+            dict: sampled physic properties.
+        """
+        # set random seed
+        if seed is not None:
+            np.random.seed(seed)
+
+        # sample each property
+        properties = dict()
+        for name, bound in zip(self.names(), self.bounds()):
+            properties[name] = np.random.uniform(low=bound[0], high=bound[1])
+        return properties
+
+    def randomize(self, seed=None):
+        """
+        Randomize the physics properties and set them in the simulator.
+
+        Args:
+            seed (int, None): random seed.
+        """
+        sampled_properties = self.sample(seed)
+        self.set_properties(sampled_properties)
+
+    def seed(self, seed=None):
+        """
+        Set the given seed when sampling or randomizing the environment.
+
+        Args:
+            seed (int): random seed.
+        """
+        if seed is not None:
+            np.random.seed(seed)

pyrobolearn/physics/robot_physics_randomizer.py

@@ -0,0 +1,135 @@
+#!/usr/bin/env python
+"""Define the `RobotPhysicsRandomizer` class which randomizes the physical attributes / properties of links and joints.
+
+Dependencies:
+- `pyrobolearn.physics`
+"""
+
+import collections
+
+from pyrobolearn.physics.body_physics_randomizer import BodyPhysicsRandomizer
+from pyrobolearn.physics.link_physics_randomizer import LinkPhysicsRandomizer
+from pyrobolearn.physics.joint_physics_randomizer import JointPhysicsRandomizer
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class RobotPhysicsRandomizer(BodyPhysicsRandomizer):
+    r"""Robot Physics Randomizer
+
+    The robot physics randomizer can randomize the physical attributes of a robot. It can notably change its mass,
+    the contact frictions, the inertia of the links, the friction and damping coefficients of the joints, etc.
+    """
+
+    def __init__(self, body, links=None, joints=None, **kwargs):
+        """
+        Initialize the robot physics randomizer.
+
+        Args:
+            body (Body): multi-body object.
+            links (int, list of int, LinkPhysicsRandomizer, list of LinkPhysicsRandomizer, None): link id(s) or link
+                physics randomizer(s).
+            joints (int, list of int, JointPhysicsRandomizer, list JointPhysicsRandomizer, None): joint id(s) or joint
+                physics randomizer.
+            **kwargs (dict): range of possible physical properties. If given one value, that property won't be
+                randomized. Each range is a tuple of two values `[lower_bound, upper_bound]`.
+        """
+        super(RobotPhysicsRandomizer, self).__init__(body)
+        self.links = links
+        self.joints = joints
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def links(self):
+        """Return the list of link physics randomizers."""
+        return self._links
+
+    @links.setter
+    def links(self, links):
+        """Set the link physics randomizer or the list of link physics randomizers."""
+        if isinstance(links, int):
+            links = [LinkPhysicsRandomizer(self.body, links)]
+        elif isinstance(links, LinkPhysicsRandomizer):
+            links = [links]
+        elif isinstance(links, collections.Iterable):
+            link_list = []
+            for idx, link in enumerate(links):
+                if isinstance(link, int):
+                    link = LinkPhysicsRandomizer(self.body, links)
+                elif not isinstance(link, LinkPhysicsRandomizer):
+                    raise TypeError("The {} element of the given list of links is not an integer or a "
+                                    "LinkPhysicsRandomizer, instead got: {}".format(idx, type(link)))
+                link_list.append(link)
+            links = link_list
+        else:
+            raise TypeError("Expecting the given links to be an integer / `LinkPhysicsRandomizer` or a list of "
+                            "integers / `LinkPhysicsRandomizer`, instead got: {}".format(type(links)))
+        self._links = links
+
+    @property
+    def joints(self):
+        """Return the list of joint physics randomizers."""
+        return self._joints
+
+    @joints.setter
+    def joints(self, joints):
+        """Set the joint physics randomizer or the list of joint physics randomizers."""
+        if isinstance(joints, int):
+            joints = [JointPhysicsRandomizer(self.body, joints)]
+        elif isinstance(joints, JointPhysicsRandomizer):
+            joints = [joints]
+        elif isinstance(joints, collections.Iterable):
+            joint_list = []
+            for idx, joint in enumerate(joints):
+                if isinstance(joint, int):
+                    joint = JointPhysicsRandomizer(self.body, joints)
+                elif not isinstance(joint, JointPhysicsRandomizer):
+                    raise TypeError("The {} element of the given list of joints is not an integer or a "
+                                    "JointPhysicsRandomizer, instead got: {}".format(idx, type(joint)))
+                joint_list.append(joint)
+            joints = joint_list
+        else:
+            raise TypeError("Expecting the given joints to be an integer / `JointPhysicsRandomizer` or a list of "
+                            "integers / `JointPhysicsRandomizer`, instead got: {}".format(type(joints)))
+        self._joints = joints
+
+    ###########
+    # Methods #
+    ###########
+
+    def names(self):
+        """Return an iterator over the property names."""
+        pass
+
+    def bounds(self):
+        """Return an iterator over the bounds for each property."""
+        pass
+
+    def get_properties(self):
+        """
+        Get the physics properties.
+
+        Returns:
+            dict: current physic property values.
+        """
+        pass
+
+    def set_properties(self, properties):
+        """
+        Set the given physic property values using the simulator.
+
+        Args:
+            properties (dict): the physic property values to be set in the simulator.
+        """
+        pass

pyrobolearn/physics/world_physics_randomizer.py

@@ -0,0 +1,462 @@
+#!/usr/bin/env python
+"""Define the `WorldPhysicsRandomizer` class which randomizes the physical attributes / properties of the world.
+
+Dependencies:
+- `pyrobolearn.physics`
+- `pyrobolearn.world`
+"""
+
+import numpy as np
+
+from pyrobolearn.worlds import World
+from pyrobolearn.physics.physics_randomizer import PhysicsRandomizer
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class WorldPhysicsRandomizer(PhysicsRandomizer):
+    r"""World Physics Randomizer
+
+    The world physics randomizer can randomize the physical attributes of a world. It can notably change the gravity,
+    the light intensity, the floor friction coefficients (including lateral, spinning, and rolling frictions), etc.
+    """
+
+    def __init__(self, world, gravity=None, lateral_friction=None, rolling_friction=None, spinning_friction=None,
+                 restitution=None, contact_damping=None, contact_stiffness=None, **kwargs):
+        """
+        Initialize the world physics randomizer.
+
+        Args:
+            world (World): world instance.
+            gravity (None, np.float[3], tuple of np.float[3]): gravity bounds. If None, it will take
+                the default value returned by the world, and it will not sample from it. If it is a float[3], it will
+                set that value to the world and will always return this value when sampling from the physics randomizer.
+                If it is a tuple, it has to be of length 2 where the first item is the lower bound and the second item
+                is the upper bound.
+            lateral_friction (None, float, tuple of float): lateral friction coefficient bounds. If None, it will take
+                the default value returned by the world, and it will not sample from it. If it is a float, it will set
+                that value to the world and will always return this value when sampling from the physics randomizer.
+                If it is a tuple, it has to be of length 2 where the first item is the lower bound and the second item
+                is the upper bound.
+            rolling_friction (None, float, tuple of float): rolling friction coefficient bounds. (same types as
+                described in `lateral_friction`)
+            spinning_friction (None, float, tuple of float): spinning friction coefficient bounds. (same types as
+                described in `lateral_friction`)
+            restitution (None, float, tuple of float): restitution bounds. (same types as described in
+                `lateral_friction`)
+            contact_damping (None, float, tuple of float): contact damping bounds. (same types as described in
+                `lateral_friction`)
+            contact_stiffness (None, float, tuple of float): contact stiffness bounds. (same types as described in
+                `lateral_friction`)
+            **kwargs (dict): range of possible physical properties. If given one value, that property won't be
+                randomized. Each range is a tuple of two values `[lower_bound, upper_bound]`.
+        """
+        self.world = world
+        simulator = self.world.simulator
+        super(WorldPhysicsRandomizer, self).__init__(simulator)
+
+        # set the bounds
+        self.gravity_bounds = gravity
+        self.lateral_friction_bounds = lateral_friction
+        self.rolling_friction_bounds = rolling_friction
+        self.spinning_friction_bounds = spinning_friction
+        self.restitution_bounds = restitution
+        self.contact_damping_bounds = contact_damping
+        self.contact_stiffness_bounds = contact_stiffness
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def world(self):
+        """Return the world instance."""
+        return self._world
+
+    @world.setter
+    def world(self, world):
+        """Set the world instance."""
+        if not isinstance(world, World):
+            raise TypeError("Expecting the given world to be an instance of `World`, instead got: "
+                            "{}".format(type(world)))
+        self._world = world
+
+    @property
+    def gravity(self):
+        """Return the gravity vector."""
+        return self.world.gravity
+
+    @gravity.setter
+    def gravity(self, gravity):
+        """
+        Set the gravity vector.
+
+        Args:
+            (np.float[3]): gravity vector [x,y,z].
+        """
+        self.world.gravity = gravity
+
+    @property
+    def gravity_bounds(self):
+        """Return the upper and lower bound for the gravity vector."""
+        return self._gravity_bounds
+
+    @gravity_bounds.setter
+    def gravity_bounds(self, bounds):
+        """Set the upper and lower bounds for the gravity vector."""
+        if bounds is None:
+            bounds = (self.gravity, self.gravity)
+        elif isinstance(bounds, (list, tuple, np.ndarray)):
+            if len(bounds) == 2:
+                for bound in bounds:
+                    if not isinstance(bound, (list, tuple, np.ndarray)):
+                        raise TypeError("Expecting one of the bounds to be a 3D vector (list, tuple, or np.ndarray), "
+                                        "instead got {}".format(type(bound)))
+                    if len(bound) != 3:
+                        raise ValueError("Expecting the gravity to be a 3D vector, instead received a {}D "
+                                         "vector".format(len(bounds)))
+            elif len(bounds) == 3:
+                bounds = (bounds, bounds)
+            else:
+                raise ValueError("Expecting the gravity to be a 3D vector or a tuple of length 2 where the first item "
+                                 "is the lower bound and the second item is the upper bound of 3D vectors, instead "
+                                 "the given element has a length of {}".format(len(bounds)))
+        else:
+            raise TypeError("Expecting the gravity bounds to be a 3d vector, a tuple of 3d vectors, or None. Instead "
+                            "got {}".format(type(bounds)))
+        self._gravity_bounds = bounds
+
+    @property
+    def lateral_friction(self):
+        """Return the floor lateral friction coefficient."""
+        return self.world.lateral_friction
+
+    @lateral_friction.setter
+    def lateral_friction(self, coefficient):
+        """
+        Set the floor lateral friction coefficient.
+
+        Args:
+            coefficient (float): lateral friction coefficient.
+        """
+        self.world.lateral_friction = coefficient
+
+    @property
+    def lateral_friction_bounds(self):
+        """Return the upper and lower bound for the lateral friction coefficient."""
+        return self._lateral_friction_bounds
+
+    @lateral_friction_bounds.setter
+    def lateral_friction_bounds(self, bounds):
+        """Set the upper and lower bounds for the lateral friction coefficient."""
+        if bounds is None:
+            bounds = (self.lateral_friction, self.lateral_friction)
+        elif isinstance(bounds, float):
+            bounds = (bounds, bounds)
+        elif isinstance(bounds, (tuple, list)):
+            if len(bounds) == 2:
+                for bound in bounds:
+                    if not isinstance(bound, float):
+                        raise TypeError("Expecting one of the bounds to be a float instead got {}".format(type(bound)))
+            else:
+                raise ValueError("Expecting the bounds to be a tuple of length 2 where the first item is the lower "
+                                 "bound and the second item is the upper bound of the lateral friction, instead got "
+                                 "a length of {}".format(len(bounds)))
+        else:
+            raise TypeError("Expecting the lateral friction bounds to be a float, or a tuple of float, or None. "
+                            "Instead got {}".format(type(bounds)))
+        self._lateral_friction_bounds = bounds
+
+    @property
+    def rolling_friction(self):
+        """Return the floor rolling friction coefficient."""
+        return self.world.rolling_friction
+
+    @rolling_friction.setter
+    def rolling_friction(self, coefficient):
+        """
+        Set the floor rolling friction coefficient.
+
+        Args:
+            coefficient (float): rolling friction coefficient.
+        """
+        self.world.rolling_friction = coefficient
+
+    @property
+    def rolling_friction_bounds(self):
+        """Return the upper and lower bound for the rolling friction coefficient."""
+        return self._rolling_friction_bounds
+
+    @rolling_friction_bounds.setter
+    def rolling_friction_bounds(self, bounds):
+        """Set the upper and lower bounds for the rolling friction coefficient."""
+        if bounds is None:
+            bounds = (self.rolling_friction, self.rolling_friction)
+        elif isinstance(bounds, float):
+            bounds = (bounds, bounds)
+        elif isinstance(bounds, (tuple, list)):
+            if len(bounds) == 2:
+                for bound in bounds:
+                    if not isinstance(bound, float):
+                        raise TypeError("Expecting one of the bounds to be a float instead got {}".format(type(bound)))
+            else:
+                raise ValueError("Expecting the bounds to be a tuple of length 2 where the first item is the lower "
+                                 "bound and the second item is the upper bound of the rolling friction, instead got "
+                                 "a length of {}".format(len(bounds)))
+        else:
+            raise TypeError("Expecting the rolling friction bounds to be a float, or a tuple of float, or None. "
+                            "Instead got {}".format(type(bounds)))
+        self._rolling_friction_bounds = bounds
+
+    @property
+    def spinning_friction(self):
+        """Return the floor spinning friction coefficient."""
+        return self.world.spinning_friction
+
+    @spinning_friction.setter
+    def spinning_friction(self, coefficient):
+        """
+        Set the floor spinning friction coefficient.
+
+        Args:
+            coefficient (float): spinning friction coefficient.
+        """
+        self.world.spinning_friction = coefficient
+
+    @property
+    def spinning_friction_bounds(self):
+        """Return the upper and lower bound for the spinning friction coefficient."""
+        return self._spinning_friction_bounds
+
+    @spinning_friction_bounds.setter
+    def spinning_friction_bounds(self, bounds):
+        """Set the upper and lower bounds for the spinning friction coefficient."""
+        if bounds is None:
+            bounds = (self.spinning_friction, self.spinning_friction)
+        elif isinstance(bounds, float):
+            bounds = (bounds, bounds)
+        elif isinstance(bounds, (tuple, list)):
+            if len(bounds) == 2:
+                for bound in bounds:
+                    if not isinstance(bound, float):
+                        raise TypeError("Expecting one of the bounds to be a float instead got {}".format(type(bound)))
+            else:
+                raise ValueError("Expecting the bounds to be a tuple of length 2 where the first item is the lower "
+                                 "bound and the second item is the upper bound of the spinning friction, instead got "
+                                 "a length of {}".format(len(bounds)))
+        else:
+            raise TypeError("Expecting the spinning friction bounds to be a float, or a tuple of float, or None. "
+                            "Instead got {}".format(type(bounds)))
+        self._spinning_friction_bounds = bounds
+
+    @property
+    def restitution(self):
+        """Return the floor restitution (bounciness) coefficient."""
+        return self.world.restitution
+
+    @restitution.setter
+    def restitution(self, coefficient):
+        """
+        Set the floor restitution (bounciness) coefficient.
+
+        Args:
+            coefficient (float): restitution coefficient.
+        """
+        self.world.restitution = coefficient
+
+    @property
+    def restitution_bounds(self):
+        """Return the upper and lower bound for the restitution coefficient."""
+        return self._restitution_bounds
+
+    @restitution_bounds.setter
+    def restitution_bounds(self, bounds):
+        """Set the upper and lower bounds for the restitution coefficient."""
+        if bounds is None:
+            bounds = (self.restitution, self.restitution)
+        elif isinstance(bounds, float):
+            bounds = (bounds, bounds)
+        elif isinstance(bounds, (tuple, list)):
+            if len(bounds) == 2:
+                for bound in bounds:
+                    if not isinstance(bound, float):
+                        raise TypeError("Expecting one of the bounds to be a float instead got {}".format(type(bound)))
+            else:
+                raise ValueError("Expecting the bounds to be a tuple of length 2 where the first item is the lower "
+                                 "bound and the second item is the upper bound of the restitution, instead got "
+                                 "a length of {}".format(len(bounds)))
+        else:
+            raise TypeError("Expecting the restitution bounds to be a float, or a tuple of float, or None. "
+                            "Instead got {}".format(type(bounds)))
+        self._restitution_bounds = bounds
+
+    @property
+    def contact_damping(self):
+        """Return the floor contact damping."""
+        return self.world.contact_damping
+
+    @contact_damping.setter
+    def contact_damping(self, value):
+        """
+        Set the floor contact damping.
+
+        Args:
+            value (float): contact damping value.
+        """
+        self.world.contact_damping = value
+
+    @property
+    def contact_damping_bounds(self):
+        """Return the upper and lower bound for the contact damping value."""
+        return self._contact_damping_bounds
+
+    @contact_damping_bounds.setter
+    def contact_damping_bounds(self, bounds):
+        """Set the upper and lower bounds for the contact damping value."""
+        if bounds is None:
+            bounds = (self.contact_damping, self.contact_damping)
+        elif isinstance(bounds, float):
+            bounds = (bounds, bounds)
+        elif isinstance(bounds, (tuple, list)):
+            if len(bounds) == 2:
+                for bound in bounds:
+                    if not isinstance(bound, float):
+                        raise TypeError("Expecting one of the bounds to be a float instead got {}".format(type(bound)))
+            else:
+                raise ValueError("Expecting the bounds to be a tuple of length 2 where the first item is the lower "
+                                 "bound and the second item is the upper bound of the contact damping, instead got "
+                                 "a length of {}".format(len(bounds)))
+        else:
+            raise TypeError("Expecting the contact damping bounds to be a float, or a tuple of float, or None. "
+                            "Instead got {}".format(type(bounds)))
+        self._contact_damping_bounds = bounds
+
+    @property
+    def contact_stiffness(self):
+        """Return the floor contact stiffness."""
+        return self.world.contact_stiffness
+
+    @contact_stiffness.setter
+    def contact_stiffness(self, value):
+        """
+        Set the floor contact stiffness.
+
+        Args:
+            value (float): contact stiffness value.
+        """
+        self.world.contact_stiffness = value
+
+    @property
+    def contact_stiffness_bounds(self):
+        """Return the upper and lower bound for the contact stiffness value."""
+        return self._contact_stiffness_bounds
+
+    @contact_stiffness_bounds.setter
+    def contact_stiffness_bounds(self, bounds):
+        """Set the upper and lower bounds for the contact stiffness value."""
+        if bounds is None:
+            bounds = (self.contact_stiffness, self.contact_stiffness)
+        elif isinstance(bounds, float):
+            bounds = (bounds, bounds)
+        elif isinstance(bounds, (tuple, list)):
+            if len(bounds) == 2:
+                for bound in bounds:
+                    if not isinstance(bound, float):
+                        raise TypeError("Expecting one of the bounds to be a float instead got {}".format(type(bound)))
+            else:
+                raise ValueError("Expecting the bounds to be a tuple of length 2 where the first item is the lower "
+                                 "bound and the second item is the upper bound of the contact stiffness, instead got "
+                                 "a length of {}".format(len(bounds)))
+        else:
+            raise TypeError("Expecting the contact stiffness bounds to be a float, or a tuple of float, or None. "
+                            "Instead got {}".format(type(bounds)))
+        self._contact_stiffness_bounds = bounds
+
+    @property
+    def floor_dynamics(self):
+        """Return the floor dynamical parameters (friction, restitution, etc).
+
+        Returns:
+            float: lateral friction coefficient
+            float: rolling friction coefficient
+            float: spinning friction coefficient
+            float: restitution coefficient
+            float: contact damping value
+            float: contact stiffness value
+        """
+        return self.world.floor_dynamics
+
+    @floor_dynamics.setter
+    def floor_dynamics(self, dynamics):
+        """
+        Set the floor dynamics.
+
+        Args:
+            values (dict): floor dynamics.
+        """
+        self.world.floor_dynamics = dynamics
+
+    ###########
+    # Methods #
+    ###########
+
+    def names(self):
+        """Return an iterator over the property names."""
+        for name in ['gravity', 'lateral_friction', 'rolling_friction', 'spinning_friction', 'restitution',
+                     'contact_damping', 'contact_stiffness']:
+            yield name
+
+    def bounds(self):
+        """Return an iterator over the bounds"""
+        yield self.gravity_bounds
+        yield self.lateral_friction_bounds
+        yield self.rolling_friction_bounds
+        yield self.spinning_friction_bounds
+        yield self.restitution_bounds
+        yield self.contact_damping_bounds
+        yield self.contact_stiffness_bounds
+
+    def get_properties(self):
+        """
+        Get the physics properties.
+
+        Returns:
+            dict: current physic property values.
+        """
+        properties = dict()
+        properties['gravity'] = self.gravity
+        floor_dynamics = self.floor_dynamics
+        if floor_dynamics is not None:  # there is a floor
+            properties['lateral_friction'] = floor_dynamics[0]
+            properties['rolling_friction'] = floor_dynamics[1]
+            properties['spinning_friction'] = floor_dynamics[2]
+            properties['restitution'] = floor_dynamics[3]
+            properties['contact_damping'] = floor_dynamics[4]
+            properties['contact_stiffness'] = floor_dynamics[5]
+        return properties
+
+    def set_properties(self, properties):
+        """
+        Set the given physic property values using the simulator.
+
+        Args:
+            properties (dict): the physic property values to be set in the simulator.
+        """
+        if not isinstance(properties, dict):
+            raise TypeError("Expecting the given 'properties' to be a dictionary, instead got: "
+                            "{}".format(type(properties)))
+
+        # set gravity
+        if 'gravity' in properties:
+            self.gravity = properties['gravity']
+
+        # set floor dynamics
+        self.floor_dynamics = properties

pyrobolearn/robots/__init__.py

@@ -5,7 +5,7 @@ import importlib
 import inspect
 
 # General robot class
-from .base import Object, MovableObject, ControllableObject
+from .base import Body, MovableBody, ControllableBody
 from .actuators import *
 from .sensors import *
 from .robot import Robot

pyrobolearn/robots/base.py

@@ -1,9 +1,17 @@
 #!/usr/bin/env python
-"""Define the various basic objects that are present in the simulator/world.
+"""Define the various basic bodies / objects that are present in the simulator / world.
+
+Dependencies:
+- `pyrobolearn.simulators`
+- `pyrobolearn.utils`
 """
 
 import numpy as np
-import quaternion
+# import quaternion
+
+from pyrobolearn.simulators import Simulator
+from pyrobolearn.utils.orientation import get_rpy_from_quaternion, get_matrix_from_quaternion
+
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
@@ -15,99 +23,172 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class Object(object):
-    r"""Object
+class Body(object):
+    r"""Physical (Multi-)Body
 
-    Define an object in the simulator/world.
+    Define a physical body in the simulator/world.
     """
 
-    def __init__(self, simulator, object_id):
+    def __init__(self, simulator, body_id, name=None):
+        """
+        Initialize the Body.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            body_id (int): unique body id returned by the simulator.
+            name (str): name of the body.
+        """
+        self.simulator = simulator
+        self.id = body_id
+        self.name = name
+        self.joints = None
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def simulator(self):
+        """Return the simulator."""
+        return self.sim
+
+    @simulator.setter
+    def simulator(self, simulator):
+        """Set the simulator."""
+        if not isinstance(simulator, Simulator):
+            raise TypeError("Expecting the given simulator to be an instance of `Simulator`, instead got: "
+                            "{}".format(type(simulator)))
         self.sim = simulator
-        self.id = object_id
+
+    @property
+    def id(self):
+        """Return the id."""
+        return self._id
+
+    @id.setter
+    def id(self, id_):
+        """Set the unique body id."""
+        if not isinstance(id_, int):
+            raise TypeError("Expecting the given simulator to be an integer, instead got: {}".format(type(id_)))
+        self._id = id_
 
     @property
     def name(self):
-        return self.sim.getBodyInfo(self.id)
+        """Return the name of the body (or the base if not given)."""
+        if self._name is None:
+            return self.sim.get_body_info(self.id)
+        return self._name
+
+    @name.setter
+    def name(self, name):
+        """Set the name of the body."""
+        if not isinstance(name, str):
+            raise TypeError("Expecting the given name to be a string, instead got: {}".format(type(name)))
+        self._name = name
+
+    @property
+    def pose(self):
+        """Return the body pose."""
+        return self.sim.get_base_pose(self.id)
 
     @property
     def position(self):
-        return np.array(self.sim.getBasePositionAndOrientation(self.id)[0])
+        """Return the body position."""
+        return self.sim.get_base_position(self.id)
 
     @property
-    def quaternion(self):
-        quat = self.sim.getBasePositionAndOrientation(self.id)[1]
-        return quaternion.quaternion(quat[3], *quat[:3])
+    def orientation(self):
+        """Return the body orientation as a quaternion [x,y,z,w]."""
+        return self.sim.get_base_orientation(self.id)
 
     # alias
-    orientation = quaternion
+    quaternion = orientation
 
     @property
     def rpy(self):
-        quat = self.sim.getBasePositionAndOrientation(self.id)[1]
-        y, p, r = self.sim.getEulerFromQuaternion(quat)
-        return np.array([r, p, y])
+        """Return the orientation as the Roll-Pitch-Yaw angles."""
+        return get_rpy_from_quaternion(self.orientation)
 
     @property
-    def rotation(self):
-        quat = self.sim.getBasePositionAndOrientation(self.id)[1]
-        rot = self.sim.getMatrixFromQuaternion(quat)
-        return np.array(rot).reshape(3, 3)
-
-    @property
-    def state(self):
-        pos, quat = self.sim.getBasePositionAndOrientation(self.id)
-        rpy = self.sim.getEulerFromQuaternion(quat)[::-1]
-        # return np.array(pos), quaternion.quaternion(quat[3], *quat[:3])
-        return np.array(pos+rpy)
+    def rotation_matrix(self):
+        """Return the orientation as a rotation matrix."""
+        return get_matrix_from_quaternion(self.orientation)
 
     @property
     def linear_velocity(self):
-        return np.array(self.sim.getBaseVelocity(self.id)[0])
+        """Return the linear velocity of the body's base."""
+        return self.sim.get_base_linear_velocity(self.id)
 
     @property
     def angular_velocity(self):
-        return np.array(self.sim.getBaseVelocity(self.id)[1])
+        """Return the angular velocity of the body's base."""
+        return self.sim.get_base_angular_velocity(self.id)
 
     @property
     def velocity(self):
-        lin, ang = self.sim.getBaseVelocity(self.id)
-        return np.array(lin+ang)
+        """Return the linear and angular velocity of the body."""
+        return self.sim.get_base_velocity(self.id)
 
     @property
     def color(self):
-        return self.sim.getVisualShapeData(self.id)[0][-1]
-
-    # alias
-    rgba_color = color
+        return self.sim.get_visual_shape_data(self.id)[0][-1]
 
     @property
     def mass(self):
-        links = [-1] + list(range(self.sim.getNumJoints(self.id)))
-        return np.sum([self.sim.getDynamicsInfo(self.id, linkId)[0] for linkId in links])
+        """Return the total mass of the body."""
+        return self.sim.get_mass(self.id)
 
     @property
     def dimensions(self):
-        return np.array(self.sim.getVisualShapeData(self.id)[0][3])
+        """Return the dimensions of the body. Warnings: this should not be trusted too much..."""
+        return np.array(self.sim.get_visual_shape_data(self.id)[0][3])
+
+    @property
+    def num_joints(self):
+        """Return the total number of joints."""
+        return self.sim.num_joints(self.id)
+
+    @property
+    def num_links(self):
+        """Return the total number of links. This is the same as the number of joints."""
+        return self.sim.num_links(self.id)
+
+    @property
+    def num_actuated_joints(self):
+        """Return the total number of actuated joints. This property should be overwritten in the child class."""
+        return self.sim.num_actuated_joints(self.id)
+
+    @property
+    def actuated_joints(self):
+        """Return the total number of actuated joints."""
+        if self.joints is None:
+            self.joints = self.sim.get_actuated_joint_ids(self.id)
+        return self.joints
+
+    @property
+    def center_of_mass(self):
+        """Return the center of mass."""
+        return self.sim.get_center_of_mass(self.id)
 
 
-class MovableObject(Object):
-    r"""Movable Object
+class MovableBody(Body):
+    r"""Movable Body
 
     Define a movable object in the world.
     """
 
-    def __init__(self, simulator, object_id):
-        super(MovableObject, self).__init__(simulator, object_id)
+    def __init__(self, simulator, object_id, name=None):
+        super(MovableBody, self).__init__(simulator, object_id, name=name)
 
-    def move(self, new_position=None, new_orientation=None):
-        pass
+    # def move(self, position=None, orientation=None):
+    #     pass
 
 
-class ControllableObject(MovableObject):
-    r"""Controllable Object
+class ControllableBody(MovableBody):
+    r"""Controllable Body
 
     Define a controllable object in the world.
     """
 
-    def __init__(self, simulator, object_id):
-        super(ControllableObject, self).__init__(simulator, object_id)
+    def __init__(self, simulator, object_id, name=None):
+        super(ControllableBody, self).__init__(simulator, object_id, name=name)

pyrobolearn/robots/cartpole.py

@@ -9,7 +9,7 @@ import sympy
 import sympy.physics.mechanics as mechanics
 
 from pyrobolearn.robots.robot import Robot
-from pyrobolearn.utils.orientation import getSymbolicMatrixFromAxisAngle
+from pyrobolearn.utils.orientation import get_symbolic_matrix_from_axis_angle
 
 
 class CartPole(Robot):

pyrobolearn/simulators/__init__.py

@@ -4,6 +4,9 @@
 # basic simulator
 from .simulator import Simulator
 
+# bullet simulator
+from .bullet import Bullet
+
 # PyBullet simulator
 import pybullet
 import pybullet_data

pyrobolearn/simulators/bullet.py

@@ -21,12 +21,13 @@ References:
 import time
 import numpy as np
 import quaternion
-from pyrobolearn.utils.converter import NumpyListConverter, QuaternionListConverter
 
 import pybullet
+import pybullet_data
 from pybullet_envs.bullet.bullet_client import BulletClient
 
-from simulator import Simulator
+from pyrobolearn.utils.converter import NumpyListConverter, QuaternionListConverter
+from pyrobolearn.simulators.simulator import Simulator
 
 
 __author__ = "Brian Delhaisse"
@@ -74,9 +75,11 @@ class Bullet(Simulator):
             Erwin Coumans and Yunfei Bai, 2017/2018
     """
 
-    def __init__(self, render=True):  # , converter=None):
+    def __init__(self, render=True, **kwargs):  # , converter=None):
         super(Bullet, self).__init__()
 
+        # parse the kwargs
+
         # Connect to pybullet
         if render:
             self.sim = BulletClient(connection_mode=pybullet.GUI)
@@ -84,6 +87,9 @@ class Bullet(Simulator):
             self.sim = BulletClient(connection_mode=pybullet.DIRECT)
         self.id = self.sim._client
 
+        # add additional search path when loading URDFs, SDFs, MJCFs, etc.
+        self.sim.setAdditionalSearchPath(pybullet_data.getDataPath())
+
         # Converters
         # if converter is None:
         self.conv = NumpyListConverter()
@@ -133,12 +139,22 @@ class Bullet(Simulator):
         """
         self.sim.resetSimulation()
 
+    def close(self):
+        """Close the simulator."""
+        try:
+            self.sim.disconnect(physicsClientId=self.id)
+        except pybullet.error:
+            pass
+
     def step(self, sleep_time=0.):
         """Perform a step in the simulator.
 
         "stepSimulation will perform all the actions in a single forward dynamics simulation step such as collision
         detection, constraint solving and integration. The default timestep is 1/240 second, it can be changed using
         the setTimeStep or setPhysicsEngineParameter API." [1]
+
+        Args:
+            sleep_time (float): time to sleep after performing one step in the simulation.
         """
         self.sim.stepSimulation()
         time.sleep(sleep_time)
@@ -170,7 +186,7 @@ class Bullet(Simulator):
         """
         self.sim.setTimeStep(timeStep=time_step)
 
-    def set_real_time(self, flag=True):
+    def set_real_time(self, enable=True):
         """Enable/disable real time in the simulator.
 
         "By default, the physics server will not step the simulation, unless you explicitly send a 'stepSimulation'
@@ -185,9 +201,9 @@ class Bullet(Simulator):
         allows the physicsserver  thread to add additional calls to stepSimulation." [1]
 
         Args:
-            flag (bool): If True, it will enable the real-time simulation. If False, it will disable it.
+            enable (bool): If True, it will enable the real-time simulation. If False, it will disable it.
         """
-        self.sim.setRealTimeSimulation(enableRealTimeSimulation=int(flag))
+        self.sim.setRealTimeSimulation(enableRealTimeSimulation=int(enable))
 
     def pause(self):
         """Pause the simulator if in real-time."""
@@ -201,18 +217,25 @@ class Bullet(Simulator):
         """Get the physics engine parameters.
 
         Returns:
-            dict: dictionary containing the following tags with their corresponding values: ['gravityAccelerationX',
-                'useRealTimeSimulation', 'gravityAccelerationZ', 'numSolverIterations', 'gravityAccelerationY',
-                'numSubSteps', 'fixedTimeStep']
+            dict: dictionary containing the following tags with their corresponding values: ['gravity',
+                'num_solver_iterations', 'use_real_time_simulation', 'num_sub_steps', 'fixed_time_step']
         """
-        return self.sim.getPhysicsEngineParameters()
+        d = self.sim.getPhysicsEngineParameters()
+        properties = dict()
+        properties['gravity'] = np.array([d['gravityAccelerationX'], d['gravityAccelerationY'],
+                                          d['gravityAccelerationZ']])
+        properties['num_solver_iterations'] = d['numSolverIterations']
+        properties['use_real_time_simulation'] = d['useRealTimeSimulation']
+        properties['num_sub_steps'] = d['numSubSteps']
+        properties['fixed_time_step'] = d['fixedTimeStep']
+        return properties
 
     def set_physics_properties(self, time_step=None, num_solver_iterations=None, use_split_impulse=None,
                                split_impulse_penetration_threshold=None, num_sub_steps=None,
                                collision_filter_mode=None, contact_breaking_threshold=None, max_num_cmd_per_1ms=None,
                                enable_file_caching=None, restitution_velocity_threshold=None, erp=None,
                                contact_erp=None, friction_erp=None, enable_cone_friction=None,
-                               deterministic_overlapping_pairs=None, solver_residual_threshold=None):
+                               deterministic_overlapping_pairs=None, solver_residual_threshold=None, **kwargs):
         """Set the physics engine parameters.
 
         Args:
@@ -415,7 +438,7 @@ class Bullet(Simulator):
 
         Args:
             plugin_id (int): unique plugin id.
-            args (list): list of argument values to be interpreted by the plugin. One can be a string, while the
+            *args (list): list of argument values to be interpreted by the plugin. One can be a string, while the
                 others must be integers or float.
         """
         kwargs = {}
@@ -462,9 +485,9 @@ class Bullet(Simulator):
 
         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]
+            position (vec3): create the base of the object at the specified position in world space coordinates [x,y,z]
             orientation (quat): create the base of the object at the specified orientation as world space quaternion
-                [X,Y,Z,W]
+                [x,y,z,w]
             use_maximal_coordinates (int): Experimental. By default, the joints in the URDF file are created using the
                 reduced coordinate method: the joints are simulated using the Featherstone Articulated Body algorithm
                 (btMultiBody in Bullet 2.x). The useMaximalCoordinates option will create a 6 degree of freedom rigid
@@ -529,7 +552,7 @@ class Bullet(Simulator):
         return self.sim.loadMJCF(filename, globalScaling=scaling)
 
     def load_mesh(self, filename, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.),
-                  color=None, flags=None):
+                  color=None, flags=None, *args, **kwargs):
         """
         Load a mesh in the world (only available in the simulator).
 
@@ -541,7 +564,7 @@ class Bullet(Simulator):
                 If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w)
             mass (float): mass of the mesh (in kg). If mass = 0, it won't move even if there is a collision.
             scale (float[3]): scale the mesh in the (x,y,z) directions
-            color (int[4]): color of the mesh (by default: white and opaque)
+            color (int[4], None): color of the mesh (by default: white and opaque)
             flags (int, None): if flag = `sim.GEOM_FORCE_CONCAVE_TRIMESH` (=1), this will create a concave static
                 triangle mesh. This should not be used with dynamic/moving objects, only for static (mass=0) terrain.
 
@@ -648,9 +671,10 @@ class Bullet(Simulator):
 
     def create_constraint(self, parent_body_id, parent_link_id, child_body_id, child_link_id, joint_type,
                           joint_axis, parent_frame_position, child_frame_position,
-                          parent_frame_orientation=(0., 0., 0., 1.), child_frame_orientation=(0., 0., 0., 1.)):
+                          parent_frame_orientation=(0., 0., 0., 1.), child_frame_orientation=(0., 0., 0., 1.),
+                          *args, **kwargs):
         """
-        Create a constaint.
+        Create a constraint.
 
         "URDF, SDF and MJCF specify articulated bodies as a tree-structures without loops. The 'createConstraint'
         allows you to connect specific links of bodies to close those loops. In addition, you can create arbitrary
@@ -697,7 +721,8 @@ class Bullet(Simulator):
         self.sim.removeConstraint(constraint_id)
 
     def change_constraint(self, constraint_id, child_joint_pivot=None, child_frame_orientation=None, max_force=None,
-                          gear_ratio=None, gear_auxiliary_link=None, relative_position_target=None, erp=None):
+                          gear_ratio=None, gear_auxiliary_link=None, relative_position_target=None, erp=None, *args,
+                          **kwargs):
         """
         Change the parameters of an existing constraint.
 
@@ -804,7 +829,11 @@ class Bullet(Simulator):
         return np.sum(self.get_link_masses(body_id, [-1] + list(range(self.num_links(body_id)))))
 
     def get_base_mass(self, body_id):
-        """Return the base mass of the robot."""
+        """Return the base mass of the robot.
+
+        Args:
+            body_id (int): unique object id.
+        """
         return self.get_link_masses(body_id, -1)
 
     def get_base_name(self, body_id):
@@ -991,7 +1020,7 @@ class Bullet(Simulator):
         self.sim.resetBaseVelocity(body_id, angularVelocity=angular_velocity)
 
     def apply_external_force(self, body_id, link_id=-1, force=(0., 0., 0.), position=(0., 0., 0.),
-                             flags=pybullet.LINK_FRAME):
+                             frame=pybullet.LINK_FRAME):
         """
         Apply the specified external force on the specified position on the body / link.
 
@@ -1006,10 +1035,10 @@ class Bullet(Simulator):
             force (np.float[3]): external force to be applied.
             position (np.float[3]): position on the link where the force is applied. See `flags` for coordinate
                 systems.
-            flags (int): Specify the coordinate system of force/position: either `pybullet.WORLD_FRAME` (=2) for
+            frame (int): Specify the coordinate system of force/position: either `pybullet.WORLD_FRAME` (=2) for
                 Cartesian world coordinates or `pybullet.LINK_FRAME` (=1) for local link coordinates.
         """
-        self.sim.applyExternalForce(body_id, link_id, force, position, flags)
+        self.sim.applyExternalForce(body_id, link_id, force, position, frame)
 
     def apply_external_torque(self, body_id, link_id=-1, torque=(0., 0., 0.)):
         """
@@ -1091,7 +1120,11 @@ class Bullet(Simulator):
             [15] np.float[4]:  joint orientation in parent frame
             [16] int:       parent link index, -1 for base
         """
-        return self.sim.getJointInfo(body_id, joint_id)
+        info = self.sim.getJointInfo(body_id, joint_id)
+        info[13] = np.array(info[13])
+        info[14] = np.array(info[14])
+        info[15] = np.array(info[15])
+        return info
 
     def get_joint_state(self, body_id, joint_id):
         """
@@ -1227,7 +1260,8 @@ class Bullet(Simulator):
             joint_ids (list of int): list of joint id.
             control_mode (int): POSITION_CONTROL (=2) (which is in fact CONTROL_MODE_POSITION_VELOCITY_PD),
                 VELOCITY_CONTROL (=0), TORQUE_CONTROL (=1) and PD_CONTROL (=3).
-            positions (list of float): list of target joint positions (used in POSITION_CONTROL) the target value is target position of the joint.
+            positions (list of float): list of target joint positions (used in POSITION_CONTROL) the target value is
+                target position of the joint.
             velocities (list of float): list of target joint velocities (used in PD_CONTROL, VELOCITY_CONTROL and
                 POSITION_CONTROL).
             forces (list of float): list of forces. In POSITION_CONTROL and VELOCITY_CONTROL, these are the maximum
@@ -1663,7 +1697,7 @@ class Bullet(Simulator):
             velocities (None, float, np.float[N]): desired velocity, or list of desired velocities [rad/s]
             kps (None, float, np.float[N]): position gain(s)
             kds (None, float, np.float[N]): velocity gain(s)
-            forces (float): maximum motor force(s)/torque(s) used to reach the target values.
+            forces (None, float, np.float[N]): maximum motor force(s)/torque(s) used to reach the target values.
         """
         if isinstance(joint_ids, int):
             self.set_joint_motor_control(body_id, joint_ids, control_mode=pybullet.POSITION_CONTROL, position=positions,
@@ -1699,7 +1733,7 @@ class Bullet(Simulator):
             body_id (int): unique body id.
             joint_ids (int, list of int): joint id, or list of joint ids.
             velocities (float, np.float[N]): desired velocity, or list of desired velocities [rad/s]
-            max_force (bool, float, float[N]): maximum motor forces/torques
+            max_force (None, float, np.float[N]): maximum motor forces/torques
         """
         if isinstance(joint_ids, int):
             if max_force is None:
@@ -1820,7 +1854,7 @@ class Bullet(Simulator):
         Args:
             body_id (int): unique body id.
             joint_ids (int, list of int): joint id, or list of joint ids.
-            torque (float, list of float): desired torque(s) to apply to the joint(s) [N].
+            torques (float, list of float): desired torque(s) to apply to the joint(s) [N].
         """
         if isinstance(joint_ids, int):
             self.sim.setJointMotorControl2(body_id, joint_ids, self.sim.TORQUE_CONTROL, force=torques)
@@ -2077,7 +2111,7 @@ class Bullet(Simulator):
         * orthographic projection
         * perspective projection
 
-        For the perspective projection, see `computeProjectionMatrixFOV(self)
+        For the perspective projection, see `computeProjectionMatrixFOV(self)`.
 
         Args:
             left (float): left screen (canvas) coordinate
@@ -2989,50 +3023,50 @@ class Bullet(Simulator):
 
     def calculate_forward_dynamics(self, body_id, q, dq, torques):
         r"""
-       Given the specified joint positions :math:`q` and velocities :math:`\dot{q}`, and joint torques :math:`\tau`,
-       it computes the joint accelerations :math:`\ddot{q}`. That is, :math:`\ddot{q} = FD(model, q, \dot{q}, \tau)`.
+        Given the specified joint positions :math:`q` and velocities :math:`\dot{q}`, and joint torques :math:`\tau`,
+        it computes the joint accelerations :math:`\ddot{q}`. That is, :math:`\ddot{q} = FD(model, q, \dot{q}, \tau)`.
 
-       Specifically, it uses the rigid-body equation of motion in joint space given by (see [1]):
+        Specifically, it uses the rigid-body equation of motion in joint space given by (see [1]):
 
-       .. math:: \ddot{q} = H(q)^{-1} (\tau - C(q,\dot{q}))
+        .. math:: \ddot{q} = H(q)^{-1} (\tau - C(q,\dot{q}))
 
-       where :math:`\tau` is the vector of applied torques, :math:`H(q)` is the inertia matrix, and
-       :math:`C(q,\dot{q}) \dot{q}` is the vector accounting for Coriolis, centrifugal forces, gravity, and any
-       other forces acting on the system except the applied torques :math:`\tau`.
+        where :math:`\tau` is the vector of applied torques, :math:`H(q)` is the inertia matrix, and
+        :math:`C(q,\dot{q}) \dot{q}` is the vector accounting for Coriolis, centrifugal forces, gravity, and any
+        other forces acting on the system except the applied torques :math:`\tau`.
 
-       Normally, a more popular form of this equation of motion (in joint space) is given by:
+        Normally, a more popular form of this equation of motion (in joint space) is given by:
 
-       .. math:: H(q) \ddot{q} + S(q,\dot{q}) \dot{q} + g(q) = \tau + J^T(q) F
+        .. math:: H(q) \ddot{q} + S(q,\dot{q}) \dot{q} + g(q) = \tau + J^T(q) F
 
-       which is the same as the first one with :math:`C = S\dot{q} + g(q) - J^T(q) F`. However, this last formulation
-       is useful to understand what happens when we set some variables to 0.
-       Assuming that there are no forces acting on the system, and giving desired joint torques of 0, this
-       method will return :math:`\ddot{q} = - H(q)^{-1} (S(q,\dot{q}) \dot{q} + g(q))`. If in addition
-       the joint velocities are also 0, it will return :math:`\ddot{q} = - H(q)^{-1} g(q)` which are
-       the accelerations due to gravity.
+        which is the same as the first one with :math:`C = S\dot{q} + g(q) - J^T(q) F`. However, this last formulation
+        is useful to understand what happens when we set some variables to 0.
+        Assuming that there are no forces acting on the system, and giving desired joint torques of 0, this
+        method will return :math:`\ddot{q} = - H(q)^{-1} (S(q,\dot{q}) \dot{q} + g(q))`. If in addition
+        the joint velocities are also 0, it will return :math:`\ddot{q} = - H(q)^{-1} g(q)` which are
+        the accelerations due to gravity.
 
-       For inverse dynamics, which computes the joint torques given the joint positions, velocities, and
-       accelerations (that is, :math:`\tau = ID(model, q, \dot{q}, \ddot{q})`, this can be computed using
-       :math:`\tau = H(q)\ddot{q} + C(q,\dot{q})`. For more information about different
-       control schemes (position, force, impedance control and others), or about the formulation of the equation
-       of motion in task/operational space (instead of joint space), check the references [1-4].
+        For inverse dynamics, which computes the joint torques given the joint positions, velocities, and
+        accelerations (that is, :math:`\tau = ID(model, q, \dot{q}, \ddot{q})`, this can be computed using
+        :math:`\tau = H(q)\ddot{q} + C(q,\dot{q})`. For more information about different
+        control schemes (position, force, impedance control and others), or about the formulation of the equation
+        of motion in task/operational space (instead of joint space), check the references [1-4].
 
-       Args:
-           body_id (int): unique body id.
-           q (np.float[N]): joint positions
-           dq (np.float[N]): joint velocities
-           torques (np.float[N]): desired joint torques
+        Args:
+            body_id (int): unique body id.
+            q (np.float[N]): joint positions
+            dq (np.float[N]): joint velocities
+            torques (np.float[N]): desired joint torques
 
-       Returns:
-           float[N]: joint accelerations computed using the rigid-body equation of motion
+        Returns:
+            float[N]: joint accelerations computed using the rigid-body equation of motion
 
-       References:
-           [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1
-           [2] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010
-           [3] "Springer Handbook of Robotics", Siciliano et al., 2008
-           [4] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma,
-               http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf
-       """
+        References:
+            [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1
+            [2] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010
+            [3] "Springer Handbook of Robotics", Siciliano et al., 2008
+            [4] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma,
+                http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf
+        """
         # convert numpy arrays to lists
         if isinstance(q, np.ndarray):
             q = q.ravel().tolist()

pyrobolearn/states/__init__.py

@@ -6,7 +6,7 @@ from .state import State
 from .basic_states import *
 
 # import object states
-from .object_states import *
+from .body_states import *
 
 # import time/count states
 from .time_states import *

pyrobolearn/states/body_states.py

@@ -7,8 +7,8 @@ This includes notably the joint positions, velocities, and force/torque states.
 from abc import ABCMeta, abstractmethod
 
 from pyrobolearn.states.state import State
-from pyrobolearn.worlds.world import World
-from pyrobolearn.robots import Object
+from pyrobolearn.worlds import World
+from pyrobolearn.robots import Body
 
 
 __author__ = "Brian Delhaisse"
@@ -21,15 +21,15 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class ObjectState(State):
-    """Object state (abstract)
+class BodyState(State):
+    """Body state (abstract)
     """
     __metaclass__ = ABCMeta
 
     def __init__(self, obj, world=None):
-        super(ObjectState, self).__init__()
-        if not isinstance(obj, (Object, int)):
-            raise TypeError("Expecting an instance of Object, or an identifier from the simulator/world.")
+        super(BodyState, self).__init__()
+        if not isinstance(obj, (Body, int)):
+            raise TypeError("Expecting an instance of Body, or an identifier from the simulator/world.")
         if isinstance(obj, int):
             if not isinstance(world, World):
                 # try to look for the world in global variables
@@ -37,7 +37,7 @@ class ObjectState(State):
                     world = globals()['world']
                 else:
                     raise ValueError("When giving the object identifier, the world need to be given as well.")
-            obj = Object(world.getSimulator(), obj)
+            obj = Body(world.simulator, obj)
         self.obj = obj
 
     @abstractmethod
@@ -45,7 +45,7 @@ class ObjectState(State):
         pass
 
 
-class PositionState(ObjectState):
+class PositionState(BodyState):
     """Position of an object.
     """
     def __init__(self, obj, world=None):
@@ -56,7 +56,7 @@ class PositionState(ObjectState):
         self.data = self.obj.position
 
 
-class OrientationState(ObjectState):
+class OrientationState(BodyState):
     """Orientation of an object.
     """
     def __init__(self, obj, world=None):
@@ -67,7 +67,7 @@ class OrientationState(ObjectState):
         self.data = self.obj.orientation
 
 
-class VelocityState(ObjectState):
+class VelocityState(BodyState):
     """Velocity of an object.
     """
     def __init__(self, obj, world=None):

pyrobolearn/utils/orientation.py

@@ -1,5 +1,11 @@
-# utils code to transform orientation expressed in different forms
-# This includes rotation matrices, euler angles (RPY), axis-angle, and quaternions
+#!/usr/bin/env python
+"""Provide utils code to transform orientation expressed in different forms
+
+This includes rotation matrices, euler angles (RPY), axis-angle, and quaternions.
+
+References:
+    [1] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010, chapter 2
+"""
 
 import numpy as np
 import quaternion
@@ -8,8 +14,26 @@ from collections import Iterable
 
 from pyrobolearn.utils.converter import QuaternionNumpyConverter
 
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
 
-def getMatrixFromAxisAngle(axis, angle):
+
+def get_matrix_from_axis_angle(axis, angle):
+    """Return the rotation matrix from the specified axis and angle.
+
+    Args:
+        axis (np.float[3], list of 3 float): 3d axis vector.
+        angle (float): angle.
+
+    Returns:
+        np.float[3,3]: rotation matrix.
+    """
     x, y, z = axis
     a = angle
     c, s = np.cos(a), np.sin(a)
@@ -20,7 +44,16 @@ def getMatrixFromAxisAngle(axis, angle):
     return R
 
 
-def getSymbolicMatrixFromAxisAngle(axis, angle):
+def get_symbolic_matrix_from_axis_angle(axis, angle):
+    """Return the symbolic rotation matrix from the specified axis and angle.
+
+    Args:
+        axis (np.float[3], list of 3 float, list of 3 sympy.Symbol): 3d axis vector.
+        angle (float, sympy.Symbol): angle.
+
+    Returns:
+        np.float[3,3]: rotation matrix.
+    """
     x, y, z = axis
     a = angle
     c, s = sympy.cos(a), sympy.sin(a)
@@ -31,19 +64,49 @@ def getSymbolicMatrixFromAxisAngle(axis, angle):
     return R
 
 
-def getAxisAngleFromMatrix(R):
+def get_axis_angle_from_matrix(R):
+    """Return the associated axis and angle from the specified rotation matrix.
+
+    Args:
+        R (np.float[3,3]): 3-by-3 rotation matrix.
+
+    Returns:
+        float: angle.
+        np.float[3]: 3d axis vector.
+    """
     angle = np.arccos((R[0, 0] + R[1, 1] + R[2, 2] - 1) / 2.)
     axis = 1. / (2. * np.sin(angle)) * np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])
     return angle, axis
 
 
-def getSymbolicAxisAngleFromMatrix(R):
+def get_symbolic_axis_angle_from_matrix(R):
+    """Return the symbolic axis and angle from the specified rotation matrix.
+
+    Args:
+        R (np.array of sympy.Symbol): 3-by-3 rotation matrix.
+
+    Returns:
+        sympy.Symbol: angle.
+        np.array of 3 sympy.Symbol: 3d axis vector.
+    """
     angle = sympy.acos((R[0, 0] + R[1, 1] + R[2, 2] - 1) / 2.)
     axis = 1. / (2. * sympy.sin(angle)) * np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])
     return angle, axis
 
 
-def getQuaternionFromAxisAngle(axis, angle, convert_to_quat=False, convention='xyzw'):
+def get_quaternion_from_axis_angle(axis, angle, convert_to_quat=False, convention='xyzw'):
+    """Get the quaternion associated from the axis/angle representation.
+
+    Args:
+        axis (np.float[3]): 3d axis vector.
+        angle (float): angle.
+        convert_to_quat (bool): If True, it will return an instance of `quaternion.quaternion`.
+        convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
+            'wxyz'.
+
+    Returns:
+        np.float[4], quaternion.quaternion: quaternion.
+    """
     w = np.cos(angle / 2.)
     x, y, z = np.sin(angle / 2.) * axis
     if convert_to_quat:
@@ -57,7 +120,18 @@ def getQuaternionFromAxisAngle(axis, angle, convert_to_quat=False, convention='x
             raise NotImplementedError("Asking for a convention that has not been implemented")
 
 
-def getSymbolicQuaternionFromAxisAngle(axis, angle, convention='xyzw'):
+def get_symbolic_quaternion_from_axis_angle(axis, angle, convention='xyzw'):
+    """Get the symbolic quaternion associated from the axis/angle representation.
+
+    Args:
+        axis (np.float[3], np.array of 3 sympy.Symbol): 3d axis vector.
+        angle (float, sympy.Symbol): angle.
+        convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
+            'wxyz'.
+
+    Returns:
+        np.float[4]: symbolic quaternion.
+    """
     w = sympy.cos(angle / 2.)
     x, y, z = sympy.sin(angle / 2.) * axis
     if convention == 'xyzw':
@@ -68,21 +142,55 @@ def getSymbolicQuaternionFromAxisAngle(axis, angle, convention='xyzw'):
         raise NotImplementedError("Asking for a convention that has not been implemented")
 
 
-def getRPYFromMatrix(R):
-    r = np.arctan2(R[1, 0], R[0, 0])
-    p = np.arctan2(-R[2, 0], np.sqrt(R[2, 1]**2 + R[2, 2]**2))
-    y = np.arctan2(R[2, 1], R[2, 2])
+def get_rpy_from_matrix(R):
+    """Get the Roll-Pitch-Yaw angle values from the given rotation matrix.
+
+    Args:
+        R (np.float[3,3]): 3-by-3 rotation matrix.
+
+    Returns:
+        np.float[3]: roll-pitch-yaw angle values.
+    """
+    # r = np.arctan2(R[1, 0], R[0, 0])
+    # p = np.arctan2(-R[2, 0], np.sqrt(R[2, 1]**2 + R[2, 2]**2))
+    # y = np.arctan2(R[2, 1], R[2, 2])
+
+    r = np.arctan2(R[2, 1], R[2, 2])
+    p = np.arctan2(-R[2, 0], np.sqrt(R[2, 1] ** 2 + R[2, 2] ** 2))
+    y = np.arctan2(R[1, 0], R[0, 0])
+
     return np.array([r, p, y])
 
 
-def getSymbolicRPYFromMatrix(R):
-    r = sympy.atan2(R[1, 0], R[0, 0])
+def get_symbolic_rpy_from_matrix(R):
+    """Get the symbolic Roll-Pitch-Yaw angles from the given rotation matrix.
+
+    Args:
+        R (np.float[3,3], np.array of sympy.Symbol): symbolic 3-by-3 rotation matrix.
+
+    Returns:
+        np.array of 3 sympy.Symbol: symbolic roll-pitch-yaw angles.
+    """
+    # r = sympy.atan2(R[1, 0], R[0, 0])
+    # p = sympy.atan2(-R[2, 0], sympy.sqrt(R[2, 1] ** 2 + R[2, 2] ** 2))
+    # y = sympy.atan2(R[2, 1], R[2, 2])
+
+    r = sympy.atan2(R[2, 1], R[2, 2])
     p = sympy.atan2(-R[2, 0], sympy.sqrt(R[2, 1] ** 2 + R[2, 2] ** 2))
-    y = sympy.atan2(R[2, 1], R[2, 2])
+    y = sympy.atan2(R[1, 0], R[0, 0])
+
     return np.array([r, p, y])
 
 
-def getMatrixFromRPY(rpy):
+def get_matrix_from_rpy(rpy):
+    """Get rotation matrix from the given Roll-Pitch-Yaw angles.
+
+    Args:
+        rpy (np.float[3]): roll-pitch-yaw angles
+
+    Returns:
+        np.float[3,3]: rotation matrix.
+    """
     cr, cp, cy = [np.cos(i) for i in rpy]
     sr, sp, sy = [np.sin(i) for i in rpy]
     R = np.array([[cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
@@ -91,7 +199,16 @@ def getMatrixFromRPY(rpy):
     return R
 
 
-def getSymbolicMatrixFromRPY(rpy):
+def get_symbolic_matrix_from_rpy(rpy):
+    """
+    Get the symbolic rotation matrix from the given Roll-Pitch-Yaw angles.
+
+    Args:
+        rpy (np.float[3], np.array of 3 sympy.Symbol): roll-pitch-yaw angles.
+
+    Returns:
+        3-by-3 np.array of sympy.Symbol: symbolic rotation matrix
+    """
     cr, cp, cy = [sympy.cos(i) for i in rpy]
     sr, sp, sy = [sympy.sin(i) for i in rpy]
     R = np.array([[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
@@ -100,7 +217,19 @@ def getSymbolicMatrixFromRPY(rpy):
     return R
 
 
-def getQuaternionFromMatrix(R, convert_to_quat=False, convention='xyzw'):
+def get_quaternion_from_matrix(R, convert_to_quat=False, convention='xyzw'):
+    """
+    Get the quaternion from the given rotation matrix.
+
+    Args:
+        R (np.float[3,3]): rotation matrix.
+        convert_to_quat (bool): If True, it will return an instance of `quaternion.quaternion`.
+        convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
+            'wxyz'.
+
+    Returns:
+        np.float[4], quaternion.quaternion: quaternion
+    """
     w = 1./2 * np.sqrt(R[0, 0] + R[1, 1] + R[2, 2] + 1)
     x, y, z = 1./2 * np.array([np.sign(R[2, 1] - R[1, 2]) * np.sqrt(R[0, 0] - R[1, 1] - R[2, 2] + 1),
                                np.sign(R[0, 2] - R[2, 0]) * np.sqrt(R[1, 1] - R[2, 2] - R[0, 0] + 1),
@@ -116,7 +245,18 @@ def getQuaternionFromMatrix(R, convert_to_quat=False, convention='xyzw'):
             raise NotImplementedError("Asking for a convention that has not been implemented")
 
 
-def getSymbolicQuaternionFromMatrix(R, convention='xyzw'):
+def get_symbolic_quaternion_from_matrix(R, convention='xyzw'):
+    """
+    Get the symbolic quaternion from the given rotation matrix.
+
+    Args:
+        R (3-by-3 np.array of sympy.Symbol, np.float[3,3]): (symbolic) rotation matrix
+        convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
+            'wxyz'.
+
+    Returns:
+        np.array of 4 sympy.Symbol: symbolic quaternion.
+    """
     w = 1. / 2 * sympy.sqrt(R[0, 0] + R[1, 1] + R[2, 2] + 1)
     x, y, z = 1. / 2 * np.array([sympy.sign(R[2, 1] - R[1, 2]) * sympy.sqrt(R[0, 0] - R[1, 1] - R[2, 2] + 1),
                                  sympy.sign(R[0, 2] - R[2, 0]) * sympy.sqrt(R[1, 1] - R[2, 2] - R[0, 0] + 1),
@@ -129,7 +269,18 @@ def getSymbolicQuaternionFromMatrix(R, convention='xyzw'):
         raise NotImplementedError("Asking for a convention that has not been implemented")
 
 
-def getMatrixFromQuaternion(q, convention='xyzw'):
+def get_matrix_from_quaternion(q, convention='xyzw'):
+    """
+    Get rotation matrix from the given quaternion.
+
+    Args:
+        q (np.float[4], quaternion.quaternion): quaternion
+        convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
+            'wxyz'.
+
+    Returns:
+        np.float[3,3]: rotation matrix.
+    """
     if isinstance(q, quaternion.quaternion):
         x, y, z, w = q.x, q.y, q.z, q.w
     elif isinstance(q, Iterable):
@@ -147,11 +298,146 @@ def getMatrixFromQuaternion(q, convention='xyzw'):
     return R
 
 
-def getSymbolicMatrixFromQuaternion(q, convention='xyzw'):
-    return getMatrixFromQuaternion(q, convention=convention)
+def get_symbolic_matrix_from_quaternion(q, convention='xyzw'):
+    """
+    Get symbolic rotation matrix from the given quaternion.
+
+    Args:
+        q (np.array of 4 sympy.Symbol, np.float[4]): (symbolic) quaternion.
+        convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
+            'wxyz'.
+
+    Returns:
+        3-by-3 np.array of sympy.Symbol: symbolic rotation matrix.
+    """
+    return get_matrix_from_quaternion(q, convention=convention)
 
 
-def skew(vector):
+def get_rpy_from_quaternion(q, convention='xyzw'):
+    """
+    Get the Roll-Pitch-Yaw angle from the given quaternion.
+
+    Args:
+        q (np.float[4], quaternion.quaternion): quaternion
+        convention: convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
+            'wxyz'.
+
+    Returns:
+        np.float[3]: roll-pitch-yaw angles.
+    """
+    if isinstance(q, quaternion.quaternion):
+        x, y, z, w = q.x, q.y, q.z, q.w
+    elif isinstance(q, Iterable):
+        if convention == 'xyzw':
+            x, y, z, w = q
+        elif convention == 'wxyz':
+            w, x, y, z = q
+        else:
+            raise NotImplementedError("Asking for a convention that has not been implemented")
+    else:
+        raise TypeError
+    roll = np.arctan2(2*(w*x + y*z), 1 - 2 * (x**2 + y**2))
+    pitch = np.arcsin(2 * (w*y - z*x))
+    yaw = np.arctan2(2 * (w*z + x*y), 1 - 2 * (y**2 + z**2))
+    return np.array([roll, pitch, yaw])
+
+
+def get_symbolic_rpy_from_quaternion(q, convention='xyzw'):
+    """
+    Get the symbolic Roll-Pitch-Yaw angle from the given quaternion.
+
+    Args:
+        q (np.float[4], np.array of 4 sympy.Symbol): quaternion
+        convention: convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
+            'wxyz'.
+
+    Returns:
+        np.array of 3 sympy.Symbol: symbolic roll-pitch-yaw angles.
+    """
+    if isinstance(q, quaternion.quaternion):
+        x, y, z, w = q.x, q.y, q.z, q.w
+    elif isinstance(q, Iterable):
+        if convention == 'xyzw':
+            x, y, z, w = q
+        elif convention == 'wxyz':
+            w, x, y, z = q
+        else:
+            raise NotImplementedError("Asking for a convention that has not been implemented")
+    else:
+        raise TypeError
+
+    roll = sympy.atan2(2*(w*x + y*z), 1 - 2 * (x**2 + y**2))
+    pitch = sympy.asin(2 * (w*y - z*x))
+    yaw = sympy.atan2(2 * (w*z + x*y), 1 - 2 * (y**2 + z**2))
+
+    return np.array([roll, pitch, yaw])
+
+
+def get_quaternion_from_rpy(rpy, convert_to_quat=False, convention='xyzw'):
+    """
+    Get quaternion from Roll-Pitch-Yaw angle.
+
+    Args:
+        rpy (np.float[3]): roll-pitch-yaw angles
+        convert_to_quat (bool): If True, it will return an instance of `quaternion.quaternion`.
+        convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
+            'wxyz'.
+
+    Returns:
+        np.float[4], quaternion.quaternion: quaternion
+    """
+    r, p, y = rpy
+    cr, sr = np.cos(r/2.), np.sin(r/2.)
+    cp, sp = np.cos(p/2.), np.sin(p/2.)
+    cy, sy = np.cos(y/2.), np.sin(y/2.)
+
+    w = cr * cp * cy + sr * sp * sy
+    x = sr * cp * cy - cr * sp * sy
+    y = cr * sp * cy + sr * cp * sy
+    z = cr * cp * sy - sr * sp * cy
+
+    if convert_to_quat:
+        return quaternion.quaternion(w, x, y, z)
+    else:
+        if convention == 'xyzw':
+            return np.array([x, y, z, w])
+        elif convention == 'wxyz':
+            return np.array([w, x, y, z])
+        else:
+            raise NotImplementedError("Asking for a convention that has not been implemented")
+
+
+def get_symbolic_quaternion_from_rpy(rpy, convention='xyzw'):
+    """
+    Get symbolic quaternion from Roll-Pitch-Yaw angle.
+
+    Args:
+        rpy (np.float[3], np.array of 3 sympy.Symbol): (symbolic) roll-pitch-yaw angles
+        convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
+            'wxyz'.
+
+    Returns:
+        np.array of 4 sympy.Symbol: symbolic quaternion
+    """
+    r, p, y = rpy
+    cr, sr = sympy.cos(r/2.), sympy.sin(r/2.)
+    cp, sp = sympy.cos(p/2.), sympy.sin(p/2.)
+    cy, sy = sympy.cos(y/2.), sympy.sin(y/2.)
+
+    w = cr * cp * cy + sr * sp * sy
+    x = sr * cp * cy - cr * sp * sy
+    y = cr * sp * cy + sr * cp * sy
+    z = cr * cp * sy - sr * sp * cy
+
+    if convention == 'xyzw':
+        return np.array([x, y, z, w])
+    elif convention == 'wxyz':
+        return np.array([w, x, y, z])
+    else:
+        raise NotImplementedError("Asking for a convention that has not been implemented")
+
+
+def skew_matrix(vector):
     r"""
     Return the skew-symmetric matrix of the given vector, which allows to represents the cross product between the
     given vector and another vector, as the multiplication of the returned skew-symmetric matrix with the other
@@ -184,7 +470,7 @@ def skew(vector):
                      [-y, x, 0.]])
 
 
-def RotX(angle):
+def rotation_matrix_x(angle):
     """
     Return the rotation matrix around the x-axis by the given angle.
 
@@ -200,7 +486,7 @@ def RotX(angle):
                      [0., s, c]])
 
 
-def RotY(angle):
+def rotation_matrix_y(angle):
     """
     Return the rotation matrix around the y-axis by the given angle.
 
@@ -216,7 +502,7 @@ def RotY(angle):
                      [-s, 0, c]])
 
 
-def RotZ(angle):
+def rotation_matrix_z(angle):
     """
     Return the rotation matrix around the z-axis by the given angle.
 
@@ -239,7 +525,17 @@ def RotZ(angle):
 quat_converter = QuaternionNumpyConverter(convention=1)
 
 
-def getQuaternionInverse(q, convention='xyzw'):
+def get_quaternion_inverse(q, convention='xyzw'):
+    """Return the inverse of the given quaternion.
+
+    Args:
+        q (np.float[4], quaternion.quaternion): quaternion.
+        convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
+            'wxyz'.
+
+    Returns:
+        np.float[4], quaternion.quaternion: quaternion inverse.
+    """
     if isinstance(q, quaternion.quaternion):
         return q.inverse()
     elif isinstance(q, Iterable):
@@ -255,7 +551,18 @@ def getQuaternionInverse(q, convention='xyzw'):
         raise TypeError
 
 
-def getQuaternionProduct(q1, q2, convention='xyzw'):
+def get_quaternion_product(q1, q2, convention='xyzw'):
+    """Return the quaternion product between two quaternions.
+
+    Args:
+        q1 (np.float[4], quaternion.quaternion): first quaternion
+        q2 (np.float[4], quaternion.quaternion): second quaternion
+        convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
+            'wxyz'.
+
+    Returns:
+        np.float[4], quaternion.quaternion: resulting quaternion.
+    """
     if type(q1) != type(q2):
         raise TypeError("Expecting q1 and q2 to be of the same type")
     if isinstance(q1, quaternion.quaternion):
@@ -318,7 +625,7 @@ def exponential_map(r):
 
 
 def angular_velocity_from_quaternion(q1, q2):
-    """
+    r"""
     Convert the difference between 2 quaternions using the logarithm map.
 
     Args:
@@ -331,3 +638,22 @@ def angular_velocity_from_quaternion(q1, q2):
     q1 = quat_converter.convertTo(q1)
     q2 = quat_converter.convertTo(q2)
     return 2 * logarithm_map(q1 * q2)
+
+
+# Tests
+if __name__ == "__main__":
+    import pybullet
+    import tf.transformations as tft
+
+    q = np.array([-0.043, 0.567, 0.368, 0.736])
+    rpy = get_rpy_from_quaternion(q)
+
+    print('\nRPY from quaternion: {}'.format(get_rpy_from_quaternion(q)))
+    print('RPY <- matrix <- quaternion: {}'.format(get_rpy_from_matrix(get_matrix_from_quaternion(q))))
+    print('Using pybullet: {}'.format(pybullet.getEulerFromQuaternion(q)))
+    print('Using tf.transformations: {}'.format(tft.euler_from_quaternion(q)))
+
+    print('\nQuaternion from RPY: {}'.format(get_quaternion_from_rpy(rpy)))
+    print('Quaternion <- matrix <- RPY: {}'.format(get_quaternion_from_matrix(get_matrix_from_rpy(rpy))))
+    print('Using pybullet: {}'.format(pybullet.getQuaternionFromEuler(rpy)))
+    print('Using tf.transformations: {}'.format(tft.quaternion_from_euler(*rpy)))