update actions & plotting + add plotting examples

examples/README.rst

@@ -11,6 +11,7 @@ You can check the following folders on:
 - ``simulators``: how to use a particular simulator. Currently, the Bullet simulator is the one fully operational.
 - ``worlds``: how to create a world in the simulator, load various objects inside and interact with them, use the camera, and load or generate terrains.
 - ``robots``: how to load a specific robot (biped, quadruped, wheeled, etc) into the world.
+- ``plotting``: how to use real-time plotting tools (to plot the joint values or the link frames).
 - ``interfaces``: the various interfaces (game controllers, webcam, etc) and bridges that you can use.
 - ``kinematics``: how to use forward and inverse kinematics as well as position and velocity control.
 - ``dynamics``: how to use forward and inverse dynamics as well as force control.

examples/plotting/README.rst

@@ -0,0 +1,14 @@
+Plotting examples
+=================
+
+In this folder, you will find examples where we use the plotting tools provided in PRL.
+
+Warnings: Currently, you have to close the figure before closing the simulator. If you close the simulator first,
+you might still have the process responsible to draw the figure running.
+
+- ``joints.py``: plot in real-time the joint positions (in blue), velocities (in green), accelerations (in red),
+  and/or torques (in purple).
+- ``link_frames.py``: plot in real-time the frames of the specified links.
+
+To test these classes, you can run the corresponding python file, and move the manipulator with the mouse and check the
+real-time plots.

examples/plotting/joints.py

@@ -0,0 +1,34 @@
+#!/usr/bin/env python
+"""Provide the joint real-time plotting example.
+
+Try to move the Kuka manipulator with your mouse, and check the joint values. Note that it can take few seconds to
+load the plot.
+
+Warnings: don't forget to close FIRST the figure, THEN the simulator otherwise you will have the plotting process still
+running.
+"""
+
+from itertools import count
+import pyrobolearn as prl
+
+
+# create the simulator
+sim = prl.simulators.Bullet()
+
+# create the world
+world = prl.worlds.BasicWorld(sim)
+
+# load the robot
+robot = world.load_robot('kuka_iiwa')
+
+# create the joint real-time plotting tool
+plot = prl.utils.plotting.JointRealTimePlot(robot, joint_ids=None, position=True, velocity=False,
+                                            acceleration=False, torque=False, ticks=24)
+
+# run the simulation
+for t in count():
+    # update the plot
+    plot.update()
+
+    # perform a step in the world
+    world.step(sim.dt)

examples/plotting/link_frames.py

@@ -0,0 +1,32 @@
+#!/usr/bin/env python
+"""Provide the link frame real-time plotting example.
+
+Try to move the Kuka manipulator and / or the box with your mouse. Note that it can take few seconds to load the plot.
+
+Warnings: don't forget to close FIRST the figure, THEN the simulator otherwise you will have the plotting process still
+running.
+"""
+
+import pyrobolearn as prl
+
+
+# create the simulator
+sim = prl.simulators.Bullet()
+
+# create the world
+world = prl.worlds.BasicWorld(sim)
+
+# load the robot and a box
+robot = world.load_robot('kuka_iiwa')
+box = world.load_box([0.7, 0., 0.2], dimensions=(0.2, 0.2, 0.2), color=(0.2, 0.2, 0.8, 1.), return_body=True)
+
+# create the link frame real-time plotting tool
+plot = prl.utils.plotting.LinkFrameRealTimePlot(bodies=[robot, box], link_ids=None, ticks=24)
+
+# run the simulation
+for t in prl.count():
+    # update the plot
+    plot.update()
+
+    # perform a step in the world
+    world.step(sim.dt)

pyrobolearn/__init__.py

@@ -28,6 +28,9 @@ from . import robots
 # import worlds
 from . import worlds
 
+# import utils
+from . import utils
+
 # import physics randomizer
 from . import physics
 
@@ -110,7 +113,6 @@ def signal_handler(sig, frame):
 
 signal.signal(signal.SIGINT, signal_handler)
 
-
 # https://stackoverflow.com/questions/30483246/how-to-check-if-a-python-module-has-been-imported
 # https://stackoverflow.com/questions/14050281/how-to-check-if-a-python-module-exists-without-importing-it/25045228
 def module_imported(module_name):  # TODO: improve this method

pyrobolearn/actions/__init__.py

@@ -10,3 +10,6 @@ from .robot_actions import *
 
 # import gym actions
 from .gym_actions import GymAction
+
+# impot world actions
+from .world_actions import AttachAction

pyrobolearn/actions/robot_actions/__init__.py

@@ -12,3 +12,9 @@ from .joint_actions import JointAction, JointPositionAction, JointPositionChange
 from .link_actions import LinkAction, LinkPositionAction, LinkPositionChangeAction, LinkOrientationAction, \
     LinkOrientationChangeAction, LinkPoseAction, LinkPoseChangeAction, LinkVelocityAction, LinkVelocityChangeAction, \
     LinkForceAction, LinkTorqueAction, LinkWrenchAction, ApplyForceAction, ApplyTorqueAction  # , ApplyWrenchAction
+
+# import the actuator actions
+from .actuator_actions import ActuatorAction
+
+# import grasping action
+from .grasp_actions import GraspAction

pyrobolearn/actions/robot_actions/actuator_actions.py

@@ -5,6 +5,7 @@
 from abc import ABCMeta
 import collections
 import numpy as np
+import copy
 
 from pyrobolearn.actions.action import Action
 from pyrobolearn.robots.actuators.actuator import Actuator
@@ -25,26 +26,55 @@ class ActuatorAction(Action):
     """
     __metaclass__ = ABCMeta
 
-    def __init__(self, actuators, ticks=1):
+    def __init__(self, actuator, ticks=1):
         """
         Initialize the sensor state.
 
         Args:
-            actuators (A, list of Actuator): actuator(s).
+            actuator (Actuator): actuator instance.
             ticks (int): number of ticks to sleep before setting the next action data.
         """
-        if not isinstance(actuators, collections.Iterable):
-            actuators = [actuators]
-        for actuator in actuators:
-            if not isinstance(actuator, Actuator):
-                raise TypeError("Expecting the given 'actuator' to be an instance of `Actuator`, instead got: "
-                                "{}".format(type(actuator)))
-        self.actuators = actuators
         super(ActuatorAction, self).__init__(ticks=ticks)
 
+        # set the actuator instance
+        self.actuator = actuator
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def actuator(self):
+        """Return the actuator instance."""
+        return self._actuator
+
+    @actuator.setter
+    def actuator(self, actuator):
+        """Set the actuator instance."""
+        if not isinstance(actuator, Actuator):
+            raise TypeError("Expecting the given 'actuator' to be an instance of `Actuator`, instead got: "
+                            "{}".format(type(actuator)))
+        self._actuator = actuator
+
+    ###########
+    # Methods #
+    ###########
+
+    def _write(self, data):
+        """Write the data in the actuator and execute the actuator."""
+        # set the data in the actuator
+        self.actuator.data = data
+
+        # activate the actuator
+        self.actuator.act()
+
+    #############
+    # Operators #
+    #############
+
     def __copy__(self):
         """Return a shallow copy of the action. This can be overridden in the child class."""
-        return self.__class__(actuators=self.actuators, ticks=self.ticks)
+        return self.__class__(actuator=self.actuator, ticks=self.ticks)
 
     def __deepcopy__(self, memo={}):
         """Return a deep copy of the action. This can be overridden in the child class.
@@ -55,7 +85,7 @@ class ActuatorAction(Action):
         if self in memo:
             return memo[self]
 
-        actuators = copy.deepcopy(self.actuators, memo)
+        actuators = copy.deepcopy(self.actuator, memo)
         action = self.__class__(actuators=actuators, ticks=self.ticks)
 
         memo[self] = action

pyrobolearn/actions/robot_actions/grasp_actions.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python
+"""Define grasping actions
+"""
+
+import copy
+import numpy as np
+
+from pyrobolearn.actions.robot_actions.robot_actions import RobotAction
+from pyrobolearn.robots.gripper import Gripper
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class GraspAction(RobotAction):
+    r"""Attach Action.
+
+    This is allows to
+    """
+
+    def __init__(self, gripper):
+        """
+        Initialize the grasping action.
+
+        Args:
+            gripper (Gripper): a gripper instance.
+        """
+        super(GraspAction, self).__init__(robot=gripper)
+        self.gripper = gripper
+
+    @property
+    def gripper(self):
+        """Return the gripper instance."""
+        return self._gripper
+
+    @gripper.setter
+    def gripper(self, gripper):
+        if not isinstance(gripper, Gripper):
+            raise TypeError("Expecting the given 'gripper' to be an instance of `Gripper`, but got instead: "
+                            "{}".format(type(gripper)))
+        self._gripper = gripper
+
+    def _write(self, data):
+        """
+        Write the data.
+
+        Args:
+            data (int, np.ndarray): the continuous data representing the grasping strength.
+        """
+        if isinstance(data, np.ndarray):
+            data = data[0]
+        self.gripper.grasp(strength=data)
+
+    def __copy__(self):
+        """Return a shallow copy of the action. This can be overridden in the child class."""
+        return self.__class__(gripper=self.gripper)
+
+    def __deepcopy__(self, memo={}):
+        """Return a deep copy of the action. This can be overridden in the child class.
+
+        Args:
+            memo (dict): memo dictionary of objects already copied during the current copying pass
+        """
+        if self in memo:
+            return memo[self]
+        gripper = copy.deepcopy(self.gripper)
+        action = self.__class__(gripper=gripper)
+        memo[self] = action
+        return action

