diff --git a/pyrobolearn/robots/base.py b/pyrobolearn/robots/base.py
index 67c0b6c..b510a15 100644
--- a/pyrobolearn/robots/base.py
+++ b/pyrobolearn/robots/base.py
@@ -10,7 +10,7 @@ import numpy as np
 # import quaternion
 
 from pyrobolearn.simulators import Simulator
-from pyrobolearn.utils.orientation import get_rpy_from_quaternion, get_matrix_from_quaternion
+from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_matrix_from_quaternion
 
 
 __author__ = "Brian Delhaisse"
diff --git a/pyrobolearn/robots/cartpole.py b/pyrobolearn/robots/cartpole.py
index 66396a1..3ac39ff 100644
--- a/pyrobolearn/robots/cartpole.py
+++ b/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 get_symbolic_matrix_from_axis_angle, get_matrix_from_quaternion
+from pyrobolearn.utils.transformation import get_symbolic_matrix_from_axis_angle, get_matrix_from_quaternion
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
diff --git a/pyrobolearn/robots/cubli.py b/pyrobolearn/robots/cubli.py
index 8c36d0b..368532e 100644
--- a/pyrobolearn/robots/cubli.py
+++ b/pyrobolearn/robots/cubli.py
@@ -5,7 +5,7 @@
 import os
 
 from pyrobolearn.robots.robot import Robot
-from pyrobolearn.utils.orientation import get_rpy_from_quaternion
+from pyrobolearn.utils.transformation import get_rpy_from_quaternion
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
diff --git a/pyrobolearn/robots/legged_robot.py b/pyrobolearn/robots/legged_robot.py
index 3a76346..2826ed6 100644
--- a/pyrobolearn/robots/legged_robot.py
+++ b/pyrobolearn/robots/legged_robot.py
@@ -138,7 +138,7 @@ class LeggedRobot(Robot):
                 raise TypeError("Expecting foot_id to be a list of int, or an int. Instead got: "
                                 "{}".format(type(foot_id)))
 
-    def center_of_pressure(self, use_simulator=False):
+    def center_of_pressure(self, floor_id=None):
         r"""
         Center of Pressure (CoP).
 
@@ -164,9 +164,16 @@ class LeggedRobot(Robot):
             [1] "Ground Reference Points in Legged Locomotion: Definitions, Biological Trajectories and Control
             Implications", Popovic et al., 2005
         """
