diff --git a/.gitignore b/.gitignore
index 1e2cf25..e830d81 100644
--- a/.gitignore
+++ b/.gitignore
@@ -9,3 +9,7 @@ notebooks/*/.ipynb_*
 .sass-cache/
 src/*.egg-info/
 *.odg
+.pytest_cache
+*.egg-info
+*.ipynb_checkpoints
+.idea
\ No newline at end of file
diff --git a/.travis.yml b/.travis.yml
index a1e41a8..1eafd27 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -1,12 +1,10 @@
 language: python
 python:
   - "2.7"
-  - "3.4"
   - "3.5"
   - "3.6"
 install:
-  - pip install .
-script: cd tests/ikpy && ./tests.sh
+  - pip install .[plot]
+  - pip install pytest
+script: cd tests && ./tests.sh
 cache: pip
-dist: trusty
-sudo: false
diff --git a/README.md b/README.md
index d4f0138..338bb92 100644
--- a/README.md
+++ b/README.md
@@ -11,11 +11,12 @@
 ![demo](two_arms.png)
 
 # Demo
-A live demo of what IKpy can do : (click on the image below to see the video)
+Live demos of what IKPy can do : (click on the image below to see the video)
 
 [![](http://img.youtube.com/vi/H0ysr5qSbis/0.jpg)](https://www.youtube.com/watch?v=H0ysr5qSbis)
+[![](http://img.youtube.com/vi/Jq0-DkEwwj4/0.jpg)](https://www.youtube.com/watch?v=Jq0-DkEwwj4)
 
-Also, a presentation of IKPy : [Presentation](https://github.com/Phylliade/ikpy/blob/master/tutorials/ikpy/IKPy%20speech.pdf).
+Also, a presentation of IKPy : [Presentation](https://github.com/Phylliade/ikpy/blob/master/tutorials/IKPy%20speech.pdf).
 
 # Features
 With IKPy, you can :
@@ -38,7 +39,7 @@ You have three options :
    ```bash
    pip install ikpy
    ```
-  If you intend to plot your robot, you can install the plotting dependencies (mainly matplotlib) : 
+  If you intend to plot your robot, you can install the plotting dependencies (mainly matplotlib) :
   ```bash
   pip install 'ikpy[plot]'
   ```
@@ -56,7 +57,7 @@ You have three options :
    NB : You must have the proper rights to execute this command
 
 # Quickstart
-Follow this IPython [notebook](https://github.com/Phylliade/ikpy/blob/master/tutorials/ikpy/Quickstart.ipynb).
+Follow this IPython [notebook](https://github.com/Phylliade/ikpy/blob/master/tutorials/Quickstart.ipynb).
 
 
 # Guides and Tutorials
diff --git a/doc/conf.py b/doc/conf.py
index 7596685..4e8388a 100644
--- a/doc/conf.py
+++ b/doc/conf.py
@@ -41,6 +41,7 @@ def __getattr__(cls, name):
 # add these directories to sys.path here. If the directory is relative to the
 # documentation root, use os.path.abspath to make it absolute, like shown here.
 sys.path.insert(0, os.path.abspath("../src"))
+import ikpy
 
 # -- General configuration ------------------------------------------------
 
@@ -58,6 +59,7 @@ def __getattr__(cls, name):
     'sphinx.ext.coverage',
     'sphinx.ext.mathjax',
     'sphinx.ext.viewcode',
+    'sphinx.ext.napoleon'
 ]
 
 # Add any paths that contain templates here, relative to this directory.
@@ -76,7 +78,7 @@ def __getattr__(cls, name):
 
 # General information about the project.
 project = 'ikpy'
-copyright = '2015, Pierre Manceron'
+copyright = '2015-2018, Pierre Manceron'
 author = 'Pierre Manceron'
 
 # The version info for the project you're documenting, acts as replacement for
@@ -84,9 +86,11 @@ def __getattr__(cls, name):
 # built documents.
 #
 # The short X.Y version.
-version = '1.9.99-1'
+version = ikpy.__version__
+release = version
+# version = '1.9.99-1'
 # The full version, including alpha/beta/rc tags.
-release = '1.9.99-1'
+# release = '1.9.99-1'
 
 # The language for content autogenerated by Sphinx. Refer to documentation
 # for a list of supported languages.
diff --git a/doc/modules.rst b/doc/modules.rst
deleted file mode 100644
index 9536f2d..0000000
--- a/doc/modules.rst
+++ /dev/null
@@ -1,5 +0,0 @@
-ikpy
-========================
-
-.. toctree::
-   :maxdepth: 2
diff --git a/scripts/ikpy/hand_follow.py b/scripts/hand_follow.py
similarity index 100%
rename from scripts/ikpy/hand_follow.py
rename to scripts/hand_follow.py
diff --git a/setup.cfg b/setup.cfg
deleted file mode 100644
index 3c6e79c..0000000
--- a/setup.cfg
+++ /dev/null
@@ -1,2 +0,0 @@
-[bdist_wheel]
-universal=1
diff --git a/setup.py b/setup.py
index 62207d3..6752078 100644
--- a/setup.py
+++ b/setup.py
@@ -1,20 +1,31 @@
 #!/usr/bin/env python
-from setuptools import setup
+from setuptools import setup, find_packages
+import os
 
-setup(name='ikpy',
-      version='2.2.3',
-      author="Pierre Manceron",
-      description="An inverse kinematics library aiming performance and modularity",
-      url="https://github.com/Phylliade/ikpy",
-      license="GNU GENERAL PUBLIC LICENSE Version 3",
-      packages=['ikpy'],
-      package_dir={'': 'src'},
-      setup_requires=['numpy'],
-      install_requires=['numpy', 'scipy', 'sympy', "matplotlib"],
-      # TODO: Move maptlotlib to extra_requires
-      classifiers=[
-          "Programming Language :: Python :: 2",
-          "Programming Language :: Python :: 3",
-          "Topic :: Scientific/Engineering",
-      ]
-      )
+
+def get_version():
+    version = {}
+    with open(os.path.join(os.path.dirname(__file__), "src/ikpy/_version.py")) as fp:
+        exec(fp.read(), version)
+    return version["__version__"]
+
+
+setup(
+    name='ikpy',
+    version=get_version(),
+    author="Pierre Manceron",
+    description="An inverse kinematics library aiming performance and modularity",
+    url="https://github.com/Phylliade/ikpy",
+    license="GNU GENERAL PUBLIC LICENSE Version 3",
+    packages=find_packages("src", exclude=("tests", "docs")),
+    package_dir={'': 'src'},
+    setup_requires=['numpy'],
+    install_requires=['numpy', 'scipy', 'sympy'],
+    extras_require={
+        'plot': ["matplotlib"],
+    },
+    classifiers=[
+        "Programming Language :: Python :: 2",
+        "Programming Language :: Python :: 3",
+        "Topic :: Scientific/Engineering",
+    ])
diff --git a/src/ikpy/URDF_utils.py b/src/ikpy/URDF_utils.py
index c1d2fd0..08f3d36 100644
--- a/src/ikpy/URDF_utils.py
+++ b/src/ikpy/URDF_utils.py
@@ -1,6 +1,7 @@
 # coding= utf8
 """
-.. module:: URDF_utils
+URDF_utils module
+
 This module contains helper functions used to parse URDF.
 """
 
@@ -13,10 +14,15 @@
 
 
 def find_next_joint(root, current_link, next_joint_name):
-    """Find the next joint in the URDF tree
-
-    :param xml.etree.ElementTree current_link: The current URDF link
-    :param string next_joint_name: Optional : The name of the next joint
+    """
+    Find the next joint in the URDF tree
+
+    Parameters
+    ----------
+    current_link: xml.etree.ElementTree
+        The current URDF link
+    next_joint_name: str
+        Optional : The name of the next joint
     """
     # Trouver le joint attaché
     has_next = False
@@ -45,10 +51,15 @@ def find_next_joint(root, current_link, next_joint_name):
 
 
 def find_next_link(root, current_joint, next_link_name):
-    """Find the next link in the URDF tree
-
-    :param xml.etree.ElementTree current_joint: The current URDF joint
-    :param string next_link_name: Optional : The name of the next link
+    """
+    Find the next link in the URDF tree
+
+    Parameters
+    ----------
+    current_joint: xml.etree.ElementTree
+        The current URDF joint
+    next_link_name: str
+        Optional : The name of the next link
     """
     has_next = False
     next_link = None
@@ -83,14 +94,17 @@ def get_chain_from_joints(urdf_file, joints):
 
 
 def get_urdf_parameters(urdf_file, base_elements=["base_link"], last_link_vector=None, base_element_type="link"):
-    """Returns translated parameters from the given URDF file
-
-   :param urdf_file: The path of the URDF file
-   :type urdf_file: string
-   :param base_elements: List of the links beginning the chain
-   :type base_elements: list of strings
-   :param last_link_vector: Optional : The translation vector of the tip.
-   :type last_link_vector: numpy.array
+    """
+    Returns translated parameters from the given URDF file
+
+    Parameters
+    ----------
+    urdf_file: str
+        The path of the URDF file
+    base_elements: list of strings
+        List of the links beginning the chain
+    last_link_vector: numpy.array
+        Optional : The translation vector of the tip.
     """
     tree = ET.parse(urdf_file)
     root = tree.getroot()
@@ -180,7 +194,7 @@ def get_urdf_parameters(urdf_file, base_elements=["base_link"], last_link_vector
 
 
 def _get_motor_parameters(json_file):
-    """Returns a dictionnary with joints as keys, and a description (dict) of each joint as value"""
+    """Returns a dictionary with joints as keys, and a description (dict) of each joint as value"""
     with open(json_file) as motor_fd:
         global_config = json.load(motor_fd)
 
diff --git a/src/ikpy/__init__.py b/src/ikpy/__init__.py
index 35f9ec7..61765a8 100644
--- a/src/ikpy/__init__.py
+++ b/src/ikpy/__init__.py
@@ -1 +1,2 @@
 from . import chain
+from ._version import __version__
\ No newline at end of file
diff --git a/src/ikpy/_version.py b/src/ikpy/_version.py
new file mode 100644
index 0000000..fd01c2f
--- /dev/null
+++ b/src/ikpy/_version.py
@@ -0,0 +1,2 @@
+__version__ = '2.2.3'
+
diff --git a/src/ikpy/chain.py b/src/ikpy/chain.py
index 6d891a6..aa9b4ac 100644
--- a/src/ikpy/chain.py
+++ b/src/ikpy/chain.py
@@ -3,19 +3,25 @@
 .. module:: chain
 This module implements the Chain class.
 """
+import numpy as np
 
+# IKPY imports
 from . import URDF_utils
 from . import inverse_kinematics as ik
-import numpy as np
 from . import link as link_lib
 
 
 class Chain(object):
     """The base Chain class
 
-    :param list links: List of the links of the chain
-    :param list active_links_mask: A list of boolean indicating that whether or not the corresponding link is active
-    :param string name: The name of the Chain
+    Parameters
+    ----------
+    links: list
+        List of the links of the chain
+    active_links_mask: list
+        A list of boolean indicating that whether or not the corresponding link is active
+    name: str
+        The name of the Chain
     """
     def __init__(self, links, active_links_mask=None, name="chain", profile=''"", **kwargs):
         self.name = name
@@ -37,14 +43,22 @@ def __init__(self, links, active_links_mask=None, name="chain", profile=''"", **
             self.active_links_mask = np.array([True] * len(links))
 
     def __repr__(self):
-        return("Kinematic chain name={} links={} active_links={}".format(self.name, self.links, self.active_links_mask))
+        return "Kinematic chain name={} links={} active_links={}".format(self.name, self.links, self.active_links_mask)
 
     def forward_kinematics(self, joints, full_kinematics=False):
         """Returns the transformation matrix of the forward kinematics
 
-        :param list joints: The list of the positions of each joint. Note : Inactive joints must be in the list.
-        :param bool full_kinematics: Return the transorfmation matrixes of each joint
-        :returns: The transformation matrix
+        Parameters
+        ----------
+        joints: list
+            The list of the positions of each joint. Note : Inactive joints must be in the list.
+        full_kinematics: bool
+            Return the transformation matrices of each joint
+
+        Returns
+        -------
+        frame_matrix:
+            The transformation matrix
         """
         frame_matrix = np.eye(4)
 
@@ -71,9 +85,16 @@ def forward_kinematics(self, joints, full_kinematics=False):
     def inverse_kinematics(self, target, initial_position=None, **kwargs):
         """Computes the inverse kinematic on the specified target
 
-        :param numpy.array target: The frame target of the inverse kinematic, in meters. It must be 4x4 transformation matrix
-        :param numpy.array initial_position: Optional : the initial position of each joint of the chain. Defaults to 0 for each joint
-        :returns: The list of the positions of each joint according to the target. Note : Inactive joints are in the list.
+        Parameters
+        ----------
+        target: numpy.array
+            The frame target of the inverse kinematic, in meters. It must be 4x4 transformation matrix
+        initial_position: numpy.array
+            Optional : the initial position of each joint of the chain. Defaults to 0 for each joint
+
+        Returns
+        -------
+        The list of the positions of each joint according to the target. Note : Inactive joints are in the list.
         """
         # Checks on input
         target = np.array(target)
@@ -88,10 +109,16 @@ def inverse_kinematics(self, target, initial_position=None, **kwargs):
     def plot(self, joints, ax, target=None, show=False):
         """Plots the Chain using Matplotlib
 
-        :param list joints: The list of the positions of each joint
-        :param matplotlib.axes.Axes ax: A matplotlib axes
-        :param numpy.array target: An optional target
-        :param bool show: Display the axe. Defaults to False
+        Parameters
+        ----------
+        joints: list
+            The list of the positions of each joint
+        ax: matplotlib.axes.Axes
+            A matplotlib axes
+        target: numpy.array
+            An optional target
+        show: bool
+            Display the axe. Defaults to False
         """
         from . import plot_utils
 
@@ -111,14 +138,18 @@ def plot(self, joints, ax, target=None, show=False):
     def from_urdf_file(cls, urdf_file, base_elements=["base_link"], last_link_vector=None, base_element_type="link", active_links_mask=None, name="chain"):
         """Creates a chain from an URDF file
 
-       :param urdf_file: The path of the URDF file
-       :type urdf_file: string
-       :param base_elements: List of the links beginning the chain
-       :type base_elements: list of strings
-       :param last_link_vector: Optional : The translation vector of the tip.
-       :type last_link_vector: numpy.array
-       :param list active_links: The active links
-       :param string Name: The name of the Chain
+        Parameters
+        ----------
+        urdf_file: str
+            The path of the URDF file
+        base_elements: list of strings
+            List of the links beginning the chain
+        last_link_vector: numpy.array
+            Optional : The translation vector of the tip.
+        active_links: list
+            The active links
+        name: str
+            The name of the Chain
         """
         links = URDF_utils.get_urdf_parameters(urdf_file, base_elements=base_elements, last_link_vector=last_link_vector, base_element_type=base_element_type)
         # Add an origin link at the beginning
diff --git a/src/ikpy/geometry_utils.py b/src/ikpy/geometry_utils.py
index 22e0324..55c4d1f 100644
--- a/src/ikpy/geometry_utils.py
+++ b/src/ikpy/geometry_utils.py
@@ -95,8 +95,12 @@ def from_transformation_matrix(transformation_matrix):
 def to_transformation_matrix(translation, orientation_matrix=np.zeros((3, 3))):
     """Converts a tuple (translation_vector, orientation_matrix)  to a transformation matrix
 
-    :param np.array translation: The translation of your frame presented as a 3D vector.
-    :param np.array orientation_matrix: Optional : The orientation of your frame, presented as a 3x3 matrix.
+    Parameters
+    ----------
+    translation: numpy.array
+        The translation of your frame presented as a 3D vector.
+    orientation_matrix: numpy.array
+        Optional : The orientation of your frame, presented as a 3x3 matrix.
     """
     matrix = np.eye(4)
 
@@ -133,8 +137,8 @@ def cartesian_to_homogeneous_vectors(cartesian_vector, matrix_type="numpy"):
 
 
 def homogeneous_to_cartesian_vectors(homogeneous_vector):
-        """Converts a cartesian vector to an homogenous vector"""
-        return homogeneous_vector[:-1]
+    """Converts a cartesian vector to an homogenous vector"""
+    return homogeneous_vector[:-1]
 
 
 def homogeneous_to_cartesian(homogeneous_matrix):
diff --git a/src/ikpy/inverse_kinematics.py b/src/ikpy/inverse_kinematics.py
index 4fde0d8..ef2b233 100644
--- a/src/ikpy/inverse_kinematics.py
+++ b/src/ikpy/inverse_kinematics.py
@@ -5,13 +5,21 @@
 
 
 def inverse_kinematic_optimization(chain, target_frame, starting_nodes_angles, regularization_parameter=None, max_iter=None):
-    """Computes the inverse kinematic on the specified target with an optimization method
-
-    :param ikpy.chain.Chain chain: The chain used for the Inverse kinematics.
-    :param numpy.array target: The desired target.
-    :param numpy.array starting_nodes_angles: The initial pose of your chain.
-    :param float regularization_parameter: The coefficient of the regularization.
-    :param int max_iter: Maximum number of iterations for the optimisation algorithm.
+    """
+    Computes the inverse kinematic on the specified target with an optimization method
+
+    Parameters
+    ----------
+    chain: ikpy.chain.Chain
+        The chain used for the Inverse kinematics.
+    target: numpy.array
+        The desired target.
+    starting_nodes_angles: numpy.array
+        The initial pose of your chain.
+    regularization_parameter: float
+        The coefficient of the regularization.
+    max_iter: int
+        Maximum number of iterations for the optimisation algorithm.
     """
     # Only get the position
     target = target_frame[:3, 3]
diff --git a/src/ikpy/link.py b/src/ikpy/link.py
index f1f6fa9..a8e1465 100644
--- a/src/ikpy/link.py
+++ b/src/ikpy/link.py
@@ -9,14 +9,17 @@
 
 
 class Link(object):
-    """Base Link class.
-
-    :param name: The name of the link
-    :type name: string
-    :param bounds: Optional : The bounds of the link. Defaults to None
-    :type bounds: tuple
-    :param use_symbolic_matrix: wether the transformation matrix is stored as Numpy array or as a Sympy symbolic matrix.
-    :type use_symbolic_matrix: bool
+    """
+    Base Link class.
+
+    Parameters
+    ----------
+    name: string
+        The name of the link
+    bounds: tuple
+        Optional : The bounds of the link. Defaults to None
+    use_symbolic_matrix: bool
+        wether the transformation matrix is stored as Numpy array or as a Sympy symbolic matrix.
     """
 
     def __init__(self, name, bounds=(None, None)):
@@ -117,16 +120,23 @@ def get_transformation_matrix(self, theta):
 class DHLink(Link):
     """Link in Denavit-Hartenberg representation.
 
-   :param name: The name of the link
-   :type name: string
-   :param bounds: Optional : The bounds of the link. Defaults to None
-   :type bounds: tuple
-   :param float d: offset along previous z to the common normal
-   :param float a: offset along previous   to the common normal
-   :param use_symbolic_matrix: wether the transformation matrix is stored as Numpy array or as a Sympy symbolic matrix.
-   :type use_symbolic_matrix: bool
-   :returns: The link object
-   :rtype: DHLink
+    Parameters
+    ----------
+    name: str
+        The name of the link
+    bounds: tuple
+        Optional : The bounds of the link. Defaults to None
+    d: float
+        offset along previous z to the common normal
+    a: float
+        offset along previous   to the common normal
+    use_symbolic_matrix: bool
+        whether the transformation matrix is stored as Numpy array or as a Sympy symbolic matrix.
+
+    Returns
+    -------
+    DHLink:
+        The link object
     """
 
     def __init__(self, name, d=0, a=0, bounds=None, use_symbolic_matrix=True):
diff --git a/tests/ikpy/params.py b/tests/ikpy/params.py
deleted file mode 100644
index 9938642..0000000
--- a/tests/ikpy/params.py
+++ /dev/null
@@ -1,9 +0,0 @@
-import sys
-
-resources_path = "../../resources"
-interactive = True
-try:
-    if sys.argv[1] == "--no-interactive":
-        interactive = False
-except:
-    pass
diff --git a/tests/ikpy/test_chain.py b/tests/ikpy/test_chain.py
deleted file mode 100644
index ae18d4e..0000000
--- a/tests/ikpy/test_chain.py
+++ /dev/null
@@ -1,49 +0,0 @@
-import unittest
-import numpy as np
-import sys
-from ikpy import chain
-from ikpy import plot_utils
-import params
-
-plot = params.interactive
-
-
-class TestChain(unittest.TestCase):
-    def setUp(self):
-        if plot:
-            self.ax = plot_utils.init_3d_figure()
-        self.chain1 = chain.Chain.from_urdf_file(params.resources_path + "/poppy_torso.URDF", base_elements=["base", "abs_z", "spine", "bust_y", "bust_motors", "bust_x", "chest", "r_shoulder_y"], last_link_vector=[0, 0.18, 0], active_links_mask=[False, False, False, False, True, True, True, True, True])
-        self.joints = [0] * len(self.chain1.links)
-        self.joints[-4] = 0
-        self.target = [0.1, -0.2, 0.1]
-        self.frame_target = np.eye(4)
-        self.frame_target[:3, 3] = self.target
-
-    def test_chain(self):
-        self.chain1 = chain.Chain.from_urdf_file(params.resources_path + "/poppy_torso.URDF", base_elements=["base", "abs_z", "spine", "bust_y", "bust_motors", "bust_x", "chest", "r_shoulder_y"], last_link_vector=[0, 0.18, 0], active_links_mask=[False, False, False, False, True, True, True, True, True])
-        self.chain2 = chain.Chain.from_urdf_file(params.resources_path + "/poppy_torso.URDF", base_elements=["base", "abs_z", "spine", "bust_y", "bust_motors", "bust_x", "chest", "l_shoulder_y"], last_link_vector=[0, 0.18, 0], active_links_mask=[False, False, False, False, True, True, True, True, True])
-
-        if plot:
-            self.chain1.plot(self.joints, self.ax)
-            self.chain2.plot(self.joints, self.ax)
-
-    def test_ik(self):
-
-        ik = self.chain1.inverse_kinematics(self.frame_target, initial_position=self.joints)
-
-        if plot:
-            self.chain1.plot(ik, self.ax, target=self.target)
-            plot_utils.show_figure()
-
-        np.testing.assert_almost_equal(self.chain1.forward_kinematics(ik)[:3, 3], self.target, decimal=3)
-
-    def test_ik_optimization(self):
-        """Tests the IK optimization-based method"""
-        args = {"max_iter": 3}
-        ik = self.chain1.inverse_kinematics(self.frame_target, initial_position=self.joints, **args)
-        # Check whether the results are almost equal
-        np.testing.assert_almost_equal(self.chain1.forward_kinematics(ik)[:3, 3], self.target, decimal=1)
-
-
-if __name__ == '__main__':
-    unittest.main(verbosity=2, argv=[sys.argv[0]])
diff --git a/tests/ikpy/tests.sh b/tests/ikpy/tests.sh
deleted file mode 100755
index 335c069..0000000
--- a/tests/ikpy/tests.sh
+++ /dev/null
@@ -1,3 +0,0 @@
-#!/bin/bash
-
-python test_chain.py --no-interactive
diff --git a/tests/params.py b/tests/params.py
new file mode 100644
index 0000000..3fc983d
--- /dev/null
+++ b/tests/params.py
@@ -0,0 +1,10 @@
+import sys
+
+resources_path = "../resources"
+
+interactive = False
+try:
+    if sys.argv[1] == "--interactive":
+        interactive = True
+except KeyError:
+    pass
diff --git a/tests/test_chain.py b/tests/test_chain.py
new file mode 100644
index 0000000..721317a
--- /dev/null
+++ b/tests/test_chain.py
@@ -0,0 +1,83 @@
+import unittest
+import numpy as np
+import sys
+
+# IKPy imports
+from ikpy import chain
+from ikpy import plot_utils
+import params
+from ikpy.chain import Chain
+
+plot = params.interactive
+
+
+class TestChain(unittest.TestCase):
+    def setUp(self):
+        if plot:
+            self.ax = plot_utils.init_3d_figure()
+        self.chain1 = chain.Chain.from_urdf_file(
+            params.resources_path + "/poppy_torso.URDF",
+            base_elements=[
+                "base", "abs_z", "spine", "bust_y", "bust_motors", "bust_x",
+                "chest", "r_shoulder_y"
+            ],
+            last_link_vector=[0, 0.18, 0],
+            active_links_mask=[
+                False, False, False, False, True, True, True, True, True
+            ])
+        self.joints = [0] * len(self.chain1.links)
+        self.joints[-4] = 0
+        self.target = [0.1, -0.2, 0.1]
+        self.frame_target = np.eye(4)
+        self.frame_target[:3, 3] = self.target
+
+    def test_chain(self):
+        self.chain1 = chain.Chain.from_urdf_file(
+            params.resources_path + "/poppy_torso.URDF",
+            base_elements=[
+                "base", "abs_z", "spine", "bust_y", "bust_motors", "bust_x",
+                "chest", "r_shoulder_y"
+            ],
+            last_link_vector=[0, 0.18, 0],
+            active_links_mask=[
+                False, False, False, False, True, True, True, True, True
+            ])
+        self.chain2 = chain.Chain.from_urdf_file(
+            params.resources_path + "/poppy_torso.URDF",
+            base_elements=[
+                "base", "abs_z", "spine", "bust_y", "bust_motors", "bust_x",
+                "chest", "l_shoulder_y"
+            ],
+            last_link_vector=[0, 0.18, 0],
+            active_links_mask=[
+                False, False, False, False, True, True, True, True, True
+            ])
+
+        if plot:
+            self.chain1.plot(self.joints, self.ax)
+            self.chain2.plot(self.joints, self.ax)
+
+    def test_ik(self):
+
+        ik = self.chain1.inverse_kinematics(
+            self.frame_target, initial_position=self.joints)
+
+        if plot:
+            self.chain1.plot(ik, self.ax, target=self.target)
+            plot_utils.show_figure()
+
+        np.testing.assert_almost_equal(
+            self.chain1.forward_kinematics(ik)[:3, 3], self.target, decimal=3)
+
+    def test_ik_optimization(self):
+        """Tests the IK optimization-based method"""
+        args = {"max_iter": 3}
+        ik = self.chain1.inverse_kinematics(
+            self.frame_target, initial_position=self.joints, **args)
+        # Check whether the results are almost equal
+        np.testing.assert_almost_equal(
+            self.chain1.forward_kinematics(ik)[:3, 3], self.target, decimal=1)
+
+
+if __name__ == '__main__':
+    unittest.main(verbosity=2, argv=[sys.argv[0]])
diff --git a/tests/ikpy/test_poppy_robots.py b/tests/test_poppy_robots.py
similarity index 97%
rename from tests/ikpy/test_poppy_robots.py
rename to tests/test_poppy_robots.py
index 5fb72c3..ca9d3cf 100644
--- a/tests/ikpy/test_poppy_robots.py
+++ b/tests/test_poppy_robots.py
@@ -1,9 +1,11 @@
 import unittest
 import numpy as np
 import params
+import sys
+
+# IKpy imports
 from ikpy import chain
 from ikpy import plot_utils
-import sys
 
 plot = params.interactive
 
diff --git a/tests/tests.sh b/tests/tests.sh
new file mode 100755
index 0000000..05e4cb7
--- /dev/null
+++ b/tests/tests.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+pytest test_chain.py
diff --git a/tutorials/ikpy/FK Ergo Jr.png b/tutorials/FK Ergo Jr.png
similarity index 100%
rename from tutorials/ikpy/FK Ergo Jr.png
rename to tutorials/FK Ergo Jr.png
diff --git a/tutorials/ikpy/Hand follow.ipynb b/tutorials/Hand follow.ipynb
similarity index 100%
rename from tutorials/ikpy/Hand follow.ipynb
rename to tutorials/Hand follow.ipynb
diff --git a/tutorials/ikpy/IK Ergo.png b/tutorials/IK Ergo.png
similarity index 100%
rename from tutorials/ikpy/IK Ergo.png
rename to tutorials/IK Ergo.png
diff --git a/tutorials/IK Torso.png b/tutorials/IK Torso.png
new file mode 100644
index 0000000..c13f2c5
Binary files /dev/null and b/tutorials/IK Torso.png differ
diff --git a/tutorials/ikpy/IKPy Speech.pptx b/tutorials/IKPy Speech.pptx
similarity index 100%
rename from tutorials/ikpy/IKPy Speech.pptx
rename to tutorials/IKPy Speech.pptx
diff --git a/tutorials/ikpy/IKPy speech.pdf b/tutorials/IKPy speech.pdf
similarity index 100%
rename from tutorials/ikpy/IKPy speech.pdf
rename to tutorials/IKPy speech.pdf
diff --git a/tutorials/ikpy/Inverse Kinematics with ErgoJr.ipynb b/tutorials/Inverse Kinematics with ErgoJr.ipynb
similarity index 100%
rename from tutorials/ikpy/Inverse Kinematics with ErgoJr.ipynb
rename to tutorials/Inverse Kinematics with ErgoJr.ipynb
diff --git a/tutorials/ikpy/Moving the Poppy Torso using Inverse Kinematics.ipynb b/tutorials/Moving the Poppy Torso using Inverse Kinematics.ipynb
similarity index 100%
rename from tutorials/ikpy/Moving the Poppy Torso using Inverse Kinematics.ipynb
rename to tutorials/Moving the Poppy Torso using Inverse Kinematics.ipynb
diff --git a/tutorials/ikpy/Quickstart.ipynb b/tutorials/Quickstart.ipynb
similarity index 100%
rename from tutorials/ikpy/Quickstart.ipynb
rename to tutorials/Quickstart.ipynb
diff --git a/tutorials/ikpy/README.md b/tutorials/README.md
similarity index 100%
rename from tutorials/ikpy/README.md
rename to tutorials/README.md
diff --git a/tutorials/ikpy/URDF.md b/tutorials/URDF.md
similarity index 100%
rename from tutorials/ikpy/URDF.md
rename to tutorials/URDF.md
diff --git a/tutorials/ikpy/banner.png b/tutorials/banner.png
similarity index 100%
rename from tutorials/ikpy/banner.png
rename to tutorials/banner.png
diff --git a/tutorials/ikpy/chain.md b/tutorials/chain.md
similarity index 100%
rename from tutorials/ikpy/chain.md
rename to tutorials/chain.md
diff --git a/tutorials/ikpy/chain.png b/tutorials/chain.png
similarity index 100%
rename from tutorials/ikpy/chain.png
rename to tutorials/chain.png
diff --git a/tutorials/ikpy/contributing.md b/tutorials/contributing.md
similarity index 100%
rename from tutorials/ikpy/contributing.md
rename to tutorials/contributing.md
diff --git a/tutorials/ikpy/dual-plot.png b/tutorials/dual-plot.png
similarity index 100%
rename from tutorials/ikpy/dual-plot.png
rename to tutorials/dual-plot.png
diff --git a/tutorials/ikpy/fk.png b/tutorials/fk.png
similarity index 100%
rename from tutorials/ikpy/fk.png
rename to tutorials/fk.png
diff --git a/tutorials/ikpy/getting_started.md b/tutorials/getting_started.md
similarity index 100%
rename from tutorials/ikpy/getting_started.md
rename to tutorials/getting_started.md
diff --git a/tutorials/ikpy/hand_follow.png b/tutorials/hand_follow.png
similarity index 100%
rename from tutorials/ikpy/hand_follow.png
rename to tutorials/hand_follow.png
diff --git a/tutorials/ikpy/ik.png b/tutorials/ik.png
similarity index 100%
rename from tutorials/ikpy/ik.png
rename to tutorials/ik.png
diff --git a/tutorials/ikpy/ikpy-convention.png b/tutorials/ikpy-convention.png
similarity index 100%
rename from tutorials/ikpy/ikpy-convention.png
rename to tutorials/ikpy-convention.png
diff --git a/tutorials/ikpy/inverse_kinematics.md b/tutorials/inverse_kinematics.md
similarity index 100%
rename from tutorials/ikpy/inverse_kinematics.md
rename to tutorials/inverse_kinematics.md
diff --git a/tutorials/ikpy/link-mask.png b/tutorials/link-mask.png
similarity index 100%
rename from tutorials/ikpy/link-mask.png
rename to tutorials/link-mask.png
diff --git a/tutorials/ikpy/link.md b/tutorials/link.md
similarity index 100%
rename from tutorials/ikpy/link.md
rename to tutorials/link.md
diff --git a/tutorials/ikpy/plotting.md b/tutorials/plotting.md
similarity index 100%
rename from tutorials/ikpy/plotting.md
rename to tutorials/plotting.md
diff --git a/tutorials/ikpy/plotting.png b/tutorials/plotting.png
similarity index 100%
rename from tutorials/ikpy/plotting.png
rename to tutorials/plotting.png
diff --git a/tutorials/ikpy/right_arm.png b/tutorials/right_arm.png
similarity index 100%
rename from tutorials/ikpy/right_arm.png
rename to tutorials/right_arm.png
diff --git a/tutorials/ikpy/urdf-convention.png b/tutorials/urdf-convention.png
similarity index 100%
rename from tutorials/ikpy/urdf-convention.png
rename to tutorials/urdf-convention.png