pyrobolearn/actions/robot_actions/link_actions.py

@@ -41,10 +41,10 @@ class LinkAction(RobotAction):  # TODO: multiple links
         """
         super(LinkAction, self).__init__(robot)
 
-        # get the joints of the robot
+        # get the link of the robot
         if link_id is None:
             link_id = -1
-        self.link = link_id
+        self.link = int(link_id)
 
         # if discrete values, check the type and create the space
         if discrete_values is not None:

pyrobolearn/actions/robot_actions/robot_actions.py

@@ -40,6 +40,8 @@ class RobotAction(Action):
             robot (Robot): a robot instance.
         """
         super(RobotAction, self).__init__()
+
+        # check robot instance
         if not isinstance(robot, Robot):
             raise TypeError("The 'robot' parameter has to be an instance of Robot, but instead got: "
                             "{}".format(type(robot)))
@@ -47,6 +49,7 @@ class RobotAction(Action):
 
     @property
     def robot(self):
+        """Return the robot instance."""
         return self._robot
 
     # def is_discrete(self):

pyrobolearn/actions/world_actions.py

@@ -0,0 +1,209 @@
+#!/usr/bin/env python
+"""Define world actions
+
+This includes:
+
+- AttachAction: this allows you to attach / detach a link with another link.
+"""
+
+from abc import ABCMeta
+import copy
+import numpy as np
+
+from pyrobolearn.actions.action import Action
+from pyrobolearn.robots.base import Body
+from pyrobolearn.worlds.world import World
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class WorldAction(Action):
+    r"""World action (abstract)
+
+    This provides the abstract class that allows to perform an action in the world. This includes to attach or detach
+    two bodies, and others.
+    """
+    __metaclass__ = ABCMeta
+
+    def __init__(self, world):
+        """
+        Initialize the world action.
+
+        Args:
+            world (World): world instance.
+        """
+        super(WorldAction, self).__init__()
+        self.world = world
+
+    @property
+    def world(self):
+        """Return the world instance."""
+        return self._world
+
+    @world.setter
+    def world(self, world):
+        """Set the world instance."""
+        self._world = world
+
+    def __copy__(self):
+        """Return a shallow copy of the action. This can be overridden in the child class."""
+        return self.__class__(world=self.world)
+
+    def __deepcopy__(self, memo={}):
+        """Return a deep copy of the action. This can be overridden in the child class.
+
+        Args:
+            memo (dict): memo dictionary of objects already copied during the current copying pass
+        """
+        if self in memo:
+            return memo[self]
+        world = copy.deepcopy(self.world)
+        action = self.__class__(world=world)
+        memo[self] = action
+        return action
+
+
+class AttachAction(WorldAction):
+    r"""Attach Action.
+
+    The attach action is a discrete action which can take two values: 0 (=detach) or 1 (=attach). This allows to
+    attach a robot's link with another body's link in the world. Note that is only valid in the simulator. In order to
+    attach the robot's link with the other link, they both have to be close to each other.
+
+    Warnings:
+        - This is only valid in the simulator.
+        - Currently, the other link id to which we would like to attach has to be provided.
+    """
+
+    def __init__(self, world, body1, body2, link_id1=-1, link_id2=-1, distance_threshold=0.1,
+                 body1_frame_position=(0., 0., 0.), body2_frame_position=(0., 0., 0.),
+                 body1_frame_orientation=None, body2_frame_orientation=None):
+        """
+        Initialize the attach action.
+
+        Args:
+            world (World): world instance.
+            body1 (Body): first body instance.
+            body2 (Body): second body instance.
+            link_id1 (int): unique link id of the first body instance.
+            link_id2 (int): unique link id of the second body instance.
+            distance_threshold (float): distance threshold between the two links such that they can be attached.
+            body1_frame_position (np.array[3]): position of the joint frame relative to parent CoM frame.
+            body2_frame_position (np.array[3]): position of the joint frame relative to a given child CoM frame (or
+                world origin if no child specified)
+            body1_frame_orientation (np.array[4]): the orientation of the joint frame relative to parent CoM
+                coordinate frame (expressed as a quaternion [x,y,z,w])
+            body2_frame_orientation (np.array[4]): the orientation of the joint frame relative to the child CoM
+                coordinate frame, or world origin frame if no child specified (expressed as a quaternion [x,y,z,w])
+        """
+        super(AttachAction, self).__init__(world=world)
+
+        # check body instances
+        def check_body(body, name):
+            if not isinstance(body, Body):
+                raise TypeError("Expecting the given '" + name + "' to be an instance of `Body`, but got instead: "
+                                "{}".format(type(body)))
+            return body
+
+        self._body1 = check_body(body1, 'body1')
+        self._body2 = check_body(body2, 'body2')
+
+        # check links
+        def check_link(link, name):
+            if link is None:
+                link = -1
+            if not isinstance(link, int):
+                raise TypeError("Expecting the given '" + name + "' to be an int, but got instead: "
+                                                                 "{}".format(type(link)))
+            return link
+
+        self._link1 = check_link(link_id1, 'link_id1')
+        self._link2 = check_link(link_id2, 'link_id2')
+
+        # check distance threshold
+        if not isinstance(distance_threshold, (float, int)):
+            raise TypeError("Expecting the given 'distance_threshold' to be a float or int, but got instead: "
+                            "{}".format(type(distance_threshold)))
+        if distance_threshold < 0:
+            raise ValueError("The given 'distance_threshold' should be a positive number.")
+        self._distance_threshold = distance_threshold
+
+        # set other variables
+        self._body1_frame_position = body1_frame_position
+        self._body2_frame_position = body2_frame_position
+        self._body1_frame_orientation = body1_frame_orientation
+        self._body2_frame_orientation = body2_frame_orientation
+
+        # variable to remember if they are already attached or not
+        self._attached = False
+
+    def _write(self, data):
+        """
+        Write the data.
+
+        Args:
+            data (int, np.ndarray): the binary data; 0 = detach and 1 = attach.
+        """
+        # get data
+        if isinstance(data, np.ndarray):
+            data = data[0]
+
+        if data == 1:  # attach
+            if not self._attached:  # if not already attached
+
+                # check distance
+                results = self.world.get_closest_bodies(body=self._body1, radius=self._distance_threshold,
+                                                        link_id=self._link1, body2=self._body2, link2_id=self._link2)
+
+                # if found body2 in close vicinity of body1
+                if len(results) > 0:
+                    self.world.attach(body1=self._body1, body2=self._body2, link1=self._link1, link2=self._link2,
+                                      parent_frame_position=self._body1_frame_position,
+                                      child_frame_position=self._body2_frame_position,
+                                      parent_frame_orientation=self._body1_frame_orientation,
+                                      child_frame_orientation=self._body2_frame_orientation)
+                    self._attached = not self._attached
+
+        else:  # detach
+            if self._attached:  # if attached
+                self.world.detach(body1=self._body1, body2=self._body2, link1=self._link1, link2=self._link2)
+                self._attached = not self._attached
+
+    def __copy__(self):
+        """Return a shallow copy of the action. This can be overridden in the child class."""
+        return self.__class__(world=self.world, body1=self._body1, body2=self._body2, link_id1=self._link1,
+                              link_id2=self._link2, distance_threshold=self._distance_threshold,
+                              body1_frame_position=self._body1_frame_position,
+                              body2_frame_position=self._body2_frame_position,
+                              body1_frame_orientation=self._body1_frame_orientation,
+                              body2_frame_orientation=self._body2_frame_orientation)
+
+    def __deepcopy__(self, memo={}):
+        """Return a deep copy of the action. This can be overridden in the child class.
+
+        Args:
+            memo (dict): memo dictionary of objects already copied during the current copying pass
+        """
+        if self in memo:
+            return memo[self]
+        world = copy.deepcopy(self.world)
+        body1 = copy.deepcopy(self._body1)
+        body2 = copy.deepcopy(self._body2)
+        body1_frame_position = copy.deepcopy(self._body1_frame_position)
+        body2_frame_position = copy.deepcopy(self._body2_frame_position)
+        body1_frame_orientation = copy.deepcopy(self._body1_frame_orientation)
+        body2_frame_orientation = copy.deepcopy(self._body2_frame_orientation)
+        action = self.__class__(world=world, body1=body1, body2=body2, link_id1=self._link1, link_id2=self._link2,
+                                body1_frame_position=body1_frame_position, body2_frame_position=body2_frame_position,
+                                body1_frame_orientation=body1_frame_orientation,
+                                body2_frame_orientation=body2_frame_orientation)
+        memo[self] = action
+        return action