-        # self.sim.get_contact_points(self.id, foot_id)  # use simulator
-        # use F/T sensor to get CoP
-        pass
+        if floor_id is not None:
+            # get contact points between the robot's links and the floor
+            points = self.sim.get_contact_points(body1=self.id, body2=floor_id)
+            positions = np.array([point[6] for point in points])  # contact positions in world frame
+            forces = np.array([point[9] for point in points]).reshape(-1, 1)  # normal force at contact points
+            cop = forces * positions / np.sum(forces)
+            return cop
+
+        # check if there are force/pressure sensors at the links/joints
+        raise NotImplementedError
 
     def zero_moment_point(self, update_com=False, use_simulator=False):
         r"""
diff --git a/pyrobolearn/robots/quadcopter.py b/pyrobolearn/robots/quadcopter.py
index 085b950..aead17d 100644
--- a/pyrobolearn/robots/quadcopter.py
+++ b/pyrobolearn/robots/quadcopter.py
@@ -6,7 +6,7 @@ import os
 import numpy as np
 
 from pyrobolearn.robots.uav import RotaryWingUAV
-from pyrobolearn.utils.orientation import get_matrix_from_quaternion
+from pyrobolearn.utils.transformation import get_matrix_from_quaternion
 from pyrobolearn.utils.units import inches_to_meters, rpm_to_rad_per_second
 
 __author__ = "Brian Delhaisse"
diff --git a/pyrobolearn/robots/robot.py b/pyrobolearn/robots/robot.py
index b81acce..1721762 100644
--- a/pyrobolearn/robots/robot.py
+++ b/pyrobolearn/robots/robot.py
@@ -16,7 +16,7 @@ import numpy as np
 import collections
 import os
 
-from pyrobolearn.utils.orientation import *
+from pyrobolearn.utils.transformation import *
 from pyrobolearn.robots.base import ControllableBody
 
 
@@ -3195,7 +3195,7 @@ class Robot(ControllableBody):
 
         # evals, evecs = np.linalg.eigh(X)
         # evals, evecs = evals[::-1], evecs[:,::-1]
-        # #S, orientation = np.sqrt(evals), self.angular_converter.convertFrom(quaternion.from_rotation_matrix(evecs.T))
+        # #S, orientation = np.sqrt(evals), self.angular_converter.convert_from(quaternion.from_rotation_matrix(evecs.T))
         #
         # print(V[0])
         # print(V[1])
diff --git a/pyrobolearn/robots/sensors/camera.py b/pyrobolearn/robots/sensors/camera.py
index 6f5a1a1..b227776 100644
--- a/pyrobolearn/robots/sensors/camera.py
+++ b/pyrobolearn/robots/sensors/camera.py
@@ -6,7 +6,7 @@ Cameras have one of the most richest sensory inputs (i.e. visual).
 
 import numpy as np
 
-from pyrobolearn.utils.orientation import get_rpy_from_quaternion
+from pyrobolearn.utils.transformation import get_rpy_from_quaternion
 from pyrobolearn.robots.sensors.links import LinkSensor
 
 __author__ = "Brian Delhaisse"
diff --git a/pyrobolearn/robots/sensors/links.py b/pyrobolearn/robots/sensors/links.py
index f6f22fb..2953b2f 100644
--- a/pyrobolearn/robots/sensors/links.py
+++ b/pyrobolearn/robots/sensors/links.py
@@ -6,7 +6,7 @@ These include IMU, contact, Camera, and other sensors.
 
 from abc import ABCMeta, abstractmethod
 
-from pyrobolearn.utils.orientation import get_quaternion_product
+from pyrobolearn.utils.transformation import get_quaternion_product
 from pyrobolearn.robots.sensors.sensor import Sensor
 
 
diff --git a/pyrobolearn/robots/sensors/sensor.py b/pyrobolearn/robots/sensors/sensor.py
index 0375fe2..e4566ec 100644
--- a/pyrobolearn/robots/sensors/sensor.py
+++ b/pyrobolearn/robots/sensors/sensor.py
@@ -11,7 +11,7 @@ add some noise to the returned sense value. The type of noise can also be select
 from abc import ABCMeta, abstractmethod
 import numpy as np
 
-from pyrobolearn.utils.orientation import get_quaternion_product
+from pyrobolearn.utils.transformation import get_quaternion_product
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
diff --git a/pyrobolearn/terminal_conditions/terminal_condition.py b/pyrobolearn/terminal_conditions/terminal_condition.py
index 0563a5a..fb8a7a8 100644
--- a/pyrobolearn/terminal_conditions/terminal_condition.py
+++ b/pyrobolearn/terminal_conditions/terminal_condition.py
@@ -7,7 +7,7 @@ import numpy as np
 from pyrobolearn.robots import Robot
 from pyrobolearn.states import LinkState
 
-from pyrobolearn.utils.orientation import *
+from pyrobolearn.utils.transformation import *
 
 
 __author__ = "Brian Delhaisse"
diff --git a/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_world.py b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_world.py
index c16ae41..11fb86a 100644
--- a/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_world.py
+++ b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_world.py
@@ -377,7 +377,7 @@ class BridgeMouseKeyboardWorld(Bridge):
                 # plane
                 x_screen = np.array([self.interface.mouse_x, self.interface.mouse_y, self.depth, 1])
                 x_world = self.world_camera.screen_to_world(x_screen, Vp_inv, P_inv, V_inv)[:3]
-                point = self.plane.getIntersectionPoint(x_world)
+                point = self.plane.get_intersection_point(x_world)
 
             # # draw some spheres on the plane
             # if self.display_trajectories:
diff --git a/pyrobolearn/utils/converter.py b/pyrobolearn/utils/converter.py
index 762bf58..cc084a4 100644
--- a/pyrobolearn/utils/converter.py
+++ b/pyrobolearn/utils/converter.py
@@ -1,4 +1,6 @@
-# This file describes converter classes which allows to convert from one certain data type to another.
+#!/usr/bin/env python
+"""Provide converter classes which allows to convert from one certain data type to another.
+"""
 
 from abc import ABCMeta, abstractmethod
 import numpy as np
@@ -6,6 +8,14 @@ import torch
 import quaternion
 import collections
 
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
 
 def roll(lst, shift):
     """Roll elements of a list. This is similar to `np.roll()`"""
@@ -13,10 +23,12 @@ def roll(lst, shift):
 
 
 def numpy_to_torch(tensor):
-    return torch.from_numpy(tensor)
+    """Convert from numpy array to pytorch tensor."""
+    return torch.from_numpy(tensor).float()
 
 
 def torch_to_numpy(tensor):
+    """Convert from pytorch tensor to numpy array."""
     if tensor.requires_grad:
         return tensor.detach().numpy()
     return tensor.numpy()
@@ -67,12 +79,12 @@ class TypeConverter(object):
         self._to_type = to_type
 
     @abstractmethod
-    def convertFrom(self, data):
+    def convert_from(self, data):
         """Convert to the 'from_type'"""
         raise NotImplementedError
 
     @abstractmethod
-    def convertTo(self, data):
+    def convert_to(self, data):
         """Convert to the 'to_type'"""
         raise NotImplementedError
 
@@ -81,8 +93,8 @@ class TypeConverter(object):
         Convert the data to the other type.
         """
         if isinstance(data, self.from_type): # or self.from_type is None:
-            return self.convertTo(data)
-        return self.convertFrom(data)
+            return self.convert_to(data)
+        return self.convert_from(data)
 
     def __call__(self, data):
         """
@@ -100,10 +112,10 @@ class IdentityConverter(TypeConverter):
     def __init__(self):
         super(IdentityConverter, self).__init__(None, None)
 
-    def convertFrom(self, data):
+    def convert_from(self, data):
         return data
 
-    def convertTo(self, data):
+    def convert_to(self, data):
         return data
 
 
@@ -129,7 +141,7 @@ class NumpyListConverter(TypeConverter):
             raise ValueError("Expecting the convention to belong to {0,1,2}")
         self.convention = convention
 
-    def convertFrom(self, data):
+    def convert_from(self, data):
         """Convert to list"""
         if isinstance(data, self.from_type):
             return list(data)
@@ -140,7 +152,7 @@ class NumpyListConverter(TypeConverter):
         else:
             raise TypeError("Type not known: {}".format(type(data)))
 
-    def convertTo(self, data):
+    def convert_to(self, data):
         """Convert to numpy array"""
         if isinstance(data, self.to_type):
             return data
@@ -159,13 +171,13 @@ class NumpyListConverter(TypeConverter):
     def reshape(self, data, shape):
         """Reshape the data using the converter. Only valid if data is numpy array."""
         if not isinstance(data, self.to_type):
-            data = self.convertTo(data)
+            data = self.convert_to(data)
         return data.reshape(shape)
 
     def transpose(self, data):
         """Transpose the data using the converter"""
         if not isinstance(data, self.to_type):
-            data = self.convertTo(data)
+            data = self.convert_to(data)
         return data.T
 
 
@@ -187,7 +199,7 @@ class QuaternionListConverter(TypeConverter):
             raise TypeError("Expecting convention to be 0 or 1.")
         self.convention = convention
 
-    def convertFrom(self, data):
+    def convert_from(self, data):
         """Convert to list"""
         if isinstance(data, self.from_type):
             return list(data)
@@ -196,7 +208,7 @@ class QuaternionListConverter(TypeConverter):
         else:
             raise TypeError("Type not known: {}".format(type(data)))
 
-    def convertTo(self, data):
+    def convert_to(self, data):
         """Convert to quaternion"""
         if isinstance(data, self.to_type):
             return data
@@ -224,7 +236,7 @@ class QuaternionNumpyConverter(TypeConverter):
             raise TypeError("Expecting convention to be 0 or 1.")
         self.convention = convention
 
-    def convertFrom(self, data):
+    def convert_from(self, data):
         """Convert to numpy array"""
         if isinstance(data, self.from_type):
             return data
@@ -233,7 +245,7 @@ class QuaternionNumpyConverter(TypeConverter):
         else:
             raise TypeError("Type not known: {}".format(type(data)))
 
-    def convertTo(self, data):
+    def convert_to(self, data):
         """Convert to quaternion"""
         if isinstance(data, self.to_type):
             return data
@@ -245,13 +257,13 @@ class QuaternionNumpyConverter(TypeConverter):
     def reshape(self, data, shape):
         """Reshape the data using the converter. Only valid if data is numpy array."""
         if not isinstance(data, self.from_type):
-            data = self.convertFrom(data)
+            data = self.convert_from(data)
         return data.reshape(shape)
 
     def transpose(self, data):
         """Transpose the data using the converter"""
         if not isinstance(data, self.from_type):
-            data = self.convertFrom(data)
+            data = self.convert_from(data)
         return data.T
 
 
@@ -274,7 +286,7 @@ class QuaternionPyTorchConverter(TypeConverter):
             raise TypeError("Expecting convention to be 0 or 1.")
         self.convention = convention
 
-    def convertFrom(self, data):
+    def convert_from(self, data):
         """Convert to pytorch tensor"""
         if isinstance(data, self.from_type):
             return data
@@ -283,7 +295,7 @@ class QuaternionPyTorchConverter(TypeConverter):
         else:
             raise TypeError("Type not known: {}".format(type(data)))
 
-    def convertTo(self, data):
+    def convert_to(self, data):
         """Convert to quaternion"""
         if isinstance(data, self.to_type):
             return data
@@ -295,13 +307,13 @@ class QuaternionPyTorchConverter(TypeConverter):
     def reshape(self, data, shape):
         """Reshape the data using the converter. Only valid if data is numpy array."""
         if not isinstance(data, self.from_type):
-            data = self.convertFrom(data)
+            data = self.convert_from(data)
         return data.view(shape)
 
     def transpose(self, data):
         """Transpose the data using the converter"""
         if not isinstance(data, self.from_type):
-            data = self.convertFrom(data)
+            data = self.convert_from(data)
         return data.t()
 
 
@@ -321,7 +333,7 @@ class NumpyNumberConverter(TypeConverter):
             raise ValueError("The 'dim_array' argument should be 0 or 1.")
         self.dim_array = dim_array
 
-    def convertFrom(self, data):
+    def convert_from(self, data):
         """Convert to a number"""
         if isinstance(data, self.from_type):
             return data
@@ -336,7 +348,7 @@ class NumpyNumberConverter(TypeConverter):
         else:
             raise TypeError("Type not known: {}".format(type(data)))
 
-    def convertTo(self, data):
+    def convert_to(self, data):
         """Convert to numpy array"""
         if isinstance(data, self.to_type):
             return data
@@ -370,7 +382,7 @@ class PyTorchListConverter(TypeConverter):
             raise ValueError("Expecting the convention to belong to {0,1,2}")
         self.convention = convention
 
-    def convertFrom(self, data):
+    def convert_from(self, data):
         """Convert to list"""
         if isinstance(data, self.from_type):
             return list(data)
@@ -382,7 +394,7 @@ class PyTorchListConverter(TypeConverter):
         else:
             raise TypeError("Type not known: {}".format(type(data)))
 
-    def convertTo(self, data):
+    def convert_to(self, data):
         """Convert to pytorch tensor"""
         if isinstance(data, self.to_type):
             return data
@@ -400,13 +412,13 @@ class PyTorchListConverter(TypeConverter):
     def reshape(self, data, shape):
         """Reshape the data using the converter. Only valid if data is numpy array."""
         if not isinstance(data, self.to_type):
-            data = self.convertTo(data)
+            data = self.convert_to(data)
         return data.view(shape)
 
     def transpose(self, data):
         """Transpose the data using the converter"""
         if not isinstance(data, self.to_type):
-            data = self.convertTo(data)
+            data = self.convert_to(data)
         return data.t()
 
 
@@ -419,7 +431,7 @@ class PyTorchNumpyConverter(TypeConverter):
     def __init__(self):
         super(PyTorchNumpyConverter, self).__init__(from_type=np.ndarray, to_type=torch.Tensor)
 
-    def convertFrom(self, data):
+    def convert_from(self, data):
         """Convert to numpy array"""
         if isinstance(data, self.from_type):
             return data
@@ -430,7 +442,7 @@ class PyTorchNumpyConverter(TypeConverter):
         else:
             raise TypeError("Type not known: {}".format(type(data)))
 
-    def convertTo(self, data):
+    def convert_to(self, data):
         """Convert to pytorch tensor"""
         if isinstance(data, self.to_type):
             return data
@@ -469,16 +481,16 @@ if __name__ == '__main__':
     print("on np.array: a={} with type {}".format(a, type(a)))
     b = converter(a)
     print("converter(a) gives: {} with type {}".format(b, type(b)))
-    b = converter.convertFrom(a)
-    print("converter.convertFrom(a) gives: {} with type {}".format(b, type(b)))
-    b = converter.convertTo(a)
-    print("converter.convertTo(a) gives: {} with type {}".format(b, type(b)))
+    b = converter.convert_from(a)
+    print("converter.convert_from(a) gives: {} with type {}".format(b, type(b)))
+    b = converter.convert_to(a)
+    print("converter.convert_to(a) gives: {} with type {}".format(b, type(b)))
 
     A = np.array(range(4)).reshape(2, 2)
     print("on numpy matrix: \nA={} with type {}".format(A, type(A)))
     b = converter(A)
     print("converter(a) gives: {} with type {}".format(b, type(b)))
-    b = converter.convertFrom(A)
-    print("converter.convertFrom(a) gives: {} with type {}".format(b, type(b)))
-    b = converter.convertTo(A)
-    print("converter.convertTo(a) gives: \n{} with type {}".format(b, type(b)))
+    b = converter.convert_from(A)
+    print("converter.convert_from(a) gives: {} with type {}".format(b, type(b)))
+    b = converter.convert_to(A)
+    print("converter.convert_to(a) gives: \n{} with type {}".format(b, type(b)))
diff --git a/pyrobolearn/utils/decorator.py b/pyrobolearn/utils/decorator.py
new file mode 100644
index 0000000..33e0d0a
--- /dev/null
+++ b/pyrobolearn/utils/decorator.py
@@ -0,0 +1,36 @@
+#!/usr/bin/env python
+"""Define the various decorators used in this framework.
+"""
+
+import numpy
+import torch
+
+__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 convert_numpy(f):
+    """Decorator that converts the given numpy array to a torch tensor and return it back to a numpy array if
+    specified."""
+    def wrapper(self, x, to_numpy=False):
+        # convert to torch Tensor if numpy array
+        if not isinstance(x, np.ndarray):
+            x = torch.from_numpy(x).float()
+
+        # call inner function on the given argument
+        x = f(self, x)
+
+        # reconvert to numpy array if specified, and return it
+        if to_numpy:
+            return x.numpy()
+
+        # return torch Tensor
+        return x
+
+    return wrapper
diff --git a/pyrobolearn/utils/interpolator.py b/pyrobolearn/utils/interpolator.py
index ea47d8f..c381ae4 100644
--- a/pyrobolearn/utils/interpolator.py
+++ b/pyrobolearn/utils/interpolator.py
@@ -1,6 +1,18 @@
+#!/usr/bin/env python
+"""Provide some other interpolators that are not in `scipy`.
+"""
 
 import numpy as np
 
+__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 HermiteInterpolator(object):
     r"""5th order Hermite interpolator