pyrobolearn/plotting/plot.py

@@ -1,239 +0,0 @@
-#!/usr/bin/env python
-"""Define the Plot class.
-
-Warnings: THIS IS EXPERIMENTAL.
-
-Dependencies:
-- `matplotlib`
-"""
-
-# TODO
-
-import numpy as np
-from matplotlib import pyplot as plt
-from matplotlib import animation
-import time
-import multiprocessing
-
-
-__author__ = "Brian Delhaisse"
-__copyright__ = "Copyright 2019, PyRoboLearn"
-__credits__ = ["Brian Delhaisse"]
-__license__ = "GNU GPLv3"
-__version__ = "1.0.0"
-__maintainer__ = "Brian Delhaisse"
-__email__ = "briandelhaisse@gmail.com"
-__status__ = "Development"
-
-
-class Plot(object):
-    """Plot (abstract) class.
-
-    The plotter allows to plot different things. Notably, it can plot in real-time the joint values, the orientation
-    frame of each body.
-    """
-    pass
-
-
-class RealTimePlot(Plot):
-    """Real-time plotter
-
-    This plotter spawns a new process that is responsible to update a plot in real-time. To achieve that goal, the
-    master process sent the data (through the `RealTimePlot.update` method) through a pipe to the new process which
-    updates the plot.
-    """
-    pass
-    # def __init__(self, ticks=1, blit=True):
-    #     # create pipe, queue, and process
-    #     self.pipe, pipe = multiprocessing.Pipe()
-    #     self.queue = multiprocessing.Queue()
-    #     self.process = multiprocessing.Process(target=self._plot, args=(pipe, self.queue))
-    #
-    #     # start process
-    #     self.process.start()
-    #
-    # def _plot(self, pipe, queue):
-    #     """To be implemented in the child class."""
-    #     pass
-    #
-    # def update(self):
-    #     """To be implemented in the child class."""
-    #     pass
-    #
-    # def close(self):
-    #     """close the plotter."""
-    #     # notify the plot child process
-    #     self.pipe.send('END')
-    #
-    #     # wait for the child process to close
-    #     self.process.join()
-    #
-    #     # close queue and pipe
-    #     self.queue.close()
-    #     self.pipe.close()
-    #
-    # def __del__(self):
-    #     """Closing the plotter."""
-    #     self.close()
-
-
-class BodyPlot(RealTimePlot):
-    r"""Body plotter.
-
-    The Body plotter draws the joint positions with their corresponding frame in a 3D plot.
-    """
-    pass
-
-
-class JointPlot(RealTimePlot):
-    r"""Joint plotter
-
-    The Joint plotter plots the joint position, velocity, acceleration and torque values.
-    """
-
-    def __init__(self, robot, joint_ids=None, position=False, velocity=False, acceleration=False, torque=False,
-                 ticks=1, blit=True):
-        """
-        Initialize the joint plotter.
-
-        Args:
-            robot (Robot): robot instance.
-            joint_ids (list of int, int, None): joint id(s) to plot.
-            position (bool): if True, it will plot the joint positions.
-            velocity (bool): if True, it will plot the joint velocities.
-            acceleration (bool): if True, it will plot the joint accelerations.
-            torque (bool): if True, it will plot the joint torques.
-            ticks (int): number of ticks to sleep before sending the new data.
-            blit (bool): if we should use blit, that is, if we should re-draw only the parts that have changed.
-                If blit = True, it plots faster but can only update what is inside the plot (so not the xticks,
-                yticks, xlabel, etc).
-        """
-        # set variable
-        self.robot = robot
-        self.joint_ids = joint_ids
-        self.plot_position = position
-        self.plot_velocity = velocity
-        self.plot_acceleration = acceleration
-        self.plot_torque = torque
-        self.ticks = ticks
-        self.cnt = 0
-        self.blit = blit
-
-        self.plot_exist = True
-
-        # create pipe, queue, and process
-        self.pipe, pipe = multiprocessing.Pipe()
-        self.queue = multiprocessing.Queue()
-        self.plot_process = multiprocessing.Process(target=self._plot, args=(pipe, self.queue))
-        self.plot_process.start()
-
-    def _plot(self, pipe, queue):
-        """Plot the streamed data."""
-
-        # set pipe and queue
-        self.pipe = pipe
-        self.queue = queue
-
-        # create figure, subplots, axes, titles,...
-        fig = plt.figure()
-        ax = plt.axes(xlim=(0, 2), ylim=(-2, 2))
-
-        # create line
-        line, = ax.plot([], [], lw=2)
-
-        self.x = []
-        self.y = []
-
-        # initialization function: plot the background of each frame
-        def init():
-            line.set_data([], [])
-            return line,
-
-        # def gen():
-        #     states = self.pipe.recv()
-        #     if not (isinstance(states, bool) and states):
-        #         yield states
-        #     else:
-        #         print("Over")
-
-        # animation function.  This is called sequentially
-        def animate(i):
-            states = self.pipe.recv()
-            # if isinstance(states, bool) and states:
-            #     self.anim.event_source.stop()
-
-            # print("Received states: {}".format(states))
-
-            self.y.append(states['q'][0])
-            self.y = self.y[-10:]
-
-            # print(self.y[:3])
-
-            line.set_data(range(len(self.y)), self.y)
-            # ax.set_xlim(0 + 0.01 * i, 2 + 0.01 * i)
-            # ax.set_xticklabels(np.linspace(0.01 * i, 2 + 0.01 * i, 5))
-            return line,
-
-        # create funcanimation
-        anim = animation.FuncAnimation(fig, animate, init_func=init,
-                                       frames=None, interval=0.0001, blit=self.blit)
-        plt.show()
-
-        # if we get out of the animation, notify the master process
-        self.queue.put(True)
-        self.pipe.close()
-        self.queue.close()
-
-    def update(self):
-        """Update the plot by getting the """
-        if self.cnt % self.ticks == 0 and self.plot_exist:
-
-            # get useful information
-            states = {}
-            if self.plot_position:
-                states['q'] = self.robot.get_joint_positions(joint_ids=self.joint_ids)
-            if self.plot_velocity:
-                states['dq'] = self.robot.get_joint_velocities(joint_ids=self.joint_ids)
-            if self.plot_acceleration:
-                states['ddq'] = self.robot.get_joint_accelerations(joint_ids=self.joint_ids)
-
-            # send the data to the process
-            self.pipe.send(states)
-
-        self.cnt += 1
-
-        if not self.queue.empty():
-            result = self.queue.get()
-            if result:
-                print("The animation has finished. Closing process...")
-                self.plot_process.join()
-                print("Process has been closed.")
-            else:
-                print("Got result: {}".format(result))
-
-
-class LinkPlot(RealTimePlot):
-    r"""Link plotter
-
-    The Link plotter plots a link position, velocity, acceleration, force along the 3 axis (x,y,z).
-    """
-    pass
-
-
-# Tests
-if __name__ == '__main__':
-
-    # Try to move the robot in the simulator
-
-    from itertools import count
-    import pyrobolearn as prl
-
-    sim = prl.simulators.Bullet()
-    world = prl.worlds.BasicWorld(sim)
-    robot = world.load_robot('kuka_iiwa')
-
-    plot = JointPlot(robot, joint_ids=[3], position=True, ticks=24)
-
-    for t in count():
-        plot.update()
-        world.step(sim.dt)

pyrobolearn/robots/actuators/__init__.py

@@ -1,4 +1,9 @@
 
+import os
+import importlib
+import inspect
+import re
+
 # import basic actuator
 from .actuator import Actuator
 
@@ -8,3 +13,43 @@ from .joints import JointActuator, JointPositionActuator, JointVelocityActuator,
 
 # import speaker
 from .speaker import Speaker
+
+
+# get a list of implemented actuators
+path = os.path.dirname(__file__)
+implemented_actuators = set([f[:-3] for f in os.listdir(path) if os.path.isfile(os.path.join(path, f))
+                          and f.endswith('.py')])
+# remove few items from the set
+for s in ['__init__', 'misc', 'light']:
+    if s in implemented_actuators:
+        implemented_actuators.remove(s)
+
+implemented_actuators = list(implemented_actuators)
+
+# create dictionary that maps actuator names to actuator classes
+actuator_names_to_classes = {}
+for actuator_name in implemented_actuators:
+    module = importlib.import_module('pyrobolearn.robots.actuators.' + actuator_name)
+
+    for name, cls in inspect.getmembers(module):
+        # check if it is a class, and the names match
+        if inspect.isclass(cls) and issubclass(cls, Actuator):
+            if name.lower() == ''.join(actuator_name.split('_')):
+                if actuator_name.endswith('_actuator'):
+                    actuator_name = actuator_name[:-9]
+                elif actuator_name.endswith('actuator') and len(actuator_name) > 8:
+                    actuator_name = actuator_name[:-8]
+                actuator_names_to_classes[actuator_name] = cls
+                name = actuator_name
+            else:
+                name_list = re.findall('[0-9]*[A-Z]+[0-9]*[a-z]*', name)
+                name = '_'.join([n.lower() for n in name_list])
+                if name.endswith('_actuator'):
+                    name = name[:-9]
+                elif name.endswith('actuator') and len(name) > 8:
+                    name = name[:-8]
+                actuator_names_to_classes[name] = cls
+
+implemented_actuators = set(list(actuator_names_to_classes.keys()))
+# print(implemented_actuators)
+# print(actuator_names_to_classes)

pyrobolearn/robots/robot.py

@@ -22,8 +22,8 @@ from pyrobolearn.utils.transformation import *
 from pyrobolearn.utils.manifold_utils import tensor_matrix_product, symmetric_matrix_to_vector, logarithm_map, \
     distance_spd
 from pyrobolearn.robots.base import ControllableBody
-from pyrobolearn.robots.sensors.sensor import Sensor
-from pyrobolearn.robots.actuators.actuator import Actuator
+from pyrobolearn.robots.sensors import Sensor, sensor_names_to_classes
+from pyrobolearn.robots.actuators import Actuator, actuator_names_to_classes
 
 
 __author__ = "Brian Delhaisse"
@@ -1629,8 +1629,10 @@ class Robot(ControllableBody):
                 np.array[N*4], np.array[N,4]: orientation of each link frame [x,y,z,w]
 
         """
-        return self.get_link_world_frame_positions(link_ids, flatten), self.get_link_world_frame_orientations(link_ids,
-                                                                                                              flatten)
+        positions, orientations = self.sim.get_link_frames(body_id=self.id, link_ids=link_ids)
+        if flatten:
+            return positions.reshape(-1), orientations.reshape(-1)
+        return positions, orientations
 
     def get_link_world_frame_positions(self, link_ids=None, flatten=False):
         r"""
@@ -1647,14 +1649,7 @@ class Robot(ControllableBody):
             if multiple links:
                 np.array[N*3], np.array[N,3]: link frame position of each link in world space
         """
-        if isinstance(link_ids, int):
-            return np.asarray(self.sim.get_link_state(self.id, link_ids)[4])
-        if link_ids is None:
-            link_ids = self.joints
-        pos = np.asarray([self.sim.get_link_state(self.id, link)[4] for link in link_ids])
-        if flatten:
-            return pos.reshape(-1)  # 1D array
-        return pos  # 2D array
+        return self.get_link_frames(link_ids=link_ids, flatten=flatten)[0]
 
     def get_link_world_frame_orientations(self, link_ids=None, flatten=False):
         r"""
@@ -1671,14 +1666,7 @@ class Robot(ControllableBody):
             if multiple links:
                 np.array[N*4], np.array[N,4]: orientation of each link frame [x,y,z,w]
         """
-        if isinstance(link_ids, int):
-            return self.sim.get_link_state(self.id, link_ids)[5]
-        if link_ids is None:
-            link_ids = self.joints
-        orientation = np.asarray([self.sim.get_link_state(self.id, link)[5] for link in link_ids])
-        if flatten:
-            return orientation.reshape(-1)  # 1D array
-        return orientation  # 2D array
+        return self.get_link_frames(link_ids=link_ids, flatten=flatten)[1]
 
     def get_link_world_positions(self, link_ids=None, flatten=True):
         r"""
@@ -4167,19 +4155,27 @@ class Robot(ControllableBody):
             joint_ids = self.joints
         self.sim.enable_joint_force_torque_sensor(self.id, joint_ids, enable=False)
 
-    def get_sensors(self, idx=None):
+    def get_sensors(self, name=None):
         """
         Return the specified sensor.
 
         Args:
-            idx (int): index of the sensor
+            name (str, class, None): name or class type of the sensor. If None, it will return all the sensors.
 
         Returns:
-            Sensor, Sensor[M]: return the specified sensor, or all the sensors
+            if name is None:
+                dict: all the sensors {SensorClass: [sensorInstance]}
+            else:
+                list of Sensor: return the specified sensors
         """
-        if idx is None:
+        if name is None:
             return self.sensors
-        return self.sensors[idx]
+        if isinstance(name, type):
+            return self.sensors[name]
+        elif isinstance(name, str):
+            if name in sensor_names_to_classes:
+                name = sensor_names_to_classes[name]
+                return self.sensors[name]
 
     def add_sensor(self, sensor):
         """
@@ -4228,19 +4224,27 @@ class Robot(ControllableBody):
         """
         return len(self.actuators)
 
-    def get_actuators(self, idx=None):
+    def get_actuators(self, name=None):
         """
         Return the specified actuator.
 
         Args:
-            idx (int): index of the actuator.
+            name (str, class, None): name or class type of the actuator. If None, it will return all the actuators.
 
         Returns:
-            Actuator, Actuator[M]: return the specified actuator, or all the actuators
+            if name is None:
+                dict: all the actuators {ActuatorClass: [actuatorInstance]}
+            else:
+                list of Actuator: return the specified actuators.
         """
-        if idx is None:
+        if name is None:
             return self.actuators
-        return self.actuators[idx]
+        if isinstance(name, type):
+            return self.actuators[name]
+        elif isinstance(name, str):
+            if name in actuator_names_to_classes:
+                name = actuator_names_to_classes[name]
+                return self.actuators[name]
 
     def add_actuator(self, actuator):
         """

pyrobolearn/robots/sensors/__init__.py

@@ -1,4 +1,9 @@
 
+import os
+import importlib
+import inspect
+import re
+
 # import basic sensor
 from .sensor import Sensor
 
@@ -28,3 +33,46 @@ from .ray import RaySensor, RayBatchSensor, HeightmapSensor
 
 # import miscellaneous sensors
 # from .misc import *
+
+
+# get a list of implemented sensors
+path = os.path.dirname(__file__)
+implemented_sensors = set([f[:-3] for f in os.listdir(path) if os.path.isfile(os.path.join(path, f))
+                          and f.endswith('.py')])
+# remove few items from the set
+for s in ['__init__', 'misc', 'light']:
+    if s in implemented_sensors:
+        implemented_sensors.remove(s)
+
+implemented_sensors = list(implemented_sensors)
+
+# create dictionary that maps sensor names to sensor classes
+sensor_names_to_classes = {}
+for sensor_name in implemented_sensors:
+    module = importlib.import_module('pyrobolearn.robots.sensors.' + sensor_name)
+
+    for name, cls in inspect.getmembers(module):
+        # check if it is a class, and the names match
+        if inspect.isclass(cls) and issubclass(cls, Sensor):
+            if name.lower() == ''.join(sensor_name.split('_')):
+                if sensor_name.endswith('_sensor'):
+                    sensor_name = sensor_name[:-7]
+                elif sensor_name.endswith('sensor') and len(sensor_name) > 6:
+                    sensor_name = sensor_name[:-6]
+                sensor_names_to_classes[sensor_name] = cls
+                name = sensor_name
+            else:
+                name_list = re.findall('[0-9]*[A-Z]+[0-9]*[a-z]*', name)
+                name = '_'.join([n.lower() for n in name_list])
+                if name.endswith('_sensor'):
+                    name = name[:-7]
+                elif name.endswith('sensor') and len(name) > 6:
+                    name = name[:-6]
+                # TODO: improve regex
+                if name == 'rgbcamera':
+                    name = 'rgb_camera'
+                sensor_names_to_classes[name] = cls
+
+implemented_sensors = set(list(sensor_names_to_classes.keys()))
+# print(implemented_sensors)
+# print(sensor_names_to_classes)

pyrobolearn/robots/sensors/sensor.py

@@ -154,6 +154,16 @@ class Sensor(object):  # sensor attached to a link or joint
         """Return if the sensor is disabled or not."""
         return not self._enabled
 
+    @property
+    def data(self):
+        """Return the data."""
+        return self._data
+
+    @property
+    def latent_data(self):
+        """Return the latent data."""
+        return self._latent_data
+
     ###########
     # Methods #
     ###########
@@ -201,7 +211,7 @@ class Sensor(object):  # sensor attached to a link or joint
                     self._latent_data = self._data
                 else:  # if latency
                     self._latent_cnt += 1
-                    if (self._latent_cnt % self._latency) == 0:
+                    if (self._latent_cnt % self._latency) == 0:  # TODO: use FIFO queue instead (see actuator.py)?
                         self._data = self._latent_data
                         self._latent_data = self._sense(apply_noise=apply_noise)
                         self._latent_cnt = 0

pyrobolearn/simulators/bullet.py

@@ -1711,7 +1711,7 @@ class Bullet(Simulator):
             np.array[4]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed in URDF link
                 frame
             np.array[3]: world position of the URDF link frame
-            np.array[4]: world orientation of the URDF link frame
+            np.array[4]: world orientation of the URDF link frame (expressed as a quaternion [x,y,z,w])
             np.array[3]: Cartesian world linear velocity. Only returned if `compute_velocity` is True.
             np.array[3]: Cartesian world angular velocity. Only returned if `compute_velocity` is True.
         """