@@ -8,12 +20,14 @@ class HermiteInterpolator(object):
     """
 
     def __init__(self, t, x):
-        """Calculate the coefficients for the interpolation.
+        r"""Calculate the coefficients for the interpolation.
 
         Assuming a trajectory x(t) is described by a fifth order polynomial such that:
+
         .. math:: x(t) = a_5 t^5 + a_4 t^4 + a_3 t^3 + a_2 t^2 + a_1 t + a_0
 
         then taking the derivatives with respect to time give us:
+
         .. math::
             \dot{x}(t) = 5 a_5 t^4 + 4 a_4 t^3 + 3 a_3 t^2 + 2 a_2 t + a_1
             \ddot{x}(t) = 20 a_5 t^3 + 12 a_4 t^2 + 6 a_3 t + 2 a_2
@@ -47,7 +61,7 @@ class HermiteInterpolator(object):
             b = np.array([x[-1], 0, 0, 0, 0, x[0]] + list(x[1:-1]))
         else:
             b = np.array([x[-1], 0, 0, 0, 0, x[0]])
-        #coeff = np.linalg.solve(A,b)[0]
+        # coeff = np.linalg.solve(A,b)[0]
         self.coeff = np.linalg.lstsq(A, b, rcond=None)[0]
 
     def __call__(self, t):
@@ -61,7 +75,7 @@ class HermiteInterpolator(object):
             float, float[T]: velocity
             float, float[T]: acceleration
         """
-        x = np.sum(self.coeff * np.array([[ti**i for i in range(5,-1,-1)] for ti in t]), axis=1)
+        x = np.sum(self.coeff * np.array([[ti**i for i in range(5, -1, -1)] for ti in t]), axis=1)
         xd = np.sum(self.coeff[:-1] * np.array([[5*ti**4, 4*ti**3, 3*ti**2, 2*ti, 1] for ti in t]), axis=1)
         xdd = np.sum(self.coeff[:-2] * np.array([[20*ti**3, 12*ti**2, 6*ti, 2] for ti in t]), axis=1)
         return x, xd, xdd
@@ -82,19 +96,19 @@ if __name__ == '__main__':
 
     # interpolate the data
     t = np.linspace(0., 1., 100)
-    x,xd,xdd = x_interpolator(t)
-    y,yd,ydd = y_interpolator(t)
+    x, dx, ddx = x_interpolator(t)
+    y, dy, ddy = y_interpolator(t)
 
     # plot figures
-    gs = gridspec.GridSpec(4,4)
+    gs = gridspec.GridSpec(4, 4)
     plt.subplot(gs[0, 1:3])
     plt.title('Hermite Interpolator')
-    plt.plot(x,y)
+    plt.plot(x, y)
     plt.xlabel('x(t)')
     plt.ylabel('y(t)')
 
     y_labels = ['x(t)', 'y(t)', 'dx/dt', 'dy/dt', 'd^2x/dt^2', 'd^2y/dt^2']
-    for i, (x_traj, y_traj) in enumerate(zip([x, xd, xdd], [y, yd, ydd])):
+    for i, (x_traj, y_traj) in enumerate(zip([x, dx, ddx], [y, dy, ddy])):
         plt.subplot(gs[i+1, :2])
         plt.plot(t, x_traj)
         plt.ylabel(y_labels[2*i])
@@ -107,4 +121,4 @@ if __name__ == '__main__':
             plt.xlabel('t')
 
     plt.tight_layout()
-    plt.show()
\ No newline at end of file
+    plt.show()
diff --git a/pyrobolearn/utils/math_utils.py b/pyrobolearn/utils/math_utils.py
index b3b8c3f..f7f10f8 100644
--- a/pyrobolearn/utils/math_utils.py
+++ b/pyrobolearn/utils/math_utils.py
@@ -1,21 +1,35 @@
-# This file defines mathematical operations
+#!/usr/bin/env python
+"""Defines mathematical operations.
+"""
 
 import numpy as np
 import copy
 
+__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 exp(x):
     if callable(x):
         y = copy.copy(x)
-        def exp():
+
+        def exp_():
             return np.exp(x())
-        y.__call__ = exp
+
+        y.__call__ = exp_
         return y
     else:
         return np.exp(x)
 
 
 class Plane(object):
-    """Plane class.
+    r"""Plane class.
 
     A plane is defined by its initial point and its normal vector.
     .. math:: \pi \equiv \overline{n} \cdot (\overline{x} - \overline{x}_0) = 0
@@ -38,7 +52,8 @@ class Plane(object):
         self.x0 = x0
         self.normal = normal
 
-    def convertToArray(self, pt):
+    @staticmethod
+    def convert_to_array(pt):
         if isinstance(pt, (tuple, list)):
             pt = np.array(pt)
         if not isinstance(pt, np.ndarray):
@@ -56,7 +71,7 @@ class Plane(object):
 
     @x0.setter
     def x0(self, x0):
-        self._x0 = self.convertToArray(x0)
+        self._x0 = self.convert_to_array(x0)
 
     @property
     def normal(self):
@@ -64,7 +79,7 @@ class Plane(object):
 
     @normal.setter
     def normal(self, normal):
-        normal = self.convertToArray(normal)
+        normal = self.convert_to_array(normal)
         # normalize
         norm = np.linalg.norm(normal)
         if norm < self.threshold:
@@ -73,7 +88,7 @@ class Plane(object):
 
     def __contains__(self, point):
         """Check if the given point is in the plane."""
-        point = self.convertToArray(point)
+        point = self.convert_to_array(point)
 
         # scalar product between the normal and (point-x0) vectors
         val = self.normal.T.dot(point - self.x0)
@@ -82,9 +97,9 @@ class Plane(object):
             return True
         return False
 
-    def getIntersectionPoint(self, point):
+    def get_intersection_point(self, point):
         """
         Get the intersection of the plane with a line that starts at the given point and is parallel to the normal.
         """
-        point = self.convertToArray(point)
+        point = self.convert_to_array(point)
         return point + self.normal.T.dot(self.x0 - point) * self.normal
\ No newline at end of file
diff --git a/pyrobolearn/utils/mesh.py b/pyrobolearn/utils/mesh.py
index 564e62f..de655d6 100644
--- a/pyrobolearn/utils/mesh.py
+++ b/pyrobolearn/utils/mesh.py
@@ -1,13 +1,21 @@
+#!/usr/bin/env python
+"""Provide the code to create meshes using the `Mayavi` library.
+
+Most of the meshes in the world such as the `cone`, `ellipsoid`, and others were created using the hereby code.
+"""
 
 import numpy as np
+
 try:
     from mayavi import mlab
 except ImportError as e:
     raise ImportError(repr(e) + '\nTry to install Mayavi: pip install mayavi')
+
 try:
     import gdal
 except ImportError as e:
-    raise ImportError(repr(e) + '\nTry to install gdal: pip install gdal')
+    pass
+    # raise ImportError(repr(e) + '\nTry to install gdal: pip install gdal')
 
 import subprocess
 import fileinput
@@ -15,6 +23,15 @@ import sys
 import os
 import scipy.interpolate
 
+__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 recenter(coords):
     """
@@ -37,10 +54,10 @@ def recenter(coords):
 
     c_min, c_max = coords.min(), coords.max()
     c_center = c_min + (c_max - c_min) / 2.
-    return (coords - c_center)
+    return coords - c_center
 
 
-def createMesh(x, y, z, filename=None, show=False, center=True):
+def create_mesh(x, y, z, filename=None, show=False, center=True):
     """
     Create mesh from x,y,z arrays, and save it in the obj format.
 
@@ -61,32 +78,32 @@ def createMesh(x, y, z, filename=None, show=False, center=True):
 
         x, y, z = a * np.cos(theta) * np.cos(phi), b * np.cos(theta) * np.sin(phi), c * np.sin(theta)
 
-        createMesh(x, y, z, show=True)
+        create_mesh(x, y, z, show=True)
     """
-    #if not (isinstance(x, np.ndarray) and isinstance(y, np.ndarray) and isinstance(z, np.ndarray)):
-    #    raise TypeError("Expecting x, y, and z to be numpy arrays")
+    # if not (isinstance(x, np.ndarray) and isinstance(y, np.ndarray) and isinstance(z, np.ndarray)):
+    #     raise TypeError("Expecting x, y, and z to be numpy arrays")
 
     if isinstance(x, list) and isinstance(y, list) and isinstance(z, list):
         # create several 3D mesh
-        for i,j,k in zip(x,y,z):
+        for i, j, k in zip(x, y, z):
             # if we need to recenter
             if center:
-                i,j,k = recenter([i,j,k])
-            mlab.mesh(i,j,k)
+                i, j, k = recenter([i, j, k])
+            mlab.mesh(i, j, k)
     else:
         # if we need to recenter the data
         if center:
-            x,y,z = recenter([x,y,z])
+            x, y, z = recenter([x, y, z])
 
         # create 3D mesh
-        mlab.mesh(x,y,z)
+        mlab.mesh(x, y, z)
 
     # save mesh
     if filename is not None:
-        if filename[-4:] == '.obj': # This is because the .obj saved by Mayavi is not correct (see in Meshlab)
+        if filename[-4:] == '.obj':  # This is because the .obj saved by Mayavi is not correct (see in Meshlab)
             x3dfile = filename[:-4] + '.x3d'
             mlab.savefig(x3dfile)
-            convertX3dToObj(x3dfile, removeX3d=True)
+            convert_x3d_to_obj(x3dfile, removeX3d=True)
         else:
             mlab.savefig(filename)
 
@@ -97,8 +114,8 @@ def createMesh(x, y, z, filename=None, show=False, center=True):
         mlab.close()
 
 
-def createSurfMesh(surface, filename=None, show=False, subsample=None, interpolate_fct='multiquadric',
-                   lower_bound=None, upper_bound=None, dtype=None):
+def create_surf_mesh(surface, filename=None, show=False, subsample=None, interpolate_fct='multiquadric',
+                     lower_bound=None, upper_bound=None, dtype=None):
     """
     Create surface (heightmap) mesh, and save it in the obj format.
 