@@ -1795,7 +1795,34 @@ class Bullet(Simulator):
         return np.asarray([self.sim.getDynamicsInfo(body_id, link_id)[0] for link_id in link_ids])
 
     def get_link_frames(self, body_id, link_ids):
-        pass
+        r"""
+        Return the link world frame position(s) and orientation(s).
+
+        Args:
+            body_id (int): body id.
+            link_ids (int, int[N]): link id, or list of desired link ids.
+
+        Returns:
+            if 1 link:
+                np.array[3]: the link frame position in the world space
+                np.array[4]: Cartesian orientation of the link frame [x,y,z,w]
+            if multiple links:
+                np.array[N, 3]: link frame position of each link in world space
+                np.array[N, 4]: orientation of each link frame [x,y,z,w]
+        """
+        if isinstance(link_ids, int):
+            if link_ids == -1:
+                return self.get_base_pose(body_id=body_id)
+            return self.get_link_state(body_id=body_id, link_id=link_ids)[4:6]
+        positions, orientations = [], []
+        for link_id in link_ids:
+            if link_id == -1:
+                position, orientation = self.get_base_pose(body_id)
+            else:
+                position, orientation = self.get_link_state(body_id, link_id)[4:6]
+            positions.append(position)
+            orientations.append(orientation)
+        return np.asarray(positions), np.asarray(orientations)
 
     def get_link_world_positions(self, body_id, link_ids):
         """

pyrobolearn/simulators/simulator.py

@@ -1212,6 +1212,21 @@ class Simulator(object):
         pass
 
     def get_link_frames(self, body_id, link_ids):
+        r"""
+        Return the link world frame position(s) and orientation(s).
+
+        Args:
+            body_id (int): body id.
+            link_ids (int, int[N]): link id, or list of desired link ids.
+
+        Returns:
+            if 1 link:
+                np.array[3]: the link frame position in the world space
+                np.array[4]: Cartesian orientation of the link frame [x,y,z,w]
+            if multiple links:
+                np.array[N,3]: link frame position of each link in world space
+                np.array[N,4]: orientation of each link frame [x,y,z,w]
+        """
         pass
 
     def get_link_world_positions(self, body_id, link_ids):

pyrobolearn/states/robot_states/sensor_states.py

@@ -26,17 +26,17 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class SensorState(State):  # RobotState # TODO: define refresh_rate & frequency
+class SensorState(State):
     r"""Sensor state (abstract class)
     """
     __metaclass__ = ABCMeta
 
-    def __init__(self, sensors, window_size=1, axis=None, ticks=1):
+    def __init__(self, sensor, window_size=1, axis=None, ticks=1, update=False):
         """
         Initialize the sensor state.
 
         Args:
-            sensors (Sensor, list of Sensor): sensor(s).
+            sensor (Sensor): sensor instance.
             window_size (int): window size of the state. This is the total number of states we should remember. That
                 is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`,
                 the window size is 2. By default, the :attr:`window_size` is one which means we only remember the
@@ -50,19 +50,54 @@ class SensorState(State):  # RobotState # TODO: define refresh_rate & frequency
                 (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the
                 state is not a combination of states, but is given some :attr:`data`.
             ticks (int): number of ticks to sleep before getting the next state data.
+            update (bool): if we should update the sensor, or not. Note that this is normally carried out by the
+                `robot.step` method (which is itself called by `world.step`), so normally you shouldn't set it to True.
         """
-        if not isinstance(sensors, collections.Iterable):
-            sensors = [sensors]
-        for sensor in sensors:
-            if not isinstance(sensor, Sensor):
-                raise TypeError("Expecting the given 'sensor' to be an instance of `Sensor`, instead got: "
-                                "{}".format(type(sensor)))
-        self.sensors = sensors
         super(SensorState, self).__init__(window_size=window_size, axis=axis, ticks=ticks)
 
+        # set the sensor instance
+        self.sensor = sensor
+
+        self._update = bool(update)
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def sensor(self):
+        """Return the sensor instance."""
+        return self._sensor
+
+    @sensor.setter
+    def sensor(self, sensor):
+        """Set the sensor instance."""
+        if not isinstance(sensor, Sensor):
+            raise TypeError("Expecting the given 'sensor' to be an instance of `Sensor`, instead got: "
+                            "{}".format(type(sensor)))
+        self._sensor = sensor
+
+    ###########
+    # Methods #
+    ###########
+
+    def _read(self):
+        """Read the sensor values."""
+        # update the sensor if specified (normally we don't need to do it as it is carried out by robot.step, or
+        # world.step)
+        if self._update:
+            self.sensor.sense(apply_noise=True)
+
+        # get the data from the sensor
+        self.data = self.sensor.data
+
+    #############
+    # Operators #
+    #############
+
     def __copy__(self):
         """Return a shallow copy of the state. This can be overridden in the child class."""
-        return self.__class__(sensors=self.sensors, window_size=self.window_size, axis=self.axis, ticks=self.ticks)
+        return self.__class__(sensor=self.sensor, window_size=self.window_size, axis=self.axis, ticks=self.ticks)
 
     def __deepcopy__(self, memo={}):
         """Return a deep copy of the state. This can be overridden in the child class.
@@ -73,8 +108,8 @@ class SensorState(State):  # RobotState # TODO: define refresh_rate & frequency
         if self in memo:
             return memo[self]
 
-        sensors = copy.deepcopy(self.sensors, memo)
-        state = self.__class__(sensors=sensors, window_size=self.window_size, axis=self.axis, ticks=self.ticks)
+        sensor = copy.deepcopy(self.sensor, memo)
+        state = self.__class__(sensor=sensor, window_size=self.window_size, axis=self.axis, ticks=self.ticks)
 
         memo[self] = state
         return state
@@ -105,7 +140,7 @@ class CameraState(SensorState):
             ticks (int): number of ticks to sleep before getting the next state data.
         """
         self.camera = camera
-        super(CameraState, self).__init__(sensors=camera, window_size=window_size, axis=axis, ticks=ticks)
+        super(CameraState, self).__init__(sensor=camera, window_size=window_size, axis=axis, ticks=ticks)
 
     def _read(self):
         pass
@@ -145,7 +180,7 @@ class ContactState(SensorState):
                 raise TypeError("Expecting the given 'contact' to be an instance of `ContactSensor`, instead got: "
                                 "{}".format(type(contact)))
         self.contacts = contacts
-        super(ContactState, self).__init__(sensors=contacts, window_size=window_size, axis=axis, ticks=ticks)
+        super(ContactState, self).__init__(sensor=contacts, window_size=window_size, axis=axis, ticks=ticks)
 
     def _read(self):
         contacts = np.array([int(contact.is_in_contact()) for contact in self.contacts])

pyrobolearn/utils/__init__.py

@@ -3,6 +3,31 @@ import inspect
 import types
 import numpy as np
 
+# import data structures
+from . import data_structures
+
+# import transformations
+from . import transformation
+
+# import math utils
+from . import math_utils
+from . import manifold_utils
+
+# import interpolators
+from . import interpolator
+
+# import units
+from . import units
+
+# import converters
+from . import converter
+
+# import feedback laws
+from . import feedback
+
+# import real-time plotting
+from . import plotting
+
 
 # Built-in functions
 

pyrobolearn/utils/plot.py

@@ -10,11 +10,11 @@ class Arrow3D(FancyArrowPatch):
     r"""This class allows to draw a 3D arrow"""
 
     def __init__(self, xs, ys, zs, *args, **kwargs):
-        FancyArrowPatch.__init__(self, (0,0), (0,0), *args, **kwargs)
+        FancyArrowPatch.__init__(self, (0, 0), (0, 0), *args, **kwargs)
         self._verts3d = xs, ys, zs
 
     def draw(self, renderer):
         xs3d, ys3d, zs3d = self._verts3d
         xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, renderer.M)
-        self.set_positions((xs[0],ys[0]),(xs[1],ys[1]))
-        FancyArrowPatch.draw(self, renderer)
+        self.set_positions((xs[0], ys[0]), (xs[1], ys[1]))
+        FancyArrowPatch.draw(self, renderer)

pyrobolearn/utils/plotting/README.rst

@@ -1,14 +1,22 @@
 Plotting Tools
 ==============
 
-Plotting tools are extremely important in research. For this purpose, we plan to provide plotting tools that allows to
+Plotting tools are extremely important in research. For this purpose, we provide plotting tools that allows to
 plot in real-time different quantities that can be observed in the simulator, like the joint states, the position and
 orientation of each body in the world by plotting their reference frame, the position and orientation of links of a
 specific body, the resulting trajectories in the 3D Cartesian space, etc.
 
-Warnings: THIS IS CURRENTLY AN EXPERIMENTAL STAGE.
+Warnings: Currently, you have to close the figure before closing the simulator. If you close the simulator first,
+you might still have the figure process running.
 
-References
+- ``JointRealTimePlot``: plot in real-time the joint positions (in blue), velocities (in green), accelerations (in red),
+  and/or torques (in purple).
+- ``LinkFrameRealTimePlot``: plot in real-time the frames of the specified links.
+
+To test these classes, you can run the corresponding python file and move the manipulator with the mouse and check the
+real-time plots.
+
+References:
 
 - `matplotlib <https://matplotlib.org/>`_: The standard Python plotting library.
 - `Seaborn <https://seaborn.pydata.org/>`_: Seaborn is a Python data visualization library built on top of matplotlib.