@@ -132,10 +149,10 @@ def createSurfMesh(surface, filename=None, show=False, subsample=None, interpola
         import numpy as np
 
         height = np.random.rand(100,100) # in meters
-        createSurfMesh(height, show=True)
+        create_surf_mesh(height, show=True)
     """
     if isinstance(surface, str):
-        from utils.heightmap_generator import heightmap_gdal
+        from pyrobolearn.worlds.utils.heightmap_generator import heightmap_gdal
         surface = heightmap_gdal(surface, subsample=subsample, interpolate_fct=interpolate_fct,
                                  lower_bound=lower_bound, upper_bound=upper_bound, dtype=dtype)
 
@@ -152,7 +169,7 @@ def createSurfMesh(surface, filename=None, show=False, subsample=None, interpola
         if filename[-4:] == '.obj':  # This is because the .obj saved by Mayavi is not correct (see in Meshlab)
             x3dfile = filename[:-4] + '.x3d'
             mlab.savefig(x3dfile)
-            convertX3dToObj(x3dfile, removeX3d=True)
+            convert_x3d_to_obj(x3dfile, removeX3d=True)
         else:
             mlab.savefig(filename)
 
@@ -163,8 +180,8 @@ def createSurfMesh(surface, filename=None, show=False, subsample=None, interpola
         mlab.close()
 
 
-def create3DMesh(heightmap, x=None, y=None, depth_level=1., filename=None, show=False, subsample=None,
-                 interpolate_fct='multiquadric', lower_bound=None, upper_bound=None, dtype=None, center=True):
+def create_3d_mesh(heightmap, x=None, y=None, depth_level=1., filename=None, show=False, subsample=None,
+                   interpolate_fct='multiquadric', lower_bound=None, upper_bound=None, dtype=None, center=True):
     """
     Create 3D mesh from heightmap (which can be a 2D array or an image (.tif, .png, .jpg, etc), and save it in
     the obj format.
@@ -206,7 +223,7 @@ def create3DMesh(heightmap, x=None, y=None, depth_level=1., filename=None, show=
         import numpy as np
 
         height = np.random.rand(100,100) # in meters
-        create3DMesh(height, show=True)
+        create_3d_mesh(height, show=True)
     """
     if isinstance(heightmap, str):
         # load data (raster)
@@ -221,8 +238,8 @@ def create3DMesh(heightmap, x=None, y=None, depth_level=1., filename=None, show=
         # 4 = column rotation (typically zero)
         # 5 = height of a pixel (typically negative)
 
-        # numpy array of shape: (channel, height, width)
-        #dem = data.ReadAsArray()
+        # # numpy array of shape: (channel, height, width)
+        # dem = data.ReadAsArray()
 
         # get elevation values (i.e. height values) with shape (height, width)
         band = data.GetRasterBand(1)
@@ -275,7 +292,7 @@ def create3DMesh(heightmap, x=None, y=None, depth_level=1., filename=None, show=
 
     # center the coordinates if specified
     if center:
-        x,y = recenter([x,y])
+        x, y = recenter([x, y])
 
     # create lower plane
     z0 = np.min(z) * np.ones(z.shape) - depth_level
@@ -287,16 +304,16 @@ def create3DMesh(heightmap, x=None, y=None, depth_level=1., filename=None, show=
     c4 = (np.vstack((x[:, -1], x[:, -1])), np.vstack((y[:, -1], y[:, -1])), np.vstack((z0[:, -1], z[:, -1])))
     c = [c1, c2, c3, c4]
 
-    # createMesh([x, x] + [i[0] for i in c], [y, y] + [i[1] for i in c], [z, z0] + [i[2] for i in c],
+    # create_mesh([x, x] + [i[0] for i in c], [y, y] + [i[1] for i in c], [z, z0] + [i[2] for i in c],
     #            filename=filename, show=show, center=False)
-    createMesh([x, x] + [i[0] for i in c], [y, y] + [i[1] for i in c], [z, z0] + [i[2] for i in c],
-               filename=filename, show=show, center=False)
+    create_mesh([x, x] + [i[0] for i in c], [y, y] + [i[1] for i in c], [z, z0] + [i[2] for i in c],
+                filename=filename, show=show, center=False)
 
 
-def createURDFFromMesh(meshfile, filename, position=(0.,0.,0.), orientation=(0.,0.,0.), scale=(1.,1.,1.),
-                       color=(1,1,1,1), texture=None, mass=0., inertia=(0.,0.,0.,0.,0.,0.),
-                       lateral_friction=0.5, rolling_friction=0., spinning_friction=0., restitution=0.,
-                       kp=None, kd=None): #, cfm=0., erf=0.):
+def create_urdf_from_mesh(meshfile, filename, position=(0., 0., 0.), orientation=(0., 0., 0.), scale=(1., 1., 1.),
+                          color=(1, 1, 1, 1), texture=None, mass=0., inertia=(0., 0., 0., 0., 0., 0.),
+                          lateral_friction=0.5, rolling_friction=0., spinning_friction=0., restitution=0.,
+                          kp=None, kd=None):  # , cfm=0., erf=0.):
     """
     Create a URDF file and insert the specified mesh inside.
 
@@ -314,6 +331,7 @@ def createURDFFromMesh(meshfile, filename, position=(0.,0.,0.), orientation=(0.,
         lateral_friction (float): friction coefficient
         rolling_friction (float): rolling friction coefficient orthogonal to contact normal
         spinning_friction (float): spinning friction coefficient around contact normal
+        restitution (float): restitution coefficient
         kp (float, None): contact stiffness (useful to make surfaces soft). Set it to None/-1 if not using it.
         kd (float, None): contact damping (useful to make surfaces soft). Set it to None/-1 if not using it.
         #cfm: constraint force mixing
@@ -328,13 +346,13 @@ def createURDFFromMesh(meshfile, filename, position=(0.,0.,0.), orientation=(0.,
         - "Tutorial: Using a URDF in Gazebo": http://gazebosim.org/tutorials/?tut=ros_urdf
         - SDF format: http://sdformat.org/spec
     """
-    def getStr(lst):
+    def get_str(lst):
         return ' '.join([str(i) for i in lst])
 
-    position = getStr(position)
-    orientation = getStr(orientation)
-    color = getStr(color)
-    scale = getStr(scale)
+    position = get_str(position)
+    orientation = get_str(orientation)
+    color = get_str(color)
+    scale = get_str(scale)
     name = meshfile.split('/')[-1][:-4]
     ixx, ixy, ixz, iyy, iyz, izz = [str(i) for i in inertia]
 
@@ -388,8 +406,7 @@ def createURDFFromMesh(meshfile, filename, position=(0.,0.,0.), orientation=(0.,
         f.write('</robot>')
 
 
-
-def convertX3dToObj(filename, removeX3d=True):
+def convert_x3d_to_obj(filename, removeX3d=True):
     """
     Convert a .x3d into an .obj file.
 
@@ -424,7 +441,7 @@ def convertX3dToObj(filename, removeX3d=True):
             raise OSError("Error while running the command `meshlabserver`: {}".format(e))
 
 
-def convertMesh(fromFilename, toFilename, removeFile=True):
+def convert_mesh(fromFilename, toFilename, removeFile=True):
     """
     Convert the given file containing the original mesh to the other specified format.
     The available formats are the ones supported by `meshlab`.
@@ -457,7 +474,7 @@ def convertMesh(fromFilename, toFilename, removeFile=True):
             raise OSError("Error while running the command `meshlabserver`: {}".format(e))
 
 
-def readObjFile(filename):
+def read_obj_file(filename):
     r"""
     Read an .obj file and returns the whole file, as well as the list of vertices, and faces.
 
@@ -493,7 +510,7 @@ def readObjFile(filename):
     return data, vertices, faces
 
 
-def flipFaceNormalsInObj(filename):
+def flip_face_normals_in_obj(filename):
     """
     Flip all the face normals in .obj file.
 
@@ -516,7 +533,7 @@ def flipFaceNormalsInObj(filename):
         f.writelines(data)
 
 
-def flipFaceNormalsForConvexObj(filename, outward=True):
+def flip_face_normals_for_convex_obj(filename, outward=True):
     """
     Flip the face normals for convex objects, and rewrite the obj file
 
@@ -526,7 +543,7 @@ def flipFaceNormalsForConvexObj(filename, outward=True):
             inward the object.
     """
     # read the obj file
-    data, vertices, faces = readObjFile(filename)
+    data, vertices, faces = read_obj_file(filename)
 
     # compute the center of the object
     center = np.mean(vertices, axis=0)
@@ -568,7 +585,7 @@ def flipFaceNormalsForConvexObj(filename, outward=True):
         f.writelines(data)
 
 
-def flipFaceNormalsForExpandedObj(filename, expanded_filename, outward=True, remove_expanded_file=False):
+def flip_face_normals_for_expanded_obj(filename, expanded_filename, outward=True, remove_expanded_file=False):
     r"""
     By comparing the expanded object with the original object, we can compute efficiently the normal vector to each
     face such that it points outward. Then comparing the direction of these obtained normal vectors with the ones
@@ -580,10 +597,11 @@ def flipFaceNormalsForExpandedObj(filename, expanded_filename, outward=True, rem
             has been expanded in every dimension.
         outward (bool): if the face normals should point outward. If False, they will be flipped such that they point
             inward the object.
+        remove_expanded_file (bool): if True, it will remove the expanded file.
     """
     # read the obj files
-    d1, v1, f1 = readObjFile(filename)
-    d2, v2, f2 = readObjFile(expanded_filename)
+    d1, v1, f1 = read_obj_file(filename)
+    d2, v2, f2 = read_obj_file(expanded_filename)
 
     # check the size of the obj files (they have to match)
     if len(v1) != len(v2) or len(f1) != len(f2):
@@ -633,52 +651,50 @@ def flipFaceNormalsForExpandedObj(filename, expanded_filename, outward=True, rem
 if __name__ == '__main__':
 
     # 1. create 3D ellipsoid mesh (see `https://en.wikipedia.org/wiki/Ellipsoid` for more info)
-    a,b,c,n = 1., 0.5, 0.5, 50
-    #a,b,c,n = .5, .5, .5, 37
+    a, b, c, n = 1., 0.5, 0.5, 50
+    # a, b, c, n = .5, .5, .5, 37
     theta, phi = np.meshgrid(np.linspace(-np.pi/2, np.pi/2, n), np.linspace(-np.pi, np.pi, n))
 
     x = a * np.cos(theta) * np.cos(phi)
     y = b * np.cos(theta) * np.sin(phi)
     z = c * np.sin(theta)
 
-    createMesh(x, y, z, show=True)
-    #createMesh(x, y, z, filename='ellipsoid.obj', show=True)
+    create_mesh(x, y, z, show=True)
+    # create_mesh(x, y, z, filename='ellipsoid.obj', show=True)
 
     # 2. create heightmap mesh
     height = np.random.rand(100,100) # in meters
-    createSurfMesh(height, show=True)
+    create_surf_mesh(height, show=True)
 
     # 3. create right triangular prism
-    x = np.array([[-0.5,-0.5],
+    x = np.array([[-0.5, -0.5],
                   [0.5, 0.5],
-                  [-0.5,-0.5],
-                  [-0.5,-0.5],
-                  [-0.5,0.5],
-                  [0.5,-0.5],
+                  [-0.5, -0.5],
+                  [-0.5, -0.5],
+                  [-0.5, 0.5],
+                  [0.5, -0.5],
                   [-0.5, 0.5],
                   [0.5, -0.5]])
-    y = np.array([[-0.5,0.5],
-                  [-0.5,0.5],
-                  [-0.5,0.5],
-                  [-0.5,0.5],
-                  [-0.5,-0.5],
-                  [-0.5,-0.5],
+    y = np.array([[-0.5, 0.5],
+                  [-0.5, 0.5],
+                  [-0.5, 0.5],
+                  [-0.5, 0.5],
+                  [-0.5, -0.5],
+                  [-0.5, -0.5],
                   [0.5, 0.5],
                   [0.5, 0.5]])
-    z = np.array([[0.,0.],
-                  [0.,0.],
-                  [1.,1.],
-                  [0.,0.],
-                  [0.,0.],
-                  [0.,1.],
+    z = np.array([[0., 0.],
+                  [0., 0.],
+                  [1., 1.],
+                  [0., 0.],
+                  [0., 0.],
+                  [0., 1.],
                   [0., 0.],
                   [0., 1.]])
 
-    #createMesh(x, y, z, show=True)
-    createMesh(x, y, z, filename='right_triangular_prism.obj', show=True)
-    flipFaceNormalsForConvexObj('right_triangular_prism.obj', outward=True)
-
-    exit()
+    # create_mesh(x, y, z, show=True)
+    create_mesh(x, y, z, filename='right_triangular_prism.obj', show=True)
+    flip_face_normals_for_convex_obj('right_triangular_prism.obj', outward=True)
 
     # 4. create cone
     radius, height, n = 0.5, 1., 50
@@ -686,17 +702,17 @@ if __name__ == '__main__':
     [h, theta] = np.meshgrid((0., height), np.linspace(0, 2*np.pi, n))
     x, y, z = r * np.cos(theta), r * np.sin(theta), h
     # close the cone at the bottom
-    [r, theta] =  np.meshgrid((0., radius), np.linspace(0, 2*np.pi, n))
+    [r, theta] = np.meshgrid((0., radius), np.linspace(0, 2*np.pi, n))
     x = np.vstack((x, r * np.cos(theta)))
     y = np.vstack((y, r * np.sin(theta)))
     z = np.vstack((z, np.zeros(r.shape)))
 
-    createMesh(x, y, z, show=True)
-    #createMesh(x, y, z, filename='cone.obj', show=True)
+    create_mesh(x, y, z, show=True)
+    # create_mesh(x, y, z, filename='cone.obj', show=True)
 
     # 5. create 3D heightmap
     dx, dy, dz = 5., 5., 0.01
-    x,y = np.meshgrid(np.linspace(-dx, dx, int(2*dx)), np.linspace(-dy, dy, int(2*dy)))
+    x, y = np.meshgrid(np.linspace(-dx, dx, int(2*dx)), np.linspace(-dy, dy, int(2*dy)))
     z = np.random.rand(*x.shape) + dz
 
     # z0 = np.zeros(x.shape)
@@ -709,6 +725,6 @@ if __name__ == '__main__':
     # c4 = (np.vstack((x[:,-1], x[:,-1])), np.vstack((y[:,-1], y[:,-1])), np.vstack((z0[:,-1], z[:,-1])))
     # c = [c1,c2,c3,c4]
     #
-    # createMesh([x,x]+[i[0] for i in c], [y,y]+[i[1] for i in c], [z,z0]+[i[2] for i in c], show=True)
+    # create_mesh([x,x]+[i[0] for i in c], [y,y]+[i[1] for i in c], [z,z0]+[i[2] for i in c], show=True)
 
-    create3DMesh(z, x, y, dz, show=True)
+    create_3d_mesh(z, x, y, dz, show=True)
diff --git a/pyrobolearn/utils/orientation.py b/pyrobolearn/utils/transformation.py
similarity index 99%
rename from pyrobolearn/utils/orientation.py
rename to pyrobolearn/utils/transformation.py
index 08ea087..1a62923 100644
--- a/pyrobolearn/utils/orientation.py
+++ b/pyrobolearn/utils/transformation.py
@@ -610,7 +610,7 @@ def logarithm_map(q):
     Returns:
         float[3]: resulting 3d vector
     """
-    q = quat_converter.convertTo(q)
+    q = quat_converter.convert_to(q)
     v, u = q.w,  np.array([q.x, q.y, q.z])
 
     zero = np.zeros(3)
@@ -647,8 +647,8 @@ def angular_velocity_from_quaternion(q1, q2):
     Returns:
         float[3]: angular velocity (angular error in :math:`R^3`)
     """
-    q1 = quat_converter.convertTo(q1)
-    q2 = quat_converter.convertTo(q2)
+    q1 = quat_converter.convert_to(q1)
+    q2 = quat_converter.convert_to(q2)
     return 2 * logarithm_map(q1 * q2)
 
 
diff --git a/pyrobolearn/worlds/world_camera.py b/pyrobolearn/worlds/world_camera.py
index c13df53..6b05775 100644
--- a/pyrobolearn/worlds/world_camera.py
+++ b/pyrobolearn/worlds/world_camera.py
@@ -9,7 +9,7 @@ Dependencies:
 
 import numpy as np
 
-from pyrobolearn.utils.orientation import get_quaternion_from_matrix, get_rpy_from_matrix, get_rpy_from_quaternion
+from pyrobolearn.utils.transformation import get_quaternion_from_matrix, get_rpy_from_matrix, get_rpy_from_quaternion
 from pyrobolearn.simulators import Simulator