pyrobolearn/utils/plotting/__init__.py

@@ -0,0 +1,7 @@
+
+
+# import joint real-time plot
+from .joint_plot import JointRealTimePlot
+
+# import link frame real-time plot
+from .frame_plot import LinkFrameRealTimePlot

pyrobolearn/utils/plotting/frame_plot.py

@@ -0,0 +1,204 @@
+#!/usr/bin/env python
+"""Define the link frame real-time plotting class.
+
+Warnings: DON'T FORGET TO CLOSE FIRST THE FIGURE THEN THE SIMULATOR OTHERWISE YOU WILL HAVE THE PLOTTING PROCESS STILL
+RUNNING
+"""
+
+import pyrobolearn as prl
+from pyrobolearn.utils.plotting.plot import RealTimePlot
+from pyrobolearn.utils.transformation import get_matrix_from_quaternion
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class LinkFrameRealTimePlot(RealTimePlot):
+    r"""Link Frame plotting tool
+
+    The link frame plotting tool plots link frames in a 3D plot in real-time.
+    """
+
+    def __init__(self, bodies, link_ids=None, xlims=None, ylims=None, zlims=None, suptitle='Link frames',
+                 ticks=1, blit=True, interval=0.0001):
+        """
+        Initialize the link frame plotting tool.
+
+        Args:
+            bodies ((list of) Body): body instance(s).
+            link_ids ((list of) list of int, (list of) int, None): link frame id(s) to plot. If None, it will take all
+                the actuated links. if -1, it will plot the base frames.
+            xlims ((list of) tuple of float, None): x-limits for each subplot.
+            ylims ((list of) tuple of float, None): y-limits for each subplot.
+            zlims ((list of) tuple of float, None): z-limits for each 3d subplot.
+            suptitle (str): main title for the subplots.
+            ticks (int): number of ticks to sleep before sending the new data.
+            blit (bool): if we should use blit, that is, if we should re-draw only the parts that have changed.
+                If blit = True, it plots faster but can only update what is inside the plot (so not the xticks,
+                yticks, xlabel, etc).
+            interval (float): Delay between frames in milliseconds.
+        """
+        # check bodies
+        if not isinstance(bodies, (list, tuple)):
+            bodies = [bodies]
+        for body in bodies:
+            if not isinstance(body, prl.robots.Body):
+                raise TypeError("Expecting each body to be an instance of `Body`, but got instead: "
+                                "{}".format(type(body)))
+        if len(bodies) == 0:
+            raise ValueError("Expecting to be given at least one body, but none were provided...")
+        self._bodies = bodies
+
+        # get simulator instance
+        self._sim = self._bodies[0].simulator
+
+        # check links
+        if isinstance(link_ids, int):
+            link_ids = [link_ids] * len(self._bodies)
+        if link_ids is None:
+            link_ids = []
+            for body in bodies:
+                links = []
+                for joint_id in range(body.num_joints):
+                    joint_info = self._sim.get_joint_info(body.id, joint_id)
+                    if joint_info[2] != self._sim.JOINT_FIXED:
+                        links.append(joint_info[0])
+
+                if len(links) == 0:  # if no links found, add the base
+                    links.append(-1)
+
+                link_ids.append(links)
+        if not isinstance(link_ids, (tuple, list)):
+            raise TypeError("Expecting the given link_ids to be a list / tuple of int, but got instead: "
+                            "{}".format(type(link_ids)))
+        if len(link_ids) != len(self._bodies):
+            if len(self._bodies) == 1:  # if one body
+                for link_id in link_ids:
+                    if not isinstance(link_id, int):
+                        raise TypeError("Expecting an int for each link id, but got instead: {}".format(type(link_id)))
+                link_ids = [link_ids]
+            else:
+                raise ValueError("Expecting the number of bodies (={}) to match with the number of set of links "
+                                 "(={})".format(len(self._bodies), len(link_ids)))
+        self._link_ids = link_ids
+
+        # set limits
+        if xlims is None:
+            xlims = (-2., 2.)
+        if ylims is None:
+            ylims = (-2., 2.)
+        if zlims is None:
+            zlims = (0., 2.)
+
+        # call parent constructor
+        super(LinkFrameRealTimePlot, self).__init__(nrows=1, ncols=1, suptitle=suptitle, xlims=xlims, ylims=ylims,
+                                                    zlims=zlims, projection='3d', ticks=ticks, blit=blit,
+                                                    interval=interval)
+
+    def _init(self, axes):
+        """Init the plots by creating the lines for each frame in the main axis."""
+        ax = axes[0]
+
+        ax.set_xlabel('x')
+        ax.set_ylabel('y')
+        ax.set_zlabel('z')
+
+        # create lines;  list of size N_body * N_links/body * 3, where 3 is for (x,y,z)
+        self._lines = []
+
+        for link_ids in self._link_ids:
+            if isinstance(link_ids, int):
+                link_ids = [link_ids]
+            for link_id in link_ids:
+                # x axis
+                line, = ax.plot(xs=[], ys=[], zs=[], lw=self._linewidths[0], color='red')
+                self._lines.append(line)
+
+                # y axis
+                line, = ax.plot(xs=[], ys=[], zs=[], lw=self._linewidths[0], color='green')
+                self._lines.append(line)
+
+                # z axis
+                line, = ax.plot(xs=[], ys=[], zs=[], lw=self._linewidths[0], color='blue')
+                self._lines.append(line)
+
+    def _init_anim(self):
+        """Init function (plot the background of each frame) that is passed to FuncAnimation. This has to be
+        implemented in the child class."""
+        for line in self._lines:
+            line.set_data([], [])
+        return self._lines
+
+    def _update_frame(self, line_idx, position, orientation):
+        """Update the lines that compose the frames."""
+        pos = position
+        rot = get_matrix_from_quaternion(orientation)  # (3,3)
+
+        # set data for x, y, z axes  (NOTE: there is no .set_data() for 3 dim data...)
+        for i in range(3):
+            new_pos = pos + rot[:, i]/10.
+            line = self._lines[line_idx+i]
+            line.set_data([pos[0], new_pos[0]], [pos[1], new_pos[1]])
+            line.set_3d_properties([pos[2], new_pos[2]])
+
+        line_idx += 3
+        return line_idx
+
+    def _animate_data(self, i, data):
+        """Animate function that is passed to FuncAnimation. This has to be implemented in the child class.
+
+        Args:
+            i (int): frame counter.
+            data (dict): data that has been received from the pipe.
+
+        Returns:
+            tuple of object: list of object to update
+        """
+        line_idx = 0
+        for i, link_ids in enumerate(self._link_ids):
+            positions, orientations = data[i]
+            for position, orientation in zip(positions, orientations):
+                line_idx = self._update_frame(line_idx, position, orientation)
+
+        return self._lines
+
+    def _update(self):
+        """This return the next data to be plotted; this has to be implemented in the child class.
+
+        Returns:
+            list: data to be sent through the pipe and that have to be plotted. This will be given to `_animate_data`.
+        """
+        data = []
+
+        for body, link_ids in zip(self._bodies, self._link_ids):
+            positions, orientations = self._sim.get_link_frames(body_id=body.id, link_ids=link_ids)
+            data.append([positions, orientations])
+
+        return data
+
+
+# Tests
+if __name__ == '__main__':
+
+    # Try to move the robot in the simulator
+    # WARNING: DON'T FORGET TO CLOSE FIRST THE FIGURE THEN THE SIMULATOR OTHERWISE YOU WILL HAVE THE PLOTTING PROCESS
+    # STILL RUNNING
+    from itertools import count
+
+    sim = prl.simulators.Bullet()
+    world = prl.worlds.BasicWorld(sim)
+    robot = world.load_robot('kuka_iiwa')
+    box = world.load_box([0.7, 0., 0.2], dimensions=(0.2, 0.2, 0.2), color=(0.2, 0.2, 0.8, 1.), return_body=True)
+
+    plot = LinkFrameRealTimePlot([robot, box], link_ids=None, ticks=24)
+
+    for t in count():
+        plot.update()
+        world.step(sim.dt)

pyrobolearn/utils/plotting/joint_plot.py

@@ -0,0 +1,208 @@
+#!/usr/bin/env python
+"""Define the joint real-time plotting class.
+
+Warnings: DON'T FORGET TO CLOSE FIRST THE FIGURE THEN THE SIMULATOR OTHERWISE YOU WILL HAVE THE PLOTTING PROCESS STILL
+RUNNING
+"""
+
+import numpy as np
+
+from pyrobolearn.utils.plotting.plot import RealTimePlot
+import pyrobolearn as prl
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class JointRealTimePlot(RealTimePlot):
+    r"""Joint plotting tool
+
+    The joint plotting tool plots the joint position, velocity, acceleration and torque values in real-time.
+    """
+
+    def __init__(self, robot, joint_ids=None, position=False, velocity=False, acceleration=False, torque=False,
+                 num_points=100, xlims=None, ylims=None, suptitle='Joint states', ticks=1, blit=True, interval=0.0001):
+        """
+        Initialize the joint plotting tool.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (list of int, int, None): joint id(s) to plot. If None, it will take all the actuated joints.
+            position (bool): if True, it will plot the joint positions.
+            velocity (bool): if True, it will plot the joint velocities.
+            acceleration (bool): if True, it will plot the joint accelerations.
+            torque (bool): if True, it will plot the joint torques.
+            num_points (int): number of points to keep in the plots.
+            xlims ((list of) tuple of float, None): x-limits for each subplot.
+            ylims ((list of) tuple of float, None): y-limits for each subplot.
+            suptitle (str): main title for the subplots.
+            ticks (int): number of ticks to sleep before sending the new data.
+            blit (bool): if we should use blit, that is, if we should re-draw only the parts that have changed.
+                If blit = True, it plots faster but can only update what is inside the plot (so not the xticks,
+                yticks, xlabel, etc).
+            interval (float): Delay between frames in milliseconds.
+        """
+        # set robot
+        if not isinstance(robot, prl.robots.Robot):
+            raise TypeError("Expecting the given 'robot' to be an instance of `Robot`, but got instead: "
+                            "{}".format(robot))
+        self._robot = robot
+
+        # set joint_ids
+        if joint_ids is None:
+            joint_ids = self._robot.joints
+        if isinstance(joint_ids, int):
+            joint_ids = [joint_ids]
+        self._joint_ids = joint_ids
+
+        nrows, ncols = 1, 1
+        if len(joint_ids) <= 4:
+            ncols = len(joint_ids)
+        else:
+            ncols = 4
+            if len(joint_ids) % ncols == 0:
+                nrows = int(len(joint_ids) / ncols)
+            else:
+                nrows = int(len(joint_ids) / ncols) + 1
+
+        # set what we should plot
+        self._plot_position = bool(position)
+        self._plot_velocity = bool(velocity)
+        self._plot_acceleration = bool(acceleration)
+        self._plot_torque = bool(torque)
+
+        states = np.array([self._plot_position, self._plot_velocity, self._plot_acceleration, self._plot_torque])
+        self._num_states = len(states[states])
+
+        if self._num_states == 0:
+            raise ValueError("Expecting to plot at least something (position, velocity, acceleration or torque)")
+
+        # set num_points
+        self._num_points = num_points if num_points > 10 else 10
+
+        # check xlim and ylim
+        if xlims is None:
+            xlims = (0, self._num_points)
+        if ylims is None:
+            ylims = (-2*np.pi, 2*np.pi)
+
+        super(JointRealTimePlot, self).__init__(nrows=nrows, ncols=ncols, xlims=xlims, ylims=ylims,
+                                                titles=self._robot.get_joint_names(self._joint_ids),
+                                                suptitle=suptitle, ticks=ticks, blit=blit, interval=interval)
+
+    def _init(self, axes):
+        """Init the plots by creating the lines in each axis."""
+        # create lines
+        self._lines = []
+
+        for i, joint_id in enumerate(self._joint_ids):
+            if self._plot_position:
+                line, = axes[i].plot([], [], lw=self._linewidths[i], color='blue')
+                self._lines.append(line)
+            if self._plot_velocity:
+                line, = axes[i].plot([], [], lw=self._linewidths[i], color='green')
+                self._lines.append(line)
+            if self._plot_acceleration:
+                line, = axes[i].plot([], [], lw=self._linewidths[i], color='red')
+                self._lines.append(line)
+            if self._plot_torque:
+                line, = axes[i].plot([], [], lw=self._linewidths[i], color='purple')
+                self._lines.append(line)
+
+        self._x = []
+        length = len(self._joint_ids) * self._num_states
+        self._ys = [[] for _ in range(length)]
+
+    def _init_anim(self):
+        """Init function (plot the background of each frame) that is passed to FuncAnimation. This has to be
+        implemented in the child class."""
+        for line in self._lines:
+            line.set_data([], [])
+        return self._lines
+
+    def _set_line(self, joint_idx, line_idx, data, state_name):
+        """Set the new data for the line.
+
+        Args:
+            joint_idx (int): joint index.
+            line_idx (int): line index.
+            data (dict): data that was sent through the pipe.
+            state_name (str): name of the state; select between {'q', 'dq', 'ddq', 'tau'}
+        """
+        self._ys[line_idx].append(data[state_name][joint_idx])
+        self._ys[line_idx] = self._ys[line_idx][-self._num_points:]
+        self._lines[line_idx].set_data(self._x, self._ys[line_idx])
+        line_idx += 1
+        return line_idx
+
+    def _animate_data(self, i, data):
+        """Animate function that is passed to FuncAnimation. This has to be implemented in the child class.
+
+        Args:
+            i (int): frame counter.
+            data (dict): data that has been received from the pipe.
+
+        Returns:
+            tuple of object: list of object to update
+        """
+        if len(self._x) < self._num_points:
+            self._x = range(len(self._x) + 1)
+
+        k = 0
+        for j in range(len(self._joint_ids)):
+            if self._plot_position:
+                k = self._set_line(joint_idx=j, line_idx=k, data=data, state_name='q')
+            if self._plot_velocity:
+                k = self._set_line(joint_idx=j, line_idx=k, data=data, state_name='dq')
+            if self._plot_acceleration:
+                k = self._set_line(joint_idx=j, line_idx=k, data=data, state_name='ddq')
+            if self._plot_torque:
+                k = self._set_line(joint_idx=j, line_idx=k, data=data, state_name='tau')
+
+        # ax.set_xlim(0 + 0.01 * i, 2 + 0.01 * i)
+        # ax.set_xticklabels(np.linspace(0.01 * i, 2 + 0.01 * i, 5))
+        return self._lines
+
+    def _update(self):
+        """This return the next data to be plotted; this has to be implemented in the child class.
+
+        Returns:
+            dict: data to be sent through the pipe and that have to be plotted. This will be given to `_animate_data`.
+        """
+        data = {}
+        if self._plot_position:
+            data['q'] = self._robot.get_joint_positions(joint_ids=self._joint_ids)
+        if self._plot_velocity:
+            data['dq'] = self._robot.get_joint_velocities(joint_ids=self._joint_ids)
+        if self._plot_acceleration:
+            data['ddq'] = self._robot.get_joint_accelerations(joint_ids=self._joint_ids)
+        if self._plot_torque:
+            data['tau'] = self._robot.get_joint_torques(joint_ids=self._joint_ids)
+        return data
+
+
+# Tests
+if __name__ == '__main__':
+
+    # Try to move the robot in the simulator
+    # WARNING: DON'T FORGET TO CLOSE FIRST THE FIGURE THEN THE SIMULATOR OTHERWISE YOU WILL HAVE THE PLOTTING PROCESS
+    # STILL RUNNING
+    from itertools import count
+
+    sim = prl.simulators.Bullet()
+    world = prl.worlds.BasicWorld(sim)
+    robot = world.load_robot('kuka_iiwa')
+
+    plot = JointRealTimePlot(robot, joint_ids=None, position=True, velocity=False, acceleration=False,
+                             torque=False, ticks=24)
+
+    for t in count():
+        plot.update()
+        world.step(sim.dt)

pyrobolearn/utils/plotting/plot.py

@@ -0,0 +1,287 @@
+#!/usr/bin/env python
+"""Define the Plot class.
+
+Warnings: THIS IS EXPERIMENTAL.
+
+Dependencies:
+- `matplotlib`
+"""
+
+import numpy as np
+from matplotlib import pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+from matplotlib import animation
+import time
+import multiprocessing
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class Plot(object):
+    """Plot (abstract) class.
+
+    The plotting tool allows to plot different things. Notably, it can plot in real-time the joint values, the
+    orientation frame of each body.
+    """
+
+    def __init__(self, nrows=1, ncols=1, suptitle=None, titles=None, xlims=None, ylims=None, zlims=None, linewidths=1,
+                 colors=None, legend=True, projection='2d'):
+        """
+        Initialize the plot.
+
+        Args:
+            nrows (int): number of rows in the subplot.
+            ncols (int): number of columns in the subplot.
+            suptitle (str): main title for the subplots.
+            titles ((list of) str): title for each subplot.
+            xlims ((list of) tuple of float, None): x-limits for each subplot.
+            ylims ((list of) tuple of float, None): y-limits for each subplot.
+            zlims ((list of) tuple of float, None): z-limits for each 3d subplot.
+            linewidths ((list of) int): linewidth for each subplot.
+            colors ((list of) str, (list of) tuple of float, None): colors described as strings, or tuple of floats
+                (for each channel in RGB or RGBA) for each subplot.
+            legend (bool): if True, it will add a legend.
+            projection (str): projection, select between {'2d', '3d'}. If '3d', it will plot in 3D.
+        """
+        self._nrows = nrows
+        self._ncols = ncols
+        self._nplots = nrows * ncols
+        self._suptitle = suptitle
+
+        def to_list(vars, name):
+            if not isinstance(vars, list):
+                vars = [vars] * self._nplots
+            # if len(vars) != self._nplots:
+            #     raise ValueError("Expecting the given '" + name + "' to be a list of the same length of the number "
+            #                      "subplots")
+            return vars
+
+        self._projection = projection
+        self._titles = to_list(titles, 'titles')
+        self._xlims = to_list(xlims, 'xlims')
+        self._ylims = to_list(ylims, 'ylims')
+        if self._projection == '3d':
+            self._zlims = to_list(zlims, 'zlims')
+        else:
+            self._zlims = None
+        self._linewidths = to_list(linewidths, 'linewidths')
+        self._colors = to_list(colors, 'colors')
+        self._legend = legend
+
+
+class RealTimePlot(Plot):
+    """Real-time plot tool
+
+    This plot class spawns a new process that is responsible to update a plot in real-time. To achieve that goal, the
+    master process sent the data (through the `RealTimePlot.update` method) through a pipe to the new process which
+    updates the plot.
+    """
+
+    def __init__(self, nrows=1, ncols=1, suptitle=None, titles=None, xlims=None, ylims=None, zlims=None,
+                 linewidths=None, colors=None, legend=True, projection='2d', ticks=1, blit=True, interval=0.0001):
+        """
+        Initialize the real-time plotting tool.
+
+        Args:
+            nrows (int): number of rows in the subplot.
+            ncols (int): number of columns in the subplot.
+            suptitle (str): main title for the subplots.
+            titles ((list of) str): title for each subplot.
+            xlims ((list of) tuple of float, None): x-limits for each subplot.
+            ylims ((list of) tuple of float, None): y-limits for each subplot.
+            zlims ((list of) tuple of float, None): z-limits for each 3d subplot.
+            linewidths ((list of)): linewidth for each subplot.
+            colors ((list of) str, (list of) tuple of float, None): colors described as strings, or tuple of floats
+                (for each channel in RGB or RGBA) for each subplot.
+            legend (bool): if True, it will add a legend.
+            projection (str): projection, select between {'2d', '3d'}. If '3d', it will plot in 3D.
+            ticks (int): number of ticks to sleep before sending the new data.
+            blit (bool): if we should use blit, that is, if we should re-draw only the parts that have changed.
+                If blit = True, it plots faster but can only update what is inside the plot (so not the xticks,
+                yticks, xlabel, etc).
+            interval (float): Delay between frames in milliseconds.
+        """
+        # init parent class
+        super(RealTimePlot, self).__init__(nrows=nrows, ncols=ncols, suptitle=suptitle, titles=titles, xlims=xlims,
+                                           ylims=ylims, zlims=zlims, linewidths=linewidths, colors=colors,
+                                           legend=legend, projection=projection)
+
+        # set variables for real plot
+        self._ticks = ticks
+        self._cnt = 0
+        self._blit = blit
+        self._interval = interval
+
+        self._plot_exist = True
+
+        # create pipe, queue, and process
+        self._pipe, pipe = multiprocessing.Pipe()
+        self._queue = multiprocessing.Queue()
+        self._process = multiprocessing.Process(target=self._plot_process, args=(pipe, self._queue))
+
+        # start process
+        self._process.start()
+
+    def _plot_process(self, pipe, queue):
+        """Initialize the plot in the child process."""
+        # set pipe and queue
+        self._pipe = pipe
+        self._queue = queue
+
+        # create subplots
+        if self._projection == '3d':
+            fig = plt.figure()  # figsize=plt.figaspect(0.5))
+            axes = []
+            for i in range(self._nplots):
+                axes.append(fig.add_subplot(self._nrows, self._ncols, i+1, projection='3d'))
+        else:
+            fig, axes = plt.subplots(nrows=self._nrows, ncols=self._ncols)
+        if not isinstance(axes, np.ndarray):
+            axes = np.array(axes)
+        axes = axes.reshape(-1)
+        self._fig, self._axes = fig, axes
+
+        # create main figure title
+        if self._suptitle is not None:
+            fig.suptitle(self._suptitle)
+
+        # set titles, xlims, and ylims
+        for ax, xlim, ylim, title in zip(axes, self._xlims, self._ylims, self._titles):
+            if xlim is not None:
+                ax.set_xlim(xlim)
+            if ylim is not None:
+                ax.set_ylim(ylim)
+            if title is not None:
+                ax.set_title(title)
+
+        if self._zlims is not None:
+            for zlim in self._zlims:
+                ax.set_zlim(zlim)
+
+        # tight layout
+        fig.tight_layout()
+
+        # def gen():
+        #     states = self.pipe.recv()
+        #     if not (isinstance(states, bool) and states):
+        #         yield states
+        #     else:
+        #         print("Over")
+
+        # initialize variables
+        self._init(axes)
+
+        # create funcanimation
+        anim = animation.FuncAnimation(fig, self._animate, init_func=self._init_anim,
+                                       frames=None, interval=self._interval, blit=self._blit)
+        plt.show()
+
+        # if we get out of the animation, notify the master process
+        self._queue.put(True)
+        self._pipe.close()
+        self._queue.close()
+
+    def _init(self, axes):
+        """Initialization of other variables before creating the animation. You can for instance create the various
+        lines here."""
+        pass
+
+    def _init_anim(self):
+        """Init function (plot the background of each frame) that is passed to FuncAnimation. This has to be
+        implemented in the child class."""
+        raise NotImplementedError
+
+    def _animate(self, i):
+        """Animate function that is passed to FuncAnimation.
+
+        Args:
+            i (int): frame counter.
+
+        Returns:
+            tuple of object: list of object to update
+        """
+        # receive data from the pipe
+        data = self._pipe.recv()
+
+        # animate the data
+        return self._animate_data(i, data)
+
+    def _animate_data(self, i, data):
+        """Animate function that is passed to FuncAnimation. This has to be implemented in the child class.
+
+        Args:
+            i (int): frame counter.
+            data (dict): data that has been received from the pipe.
+
+        Returns:
+            tuple of object: list of object to update
+        """
+        raise NotImplementedError
+
+    def update(self):
+        """Update the plot: this call `_update` and send the resulting data through the pipe."""
+        # if time to update
+        if self._cnt % self._ticks == 0 and self._plot_exist:
+
+            # get data and send it to the process through the pipe
+            data = self._update()
+            self._pipe.send(data)
+
+        self._cnt += 1
+
+        if not self._queue.empty():
+            result = self._queue.get()
+            if result:
+                print("The animation has finished. Closing process...")
+                self._process.join()
+                print("Process has been closed.")
+            else:
+                print("Got result: {}".format(result))
+
+    def _update(self):
+        """This return the next data to be plotted; this has to be implemented in the child class.
+
+        Returns:
+            dict: data to be sent through the pipe and that have to be plotted. This will be given to `_animate_data`.
+        """
+        raise NotImplementedError
+
+    def close(self):
+        """close the plotting tool."""
+        # notify the plot child process
+        self._pipe.send('END')
+
+        # wait for the child process to close
+        self._process.join()
+
+        # close queue and pipe
+        self._queue.close()
+        self._pipe.close()
+
+    def __str__(self):
+        """Return a string describing the class."""
+        return self.__class__.__name__
+
+    def __del__(self):
+        """Closing the plotting tool."""
+        self.close()
+
+    def __call__(self):
+        """update the plot."""
+        self.update()
+
+
+# class LinkPlot(RealTimePlot):
+#     r"""Link plotting tool
+#
+#     The Link plotting tool plots a link position, velocity, acceleration, force along the 3 axis (x,y,z).
+#     """
+#     pass