diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml
new file mode 100644
index 00000000..7b751b75
--- /dev/null
+++ b/.github/workflows/tests.yml
@@ -0,0 +1,26 @@
+name: Adam
+
+on:
+  push:
+  pull_request:
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    strategy:
+      matrix:
+        python-version: [3.8]
+
+    steps:
+      - uses: actions/checkout@v2
+      - name: Set up Python ${{ matrix.python-version }}
+        uses: actions/setup-python@v2
+        with:
+          python-version: ${{ matrix.python-version }}
+      - name: Install dependencies
+        run: |
+          python -m pip install --upgrade pip
+          pip install .[test]
+      - name: Test with pytest
+        run: |
+          pytest
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 00000000..2c558a5e
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,139 @@
+**.vscode
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/
diff --git a/README.md b/README.md
index b9d31786..1bdbcaf0 100644
--- a/README.md
+++ b/README.md
@@ -1,2 +1,87 @@
 # ADAM
-Automatic Differentiation for rigid body dynamics algorithMs
+
+**Automatic Differentiation for rigid-body-dynamics AlgorithMs**
+
+This library implements kinematics and dynamics algorithms for **floating-base** robots, in _mixed representation_ (see [Traversaro's A Unified View of the Equations of Motion used for Control Design of Humanoid Robots](https://www.researchgate.net/publication/312200239_A_Unified_View_of_the_Equations_of_Motion_used_for_Control_Design_of_Humanoid_Robots)).
+
+ADAM employs [CasADi](https://web.casadi.org/), which embeds the computed kinematics and dynamics quantities in expression graphs and provides if needed, gradients, Jacobians, and Hessians. This approach enables the design of optimal control strategies in robotics. Using its `CodeGenerator`, CasADi enables also the generation of C-code - usable also in `Matlab` or `C++`.
+
+Adam is based on Roy Featherstone's Rigid Body Dynamics Algorithms.
+
+This work is still at an early stage and bugs could jump out!
+PRs are welcome! :rocket:
+
+## :hammer: Dependencies
+
+- [`python3`](https://wiki.python.org/moin/BeginnersGuide)
+
+Other requisites are:
+
+- `urdf_parser_py`
+- `casadi`
+
+They will be installed in the installation step!
+
+## :floppy_disk: Installation
+
+Install `python3`, if not installed (in **Ubuntu 20.04**):
+
+```bash
+sudo apt install python3.8
+```
+
+Clone the repo and install the library:
+
+```bash
+git clone https://github.com/dic-iit/ADAM.git
+cd ADAM
+pip install .
+```
+
+preferably in a [virtual environment](https://docs.python.org/3/library/venv.html#venv-def). For example:
+
+```bash
+pip install virtualenv
+python3 -m venv your_virtual_env
+source your_virtual_env/bin/activate
+```
+
+## :rocket: Usage
+
+```python
+from adam.core.computations import KinDynComputations
+import gym_ignition_models
+import numpy as np
+
+# if you want to use gym-ignition https://github.com/robotology/gym-ignition to retrieve the urdf
+model_path = gym_ignition_models.get_model_file("iCubGazeboV2_5")
+# The joint list
+joints_name_list = [
+    'torso_pitch', 'torso_roll', 'torso_yaw', 'l_shoulder_pitch',
+    'l_shoulder_roll', 'l_shoulder_yaw', 'l_elbow', 'r_shoulder_pitch',
+    'r_shoulder_roll', 'r_shoulder_yaw', 'r_elbow', 'l_hip_pitch', 'l_hip_roll',
+    'l_hip_yaw', 'l_knee', 'l_ankle_pitch', 'l_ankle_roll', 'r_hip_pitch',
+    'r_hip_roll', 'r_hip_yaw', 'r_knee', 'r_ankle_pitch', 'r_ankle_roll'
+]
+# Specify the root link
+root_link = 'root_link'
+kinDyn = KinDynComputations(urdf_path, joints_name_list, root_link)
+w_H_b = np.eye(4)
+joints = np.ones(len(joints_name_list))
+M = kinDyn.mass_matrix_fun()
+print(M(w_H_b, joints))
+```
+
+## Todo
+
+- [x] Center of Mass position
+- [x] Jacobians
+- [x] Forward kinematics
+- [x] Mass Matrix via CRBA
+- [x] Centroidal Momentum Matrix via CRBA
+- [ ] Recursive Newton-Euler algorithm
+- [ ] Articulated Body algorithm
+
+---
+
+The structure of the library is inspired by the module [urdf2casadi](https://github.com/mahaarbo/urdf2casadi/blob/master/README.md), which generates kinematic and dynamics quantities using CasADi, for Fixed-Base robots. Please check their interesting work!
diff --git a/adam/__init__.py b/adam/__init__.py
new file mode 100644
index 00000000..dbefbdf3
--- /dev/null
+++ b/adam/__init__.py
@@ -0,0 +1,2 @@
+from . import core
+from . import geometry
diff --git a/adam/core/__init__.py b/adam/core/__init__.py
new file mode 100644
index 00000000..fcae3def
--- /dev/null
+++ b/adam/core/__init__.py
@@ -0,0 +1 @@
+from . import computations
diff --git a/adam/core/computations.py b/adam/core/computations.py
new file mode 100644
index 00000000..3fd44cf2
--- /dev/null
+++ b/adam/core/computations.py
@@ -0,0 +1,471 @@
+import logging
+from dataclasses import dataclass, field
+from os import error
+
+import casadi as cs
+import numpy as np
+from prettytable import PrettyTable
+from urdf_parser_py.urdf import URDF
+
+from adam.geometry import utils
+
+
+@dataclass
+class Tree:
+    joints: list = field(default_factory=list)
+    links: list = field(default_factory=list)
+    parents: list = field(default_factory=list)
+    N: int = None
+
+
+@dataclass
+class Element:
+    name: str
+    idx: int = None
+
+
+class KinDynComputations:
+    """This is a small class that retrieves robot quantities represented in a symbolic fashion using CasADi
+    in mixed representation, for Floating Base systems - as humanoid robots.
+    """
+
+    joint_types = ["prismatic", "revolute", "continuous"]
+
+    def __init__(
+        self,
+        urdfstring: str,
+        joints_name_list: list,
+        root_link: str = "root_link",
+        f_opts: dict = dict(jit=False, jit_options=dict(flags="-Ofast")),
+    ) -> None:
+        """
+        Args:
+            urdfstring (str): path of the urdf
+            joints_name_list (list): list of the actuated joints
+            root_link (str, optional): the first link. Defaults to 'root_link'.
+        """
+        self.robot_desc = URDF.from_xml_file(urdfstring)
+        self.joints_list = self.get_joints_info_from_reduced_model(joints_name_list)
+        self.NDoF = len(self.joints_list)
+        self.root_link = root_link
+        self.f_opts = f_opts
+        (
+            self.links_with_inertia,
+            frames,
+            self.connecting_joints,
+            self.tree,
+        ) = self.load_model()
+
+    def get_joints_info_from_reduced_model(self, joints_name_list: list) -> list:
+        joints_list = []
+        for item in self.robot_desc.joint_map:
+            self.robot_desc.joint_map[item].idx = None
+
+        for [idx, joint_str] in enumerate(joints_name_list):
+            # adding the field idx to the reduced joint list
+            self.robot_desc.joint_map[joint_str].idx = idx
+            joints_list += [self.robot_desc.joint_map[joint_str]]
+        if len(joints_list) != len(joints_name_list):
+            raise error("Some joints are not in the URDF")
+        return joints_list
+
+    def load_model(self):
+        """This function computes the branched tree graph.
+
+        Returns:
+            The list of links, frames, the connecting joints and the tree.
+            It also prints the urdf elements.
+        """
+        links = []
+        frames = []
+        joints = []
+        tree = Tree()
+        # If the link does not have inertia is considered a frame
+        for item in self.robot_desc.link_map:
+            if self.robot_desc.link_map[item].inertial is not None:
+                links += [item]
+            else:
+                frames += [item]
+
+        table_links = PrettyTable(["Idx", "Link name"])
+        table_links.title = "Links"
+        for [i, item] in enumerate(links):
+            table_links.add_row([i, item])
+        logging.debug(table_links)
+        table_frames = PrettyTable(["Idx", "Frame name", "Parent"])
+        table_frames.title = "Frames"
+        for [i, item] in enumerate(frames):
+            try:
+                table_frames.add_row(
+                    [
+                        i,
+                        item,
+                        self.robot_desc.parent_map[item][1],
+                    ]
+                )
+            except:
+                pass
+        logging.debug(table_frames)
+        """The node 0 contains the 1st link, the fictitious joint that connects the root the the world
+        and the world"""
+        tree.links.append(self.robot_desc.link_map[self.root_link])
+        joint_0 = Element("fictitious_joint")
+        tree.joints.append(joint_0)
+        parent_0 = Element("world_link")
+        tree.parents.append(parent_0)
+
+        i = 1
+
+        table_joints = PrettyTable(["Idx", "Joint name", "Type", "Parent", "Child"])
+        table_joints.title = "Joints"
+        # Building the tree. Links (with inertia) are connected with joints
+        for item in self.robot_desc.joint_map:
+            # I'm assuming that the only possible active joint is revolute (not prismatic)
+            parent = self.robot_desc.joint_map[item].parent
+            child = self.robot_desc.joint_map[item].child
+            if (
+                self.robot_desc.link_map[child].inertial is not None
+                and self.robot_desc.link_map[parent].inertial is not None
+            ):
+                joints += [item]
+                table_joints.add_row(
+                    [
+                        i,
+                        item,
+                        self.robot_desc.joint_map[item].type,
+                        parent,
+                        child,
+                    ]
+                )
+                i += 1
+                tree.joints.append(self.robot_desc.joint_map[item])
+                tree.links.append(
+                    self.robot_desc.link_map[self.robot_desc.joint_map[item].child]
+                )
+                tree.parents.append(
+                    self.robot_desc.link_map[self.robot_desc.joint_map[item].parent]
+                )
+        tree.N = len(tree.links)
+        logging.debug(table_joints)
+        return links, frames, joints, tree
+
+    def crba(self):
+        """This function computes the Composite Rigid body algorithm (Roy Featherstone) that computes the Mass Matrix.
+         The algorithm is complemented with Orin's modifications computing the Centroidal Momentum Matrix
+
+        Returns:
+            M (casADi function): Mass Matrix
+            Jcm (casADi function): Centroidal Momentum Matrix
+        """
+        q = cs.SX.sym("q", self.NDoF)
+        T_b = cs.SX.sym("T_b", 4, 4)
+        Ic = [None] * len(self.tree.links)
+        X_p = [None] * len(self.tree.links)
+        Phi = [None] * len(self.tree.links)
+        M = cs.SX.zeros(self.NDoF + 6, self.NDoF + 6)
+
+        for i in range(self.tree.N):
+            link_i = self.tree.links[i]
+            link_pi = self.tree.parents[i]
+            joint_i = self.tree.joints[i]
+            I = link_i.inertial.inertia
+            mass = link_i.inertial.mass
+            o = link_i.inertial.origin.xyz
+            rpy = link_i.inertial.origin.rpy
+            Ic[i] = utils.spatial_inertia(I, mass, o, rpy)
+
+            if link_i.name == self.root_link:
+                # The first "real" link. The joint is universal.
+                X_p[i] = utils.spatial_transform(np.eye(3), np.zeros(3))
+                Phi[i] = cs.np.eye(6)
+            elif joint_i.type == "fixed":
+                X_J = utils.X_fixed_joint(joint_i.origin.xyz, joint_i.origin.rpy)
+                X_p[i] = X_J
+                Phi[i] = cs.vertcat(0, 0, 0, 0, 0, 0)
+            elif joint_i.type == "revolute":
+                if joint_i.idx is not None:
+                    q_ = q[joint_i.idx]
+                else:
+                    q_ = 0.0
+                X_J = utils.X_revolute_joint(
+                    joint_i.origin.xyz,
+                    joint_i.origin.rpy,
+                    joint_i.axis,
+                    q_,
+                )
+                X_p[i] = X_J
+                Phi[i] = cs.vertcat(
+                    [
+                        0,
+                        0,
+                        0,
+                        joint_i.axis[0],
+                        joint_i.axis[1],
+                        joint_i.axis[2],
+                    ]
+                )
+
+        for i in range(self.tree.N - 1, -1, -1):
+            link_i = self.tree.links[i]
+            link_pi = self.tree.parents[i]
+            joint_i = self.tree.joints[i]
+            if link_pi.name != self.tree.parents[0].name:
+                pi = self.tree.links.index(link_pi)
+                Ic[pi] = Ic[pi] + X_p[i].T @ Ic[i] @ X_p[i]
+            F = Ic[i] @ Phi[i]
+            if joint_i.idx is not None:
+                M[joint_i.idx + 6, joint_i.idx + 6] = Phi[i].T @ F
+            if link_i.name == self.root_link:
+                M[:6, :6] = Phi[i].T @ F
+            j = i
+            link_j = self.tree.links[j]
+            link_pj = self.tree.parents[j]
+            joint_j = self.tree.joints[j]
+            while self.tree.parents[j].name != self.tree.parents[0].name:
+                F = X_p[j].T @ F
+                j = self.tree.links.index(self.tree.parents[j])
+                joint_j = self.tree.joints[j]
+                if joint_i.name == self.tree.joints[0].name and joint_j.idx is not None:
+                    M[:6, joint_j.idx + 6] = F.T @ Phi[j]
+                    M[joint_j.idx + 6, :6] = M[:6, joint_j.idx + 6].T
+                elif (
+                    joint_j.name == self.tree.joints[0].name and joint_i.idx is not None
+                ):
+                    M[joint_i.idx + 6, :6] = F.T @ Phi[j]
+                    M[:6, joint_i.idx + 6] = M[joint_i.idx + 6, :6].T
+                elif joint_i.idx is not None and joint_j.idx is not None:
+                    M[joint_i.idx + 6, joint_j.idx + 6] = F.T @ Phi[j]
+                    M[joint_j.idx + 6, joint_i.idx + 6] = M[
+                        joint_i.idx + 6, joint_j.idx + 6
+                    ].T
+
+        X_G = [None] * len(self.tree.links)
+        O_X_G = cs.SX.eye(6)
+        O_X_G[:3, 3:] = M[:3, 3:6].T / M[0, 0]
+        Jcm = cs.SX.zeros(6, self.NDoF + 6)
+        for i in range(self.tree.N):
+            link_i = self.tree.links[i]
+            link_pi = self.tree.parents[i]
+            joint_i = self.tree.joints[i]
+            if link_pi.name != self.tree.parents[0].name:
+                pi = self.tree.links.index(link_pi)
+                pi_X_G = X_G[pi]
+            else:
+                pi_X_G = O_X_G
+            X_G[i] = X_p[i] @ pi_X_G
+            if link_i.name == self.tree.links[0].name:
+                Jcm[:, :6] = X_G[i].T @ Ic[i] @ Phi[i]
+            elif joint_i.idx is not None:
+                Jcm[:, joint_i.idx + 6] = X_G[i].T @ Ic[i] @ Phi[i]
+
+        # Until now the algorithm returns the quantities in Body Fixed representation
+        # Moving to mixed representation...
+        X_to_mixed = cs.SX.eye(self.NDoF + 6)
+        X_to_mixed[:3, :3] = T_b[:3, :3].T
+        X_to_mixed[3:6, 3:6] = T_b[:3, :3].T
+        M = X_to_mixed.T @ M @ X_to_mixed
+        Jcc = X_to_mixed[:6, :6].T @ Jcm @ X_to_mixed
+
+        M = cs.Function("M", [T_b, q], [M], self.f_opts)
+        Jcm = cs.Function("Jcm", [T_b, q], [Jcc], self.f_opts)
+        return M, Jcm
+
+    def mass_matrix_fun(self):
+        """Returns the Mass Matrix functions computed the CRBA
+
+        Returns:
+            M (casADi function): Mass Matrix
+        """
+        [M, _] = self.crba()
+        return M
+
+    def centroidal_momentum_matrix_fun(self):
+        """Returns the Centroidal Momentum Matrix functions computed the CRBA
+
+        Returns:
+            Jcc (casADi function): Centroidal Momentum matrix
+        """
+        [_, Jcm] = self.crba()
+        return Jcm
+
+    def forward_kinematics_fun(self, frame):
+        """Computes the forward kinematics relative to the specified frame
+
+        Args:
+            frame (str): The frame to which the fk will be computed
+
+        Returns:
+            T_fk (casADi function): The fk represented as Homogenous transformation matrix
+        """
+        chain = self.robot_desc.get_chain(self.root_link, frame)
+        q = cs.SX.sym("q", self.NDoF)
+        T_b = cs.SX.sym("T_b", 4, 4)
+        T_fk = cs.SX.eye(4)
+        T_fk = T_fk @ T_b
+        for item in chain:
+            if item in self.robot_desc.joint_map:
+                joint = self.robot_desc.joint_map[item]
+                if joint.type == "fixed":
+                    xyz = joint.origin.xyz
+                    rpy = joint.origin.rpy
+                    joint_frame = utils.H_from_PosRPY(xyz, rpy)
+                    T_fk = T_fk @ joint_frame
+                if joint.type == "revolute":
+                    # if the joint is actuated set the value
+                    if joint.idx is not None:
+                        q_ = q[joint.idx]
+                    else:
+                        q_ = 0.0
+                    T_joint = utils.H_revolute_joint(
+                        joint.origin.xyz,
+                        joint.origin.rpy,
+                        joint.axis,
+                        q_,
+                    )
+                    T_fk = T_fk @ T_joint
+        T_fk = cs.Function("T_fk", [T_b, q], [T_fk], self.f_opts)
+        return T_fk
+
+    def jacobian_fun(self, frame):
+        """Returns the Jacobian relative to the specified frame
+
+        Args:
+            frame (str): The frame to which the jacobian will be computed
+
+        Returns:
+            J_tot (casADi function): The Jacobian relative to the frame
+        """
+        chain = self.robot_desc.get_chain(self.root_link, frame)
+        q = cs.SX.sym("q", self.NDoF)
+        T_b = cs.SX.sym("T_b", 4, 4)
+        T_fk = cs.SX.eye(4)
+        T_fk = T_fk @ T_b
+        J = cs.SX.zeros(6, self.NDoF)
+        T_ee = self.forward_kinematics_fun(frame)
+        T_ee = T_ee(T_b, q)
+        P_ee = T_ee[:3, 3]
+        for item in chain:
+            if item in self.robot_desc.joint_map:
+                joint = self.robot_desc.joint_map[item]
+                if joint.type == "fixed":
+                    xyz = joint.origin.xyz
+                    rpy = joint.origin.rpy
+                    joint_frame = utils.H_from_PosRPY(xyz, rpy)
+                    T_fk = T_fk @ joint_frame
+                if joint.type == "revolute":
+                    if joint.idx is not None:
+                        q_ = q[joint.idx]
+                    else:
+                        q_ = 0.0
+                    T_joint = utils.H_revolute_joint(
+                        joint.origin.xyz,
+                        joint.origin.rpy,
+                        joint.axis,
+                        q_,
+                    )
+                    T_fk = T_fk @ T_joint
+                    p_prev = P_ee - T_fk[:3, 3]
+                    z_prev = T_fk[:3, :3] @ joint.axis
+                    # J[:, joint.idx] = cs.vertcat(
+                    #     cs.jacobian(P_ee, q[joint.idx]), z_prev) # using casadi jacobian
+                    if joint.idx is not None:
+                        J[:, joint.idx] = cs.vertcat(cs.skew(z_prev) @ p_prev, z_prev)
+
+        # Adding the floating base part of the Jacobian, in Mixed representation
+        J_tot = cs.SX.zeros(6, self.NDoF + 6)
+        J_tot[:3, :3] = cs.np.eye(3)
+        J_tot[:3, 3:6] = -cs.skew((P_ee - T_b[:3, 3]))
+        J_tot[:3, 6:] = J[:3, :]
+        J_tot[3:, 3:6] = cs.np.eye(3)
+        J_tot[3:, 6:] = J[3:, :]
+        return cs.Function("J_tot", [T_b, q], [J_tot], self.f_opts)
+
+    def relative_jacobian_fun(self, frame):
+        """Returns the Jacobian between the root link and a specified frame frames
+
+        Args:
+            frame (str): The tip of the chain
+
+        Returns:
+            J (casADi function): The Jacobian between the root and the frame
+        """
+        chain = self.robot_desc.get_chain(self.root_link, frame)
+        q = cs.SX.sym("q", self.NDoF)
+        T_b = np.eye(4)
+        T_fk = cs.SX.eye(4)
+        T_fk = T_fk @ T_b
+        J = cs.SX.zeros(6, self.NDoF)
+        T_ee = self.forward_kinematics_fun(frame)
+        T_ee = T_ee(T_b, q)
+        P_ee = T_ee[:3, 3]
+        for item in chain:
+            if item in self.robot_desc.joint_map:
+                joint = self.robot_desc.joint_map[item]
+                if joint.type == "fixed":
+                    xyz = joint.origin.xyz
+                    rpy = joint.origin.rpy
+                    joint_frame = utils.H_from_PosRPY(xyz, rpy)
+                    T_fk = T_fk @ joint_frame
+                if joint.type == "revolute":
+                    if joint.idx is not None:
+                        q_ = q[joint.idx]
+                    else:
+                        q_ = 0.0
+                    T_joint = utils.H_revolute_joint(
+                        joint.origin.xyz,
+                        joint.origin.rpy,
+                        joint.axis,
+                        q_,
+                    )
+                    T_fk = T_fk @ T_joint
+                    p_prev = P_ee - T_fk[:3, 3]
+                    z_prev = T_fk[:3, :3] @ joint.axis
+                    # J[:, joint.idx] = cs.vertcat(
+                    #     cs.jacobian(P_ee, q[joint.idx]), z_prev) # using casadi jacobian
+                    if joint.idx is not None:
+                        J[:, joint.idx] = cs.vertcat(cs.skew(z_prev) @ p_prev, z_prev)
+        return cs.Function("J", [q], [J], self.f_opts)
+
+    def CoM_position_fun(self):
+        """Returns the CoM positon
+
+        Returns:
+            com (casADi function): The CoM position
+        """
+        q = cs.SX.sym("q", self.NDoF)
+        T_b = cs.SX.sym("T_b", 4, 4)
+        com_pos = cs.SX.zeros(3, 1)
+        for item in self.robot_desc.link_map:
+            link = self.robot_desc.link_map[item]
+            if link.inertial is not None:
+                fk = self.forward_kinematics_fun(item)
+                T_fk = fk(T_b, q)
+                T_link = utils.H_from_PosRPY(
+                    link.inertial.origin.xyz,
+                    link.inertial.origin.rpy,
+                )
+                # Adding the link transform
+                T_fk = T_fk @ T_link
+                com_pos += T_fk[:3, 3] * link.inertial.mass
+        com_pos /= self.get_total_mass()
+        com = cs.Function("CoM_pos", [T_b, q], [com_pos], self.f_opts)
+        return com
+
+    def get_total_mass(self):
+        """Returns the total mass of the robot
+
+        Returns:
+            mass: The total mass
+        """
+        mass = 0.0
+        for item in self.robot_desc.link_map:
+            link = self.robot_desc.link_map[item]
+            if link.inertial is not None:
+                mass += link.inertial.mass
+        return mass
+
+    def aba(self):
+        raise NotImplementedError
+
+    def rnea(self):
+        raise NotImplementedError
diff --git a/adam/geometry/__init__.py b/adam/geometry/__init__.py
new file mode 100644
index 00000000..eb018c3f
--- /dev/null
+++ b/adam/geometry/__init__.py
@@ -0,0 +1 @@
+from . import utils
diff --git a/adam/geometry/utils.py b/adam/geometry/utils.py
new file mode 100644
index 00000000..43c5500c
--- /dev/null
+++ b/adam/geometry/utils.py
@@ -0,0 +1,94 @@
+import casadi as cs
+import numpy as np
+
+
+def R_from_axisAngle(axis, q):
+    [cq, sq] = [cs.np.cos(q), cs.np.sin(q)]
+    return cq * (cs.np.eye(3) - cs.np.outer(
+        axis, axis)) + sq * cs.skew(axis) + cs.np.outer(axis, axis)
+
+
+def Rx(q):
+    R = cs.np.eye(3)
+    [cq, sq] = [cs.np.cos(q), cs.np.sin(q)]
+    R[1, 1] = cq
+    R[1, 2] = -sq
+    R[2, 1] = sq
+    R[2, 2] = cq
+    return R
+
+
+def Ry(q):
+    R = cs.np.eye(3)
+    [cq, sq] = [cs.np.cos(q), cs.np.sin(q)]
+    R[0, 0] = cq
+    R[0, 2] = sq
+    R[2, 0] = -sq
+    R[2, 2] = cq
+    return R
+
+
+def Rz(q):
+    R = cs.np.eye(3)
+    [cq, sq] = [cs.np.cos(q), cs.np.sin(q)]
+    R[0, 0] = cq
+    R[0, 1] = -sq
+    R[1, 0] = sq
+    R[1, 1] = cq
+    return R
+
+
+def H_revolute_joint(xyz, rpy, axis, q):
+    T = cs.SX.eye(4)
+    R = R_from_RPY(rpy) @ R_from_axisAngle(axis, q)
+    T[:3, :3] = R
+    T[:3, 3] = xyz
+    return T
+
+
+def H_from_PosRPY(xyz, rpy):
+    T = cs.SX.eye(4)
+    T[:3, :3] = R_from_RPY(rpy)
+    T[:3, 3] = xyz
+    return T
+
+
+def R_from_RPY(rpy):
+    return Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0])
+
+
+def X_revolute_joint(xyz, rpy, axis, q):
+    T = H_revolute_joint(xyz, rpy, axis, q)
+    R = T[:3, :3].T
+    p = -T[:3, :3].T @ T[:3, 3]
+    return spatial_transform(R, p)
+
+
+def X_fixed_joint(xyz, rpy):
+    T = H_from_PosRPY(xyz, rpy)
+    R = T[:3, :3].T
+    p = -T[:3, :3].T @ T[:3, 3]
+    return spatial_transform(R, p)
+
+
+def spatial_transform(R, p):
+    X = cs.SX.zeros(6, 6)
+    X[:3, :3] = R
+    X[3:, 3:] = R
+    X[:3, 3:] = cs.skew(p) @ R
+    return X
+
+
+def spatial_inertia(I, mass, c, rpy):
+    # Returns the 6x6 inertia matrix expressed at the origin of the link (with rotation)"""
+    IO = np.zeros([6, 6])
+    Sc = cs.skew(c)
+    R = R_from_RPY(rpy)
+    inertia_matrix = np.array([[I.ixx, I.ixy, I.ixz], [I.ixy, I.iyy, I.iyz],
+                               [I.ixz, I.iyz, I.izz]])
+
+    IO[3:, 3:] = R @ inertia_matrix @ R.T + mass * Sc @ Sc.T
+    IO[3:, :3] = mass * Sc
+    IO[:3, 3:] = mass * Sc.T
+    IO[:3, :3] = np.eye(3) * mass
+    return IO
diff --git a/examples/iCub_example.ipynb b/examples/iCub_example.ipynb
new file mode 100644
index 00000000..731205c2
--- /dev/null
+++ b/examples/iCub_example.ipynb
@@ -0,0 +1,324 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 1,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "from adam.core.computations import KinDynComputations\n",
+    "from adam.geometry import utils\n",
+    "import numpy as np\n",
+    "import casadi as cs\n",
+    "import gym_ignition_models"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 2,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "def SX2DM(sx):\n",
+    "    return cs.DM(sx)"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Import the robot .urdf"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 3,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "urdf_path = gym_ignition_models.get_model_file(\"iCubGazeboV2_5\")\n",
+    "# The joint list\n",
+    "joints_name_list = [\n",
+    "    'torso_pitch', 'torso_roll', 'torso_yaw', 'l_shoulder_pitch',\n",
+    "    'l_shoulder_roll', 'l_shoulder_yaw', 'l_elbow', 'r_shoulder_pitch',\n",
+    "    'r_shoulder_roll', 'r_shoulder_yaw', 'r_elbow', 'l_hip_pitch', 'l_hip_roll',\n",
+    "    'l_hip_yaw', 'l_knee', 'l_ankle_pitch', 'l_ankle_roll', 'r_hip_pitch',\n",
+    "    'r_hip_roll', 'r_hip_yaw', 'r_knee', 'r_ankle_pitch', 'r_ankle_roll'\n",
+    "]\n",
+    "# Specify the root link\n",
+    "root_link = 'root_link'\n",
+    "kinDyn = KinDynComputations(urdf_path, joints_name_list, root_link)"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "The class `KinDynComputations` contains some algorithms for computing kinematics and dynamics quantities."
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 4,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Set joints and base informations\n",
+    "xyz = (np.random.rand(3) - 0.5) * 5\n",
+    "rpy = (np.random.rand(3) - 0.5) * 5\n",
+    "H_b = utils.H_from_PosRPY(xyz, rpy)\n",
+    "s = (np.random.rand(len(joints_name_list)) - 0.5) * 5"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Mass Matrix"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 5,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "Mass matrix:\n",
+      " \n",
+      "[[33.0617, -1.52991e-07, -7.91225e-07, 4.27496e-08, 1.96372, 2.50452, -0.0308722, 0.849117, 0.12387, -0.112802, -0.276508, -0.00909112, 0.0955269, 0.0553714, 0.124658, 0.0286805, -0.0497435, 0.0497159, -0.377198, 0.000395589, 0.345906, 0.03316, -0.0166539, 0.501034, -0.432756, -0.116524, -0.0142668, -0.00898903, 0.000204422], \n",
+      " [-1.52991e-07, 33.0617, -7.5361e-07, -1.96372, 8.43762e-08, 0.457341, -0.0197084, -1.76589, 0.0189669, 0.150905, -0.24933, 0.00806951, 0.0515341, -0.00418433, -0.172034, -0.0436057, -0.0200833, -0.327716, -0.30262, 0.350843, -0.0119803, -0.000841855, -0.0267633, 0.397722, -0.791713, -0.0103325, -0.419296, 0.0261161, -0.0254815], \n",
+      " [-7.91225e-07, -7.5361e-07, 33.0617, -2.50452, -0.457341, -1.27126e-07, 0.0248853, -0.830971, -0.104893, -0.066278, -0.131213, -0.00499986, 0.00616735, 0.0751473, 0.053772, 0.00941355, 0.0945284, -0.169438, 0.825284, 0.0615738, 0.10875, 0.00733727, 0.00976799, -0.375844, -0.90432, 0.308022, -0.048272, -0.00175218, -0.0209692], \n",
+      " [4.27496e-08, -1.96372, -2.50452, 1.25934, 0.158548, -0.305028, -0.3642, 0.40519, -0.34481, 0.0726921, -0.0686651, 0.0229273, 0.0257489, 0.0274445, 0.030289, 0.0456178, 0.0107734, 0.0159522, -0.0741768, 0.0666021, -0.00717085, -0.000168328, 0.00162301, 0.056486, 0.422399, -0.086926, 0.101746, -0.00984058, 0.00205437], \n",
+      " [1.96372, 8.43762e-08, -0.457341, 0.158548, 1.69768, 0.301456, 0.11764, 0.39405, 0.0756867, 0.0552735, 0.120654, 0.00423185, -0.0508867, 0.0634495, 0.0465701, 0.0277804, -0.0375793, -0.06539, 0.333647, 0.018986, -0.0344046, 0.00869359, 0.00488396, 0.134699, 0.120623, -0.122742, 0.0113996, -0.00208752, 0.0101335], \n",
+      " [2.50452, 0.457341, -1.27126e-07, -0.305028, 0.301456, 1.69396, -0.311646, -0.241263, -0.332056, 0.00408696, -0.084454, -0.0333137, 0.0245496, -0.0198824, 0.0759545, -0.0108111, -0.00219649, 0.119839, 0.0835489, -0.107754, 0.0183681, 0.00248664, 0.0162793, 0.251178, -0.320288, -0.0374391, -0.132775, 0.0162472, -0.0122335], \n",
+      " [-0.0308722, -0.0197084, 0.0248853, -0.3642, 0.11764, -0.311646, 0.492721, -0.024679, 0.482767, -0.0400862, 0.134092, 0.00747153, -0.046854, 0.00962879, -0.0610274, -0.0178179, -0.0151525, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], \n",
+      " [0.849117, -1.76589, -0.830971, 0.40519, 0.39405, -0.241263, -0.024679, 0.623785, -0.0245149, 0.0721879, 0.0555767, 0.0337858, -0.0212364, 0.0662734, 0.0138571, 0.0524753, -0.0114547, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], \n",
+      " [0.12387, 0.0189669, -0.104893, -0.34481, 0.0756867, -0.332056, 0.482767, -0.0245149, 0.487535, -0.0432377, 0.115291, 0.0104733, -0.0385444, 0.00451623, -0.0712937, -0.0183787, -0.0130443, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], \n",
+      " [-0.112802, 0.150905, -0.066278, 0.0726921, 0.0552735, 0.00408696, -0.0400862, 0.0721879, -0.0432377, 0.0860211, 0.000652103, 0.0392729, -0.00488403, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], \n",
+      " [-0.276508, -0.24933, -0.131213, -0.0686651, 0.120654, -0.084454, 0.134092, 0.0555767, 0.115291, 0.000652103, 0.13854, 0.000699203, -0.0613198, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], \n",
+      " [-0.00909112, 0.00806951, -0.00499986, 0.0229273, 0.00423185, -0.0333137, 0.00747153, 0.0337858, 0.0104733, 0.0392729, 0.000699203, 0.0416039, -0.000754623, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], \n",
+      " [0.0955269, 0.0515341, 0.00616735, 0.0257489, -0.0508867, 0.0245496, -0.046854, -0.0212364, -0.0385444, -0.00488403, -0.0613198, -0.000754623, 0.0464212, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], \n",
+      " [0.0553714, -0.00418433, 0.0751473, 0.0274445, 0.0634495, -0.0198824, 0.00962879, 0.0662734, 0.00451623, 0, 0, 0, 0, 0.0713998, 0.0025755, 0.0452363, -0.0114505, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], \n",
+      " [0.124658, -0.172034, 0.053772, 0.030289, 0.0465701, 0.0759545, -0.0610274, 0.0138571, -0.0712937, 0, 0, 0, 0, 0.0025755, 0.0739118, 0.00243631, -0.00495002, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], \n",
+      " [0.0286805, -0.0436057, 0.00941355, 0.0456178, 0.0277804, -0.0108111, -0.0178179, 0.0524753, -0.0183787, 0, 0, 0, 0, 0.0452363, 0.00243631, 0.0484814, 0.000433161, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], \n",
+      " [-0.0497435, -0.0200833, 0.0945284, 0.0107734, -0.0375793, -0.00219649, -0.0151525, -0.0114547, -0.0130443, 0, 0, 0, 0, -0.0114505, -0.00495002, 0.000433161, 0.0464232, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], \n",
+      " [0.0497159, -0.327716, -0.169438, 0.0159522, -0.06539, 0.119839, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0670466, -0.0157421, 0.000755258, 0.00621635, -0.00168042, 0.0104786, 0, 0, 0, 0, 0, 0], \n",
+      " [-0.377198, -0.30262, 0.825284, -0.0741768, 0.333647, 0.0835489, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.0157421, 0.244363, -0.0119447, -0.0451288, 0.00754562, 0.00827687, 0, 0, 0, 0, 0, 0], \n",
+      " [0.000395589, 0.350843, 0.0615738, 0.0666021, 0.018986, -0.107754, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.000755258, -0.0119447, 0.066696, 0.000797967, 0.000314587, 0.0029047, 0, 0, 0, 0, 0, 0], \n",
+      " [0.345906, -0.0119803, 0.10875, -0.00717085, -0.0344046, 0.0183681, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.00621635, -0.0451288, 0.000797967, 0.068878, -0.00650014, -0.00199813, 0, 0, 0, 0, 0, 0], \n",
+      " [0.03316, -0.000841855, 0.00733727, -0.000168328, 0.00869359, 0.00248664, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.00168042, 0.00754562, 0.000314587, -0.00650014, 0.0132919, -0.000458705, 0, 0, 0, 0, 0, 0], \n",
+      " [-0.0166539, -0.0267633, 0.00976799, 0.00162301, 0.00488396, 0.0162793, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0104786, 0.00827687, 0.0029047, -0.00199813, -0.000458705, 0.0120266, 0, 0, 0, 0, 0, 0], \n",
+      " [0.501034, 0.397722, -0.375844, 0.056486, 0.134699, 0.251178, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.151819, -0.060109, -0.0269055, -0.0561646, 0.00687754, -0.0128629], \n",
+      " [-0.432756, -0.791713, -0.90432, 0.422399, 0.120623, -0.320288, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.060109, 0.422419, -0.061983, 0.134292, -0.0169671, 0.00965696], \n",
+      " [-0.116524, -0.0103325, 0.308022, -0.086926, -0.122742, -0.0374391, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.0269055, -0.061983, 0.0656413, -0.0038013, 1.77151e-05, -0.0122275], \n",
+      " [-0.0142668, -0.419296, -0.048272, 0.101746, 0.0113996, -0.132775, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.0561646, 0.134292, -0.0038013, 0.0920327, -0.0177409, 0.00615115], \n",
+      " [-0.00898903, 0.0261161, -0.00175218, -0.00984058, -0.00208752, 0.0162472, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.00687754, -0.0169671, 1.77151e-05, -0.0177409, 0.0126946, -0.000569881], \n",
+      " [0.000204422, -0.0254815, -0.0209692, 0.00205437, 0.0101335, -0.0122335, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.0128629, 0.00965696, -0.0122275, 0.00615115, -0.000569881, 0.0120268]]\n"
+     ]
+    }
+   ],
+   "source": [
+    "M = kinDyn.mass_matrix_fun()\n",
+    "print('Mass matrix:\\n', SX2DM(M(H_b, s)))"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Centroidal Momentum Matrix"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 6,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "Centroidal Momentum Matrix:\n",
+      " \n",
+      "[[33.0617, -1.52991e-07, -7.91225e-07, 4.27496e-08, 1.96372, 2.50452, -0.0308722, 0.849117, 0.12387, -0.112802, -0.276508, -0.00909112, 0.0955269, 0.0553714, 0.124658, 0.0286805, -0.0497435, 0.0497159, -0.377198, 0.000395589, 0.345906, 0.03316, -0.0166539, 0.501034, -0.432756, -0.116524, -0.0142668, -0.00898903, 0.000204422], \n",
+      " [-1.52991e-07, 33.0617, -7.5361e-07, -1.96372, 8.43762e-08, 0.457341, -0.0197084, -1.76589, 0.0189669, 0.150905, -0.24933, 0.00806951, 0.0515341, -0.00418433, -0.172034, -0.0436057, -0.0200833, -0.327716, -0.30262, 0.350843, -0.0119803, -0.000841855, -0.0267633, 0.397722, -0.791713, -0.0103325, -0.419296, 0.0261161, -0.0254815], \n",
+      " [-7.91225e-07, -7.5361e-07, 33.0617, -2.50452, -0.457341, -1.27126e-07, 0.0248853, -0.830971, -0.104893, -0.066278, -0.131213, -0.00499986, 0.00616735, 0.0751473, 0.053772, 0.00941355, 0.0945284, -0.169438, 0.825284, 0.0615738, 0.10875, 0.00733727, 0.00976799, -0.375844, -0.90432, 0.308022, -0.048272, -0.00175218, -0.0209692], \n",
+      " [1.64746e-08, -7.01188e-08, -1.76064e-07, 0.952978, 0.123903, -0.277864, -0.363485, 0.237356, -0.35163, 0.0766344, -0.0934139, 0.0230278, 0.029277, 0.0328886, 0.0241443, 0.043741, 0.0167414, -0.0163481, -0.0296333, 0.092105, 0.000355753, 0.000337489, 0.000773349, 0.0516376, 0.30687, -0.0642061, 0.0731844, -0.00842213, -0.00104759], \n",
+      " [-5.05848e-08, 1.67415e-07, 1.94449e-07, 0.123903, 1.57472, 0.152698, 0.119818, 0.332122, 0.0668784, 0.0610566, 0.135262, 0.00470266, -0.0564753, 0.0612002, 0.0399097, 0.0262071, -0.0333171, -0.0706867, 0.367467, 0.0198143, -0.0534455, 0.00682553, 0.00600825, 0.0997407, 0.133818, -0.11156, 0.0115792, -0.00157785, 0.0098313], \n",
+      " [-1.63633e-07, 1.60928e-07, -1.83889e-07, -0.277864, 0.152698, 1.49791, -0.309035, -0.281159, -0.341702, 0.0105446, -0.0600587, -0.0327367, 0.0166003, -0.0240191, 0.068891, -0.0123806, 0.00184954, 0.120606, 0.116309, -0.112637, -0.00766958, -1.36797e-05, 0.0179111, 0.207722, -0.276553, -0.0284691, -0.125894, 0.0165668, -0.0118965]]\n"
+     ]
+    }
+   ],
+   "source": [
+    "Jcm = kinDyn.centroidal_momentum_matrix_fun()\n",
+    "print('Centroidal Momentum Matrix:\\n', SX2DM(Jcm(H_b, s)))"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Center of mass position"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 7,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "Center of Mass position:\n",
+      " [-2.05686, 1.66469, -2.15128]\n"
+     ]
+    }
+   ],
+   "source": [
+    "CoM = kinDyn.CoM_position_fun()\n",
+    "print('Center of Mass position:\\n', SX2DM(CoM(H_b, s)))"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Total Mass"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 8,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "Total mass:\n",
+      " 33.06167270000001\n"
+     ]
+    }
+   ],
+   "source": [
+    "print('Total mass:\\n', kinDyn.get_total_mass())"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Jacobian"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 9,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "Jacobian of the left sole:\n",
+      " \n",
+      "[[1, 0, 0, 0, -0.17172, 0.0113071, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.000306071, -0.176036, -0.000768873, 0.163323, 0.0285059, -0.034992, 0, 0, 0, 0, 0, 0], \n",
+      " [0, 1, 0, 0.17172, 0, -0.33334, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.0287838, -0.074059, 0.158771, -0.00254149, -0.00387031, -0.0498757, 0, 0, 0, 0, 0, 0], \n",
+      " [0, 0, 1, -0.0113071, 0.33334, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.0100419, 0.200923, 0.0594205, 0.0199754, 0.0379968, 0.0211713, 0, 0, 0, 0, 0, 0], \n",
+      " [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.697961, -0.00427738, 0.999529, -0.00339995, 0.00339995, 0.598129, 0, 0, 0, 0, 0, 0], \n",
+      " [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.242503, 0.939492, -0.00639638, -0.995101, 0.995101, -0.0812093, 0, 0, 0, 0, 0, 0], \n",
+      " [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.673827, 0.342543, 0.0300246, -0.098809, 0.098809, 0.797274, 0, 0, 0, 0, 0, 0]]\n"
+     ]
+    }
+   ],
+   "source": [
+    "J = kinDyn.jacobian_fun('l_sole')\n",
+    "print('Jacobian of the left sole:\\n', SX2DM(J(H_b, s)))"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Relative jacobian"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 10,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "Jacobian between the root link and left sole:\n",
+      " \n",
+      "[[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.0236661, 0.169379, 0.134342, -0.0605206, 0.0106206, 0.000224606, 0, 0, 0, 0, 0, 0], \n",
+      " [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4.89995e-15, 0.0304806, 0.00099996, 0.128069, 0.0464378, 0.00193775, 0, 0, 0, 0, 0, 0], \n",
+      " [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.0192186, -0.217313, 0.103397, 0.0837578, 0.00143276, -0.0644705, 0, 0, 0, 0, 0, 0], \n",
+      " [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2.07045e-13, 0.788722, -0.426993, -0.654304, 0.654304, 0.222849, 0, 0, 0, 0, 0, 0], \n",
+      " [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, -1.63523e-13, 0.719415, 0.172361, -0.172361, 0.974389, 0, 0, 0, 0, 0, 0], \n",
+      " [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.61475, 0.547831, -0.736327, 0.736327, 0.030063, 0, 0, 0, 0, 0, 0]]\n"
+     ]
+    }
+   ],
+   "source": [
+    "J_r = kinDyn.relative_jacobian_fun('l_sole')\n",
+    "print('Jacobian between the root link and left sole:\\n', SX2DM(J_r(s)))"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Forward kinematics"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 11,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "Forward kinematics to the left sole:\n",
+      " \n",
+      "[[0.598129, -0.542511, 0.58985, -2.40404], \n",
+      " [-0.0812093, -0.773267, -0.628859, 1.72914], \n",
+      " [0.797274, 0.328238, -0.50657, -2.3824], \n",
+      " [0, 0, 0, 1]]\n"
+     ]
+    }
+   ],
+   "source": [
+    "H = kinDyn.forward_kinematics_fun('l_sole')\n",
+    "print('Forward kinematics to the left sole:\\n', SX2DM(H(H_b, s)))"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3.8.5 64-bit ('base': conda)",
+   "name": "python385jvsc74a57bd01aaa65760776477b200513a4708dfe8112d71af684253b4518b287658553a52c"
+  },
+  "language_info": {
+   "name": "python",
+   "version": ""
+  },
+  "metadata": {
+   "interpreter": {
+    "hash": "1aaa65760776477b200513a4708dfe8112d71af684253b4518b287658553a52c"
+   }
+  },
+  "orig_nbformat": 3
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}
\ No newline at end of file
diff --git a/pyproject.toml b/pyproject.toml
new file mode 100644
index 00000000..4cf57d88
--- /dev/null
+++ b/pyproject.toml
@@ -0,0 +1,13 @@
+[build-system]
+requires = [
+    "wheel",
+    "setuptools>=45",
+    "setuptools_scm[toml]>=6.0",
+]
+
+[tool.black]
+line-length = 88
+
+[tool.isort]
+profile = "black"
+multi_line_output = 3
diff --git a/setup.cfg b/setup.cfg
new file mode 100644
index 00000000..8455b991
--- /dev/null
+++ b/setup.cfg
@@ -0,0 +1,23 @@
+[metadata]
+name = ADAM
+description = Automatic Differentiation for rigid body AlgorithMs
+author = "Giuseppe L'Erario"
+author_email = gl.giuseppelerario@gmail.com
+license_file = LICENSE
+
+[options]
+packages = find:
+python_requires = >=3.7
+install_requires =
+        numpy
+        scipy
+        casadi
+        matplotlib
+        prettytable
+        urdf_parser_py
+
+[options.extras_require]
+test =
+        pytest
+        idyntree
+        gym-ignition-models
diff --git a/setup.py b/setup.py
new file mode 100644
index 00000000..60684932
--- /dev/null
+++ b/setup.py
@@ -0,0 +1,3 @@
+from setuptools import setup
+
+setup()
diff --git a/tests/test_computations.py b/tests/test_computations.py
new file mode 100644
index 00000000..b1e22874
--- /dev/null
+++ b/tests/test_computations.py
@@ -0,0 +1,161 @@
+import logging
+
+import casadi as cs
+import gym_ignition_models
+import idyntree.swig as idyntree
+import numpy as np
+import pytest
+
+from adam.core.computations import KinDynComputations
+from adam.geometry import utils
+
+model_path = gym_ignition_models.get_model_file("iCubGazeboV2_5")
+
+joints_name_list = [
+    "torso_pitch",
+    "torso_roll",
+    "torso_yaw",
+    "l_shoulder_pitch",
+    "l_shoulder_roll",
+    "l_shoulder_yaw",
+    "l_elbow",
+    "r_shoulder_pitch",
+    "r_shoulder_roll",
+    "r_shoulder_yaw",
+    "r_elbow",
+    "l_hip_pitch",
+    "l_hip_roll",
+    "l_hip_yaw",
+    "l_knee",
+    "l_ankle_pitch",
+    "l_ankle_roll",
+    "r_hip_pitch",
+    "r_hip_roll",
+    "r_hip_yaw",
+    "r_knee",
+    "r_ankle_pitch",
+    "r_ankle_roll",
+]
+
+
+def SX2DM(x):
+    return cs.DM(x)
+
+
+def H_from_PosRPY_idyn(xyz, rpy):
+    T = idyntree.Transform.Identity()
+    R = idyntree.Rotation.RPY(rpy[0], rpy[1], rpy[2])
+    p = idyntree.Position()
+    [p.setVal(i, xyz[i]) for i in range(3)]
+    T.setRotation(R)
+    T.setPosition(p)
+    return T
+
+
+logging.basicConfig(level=logging.DEBUG)
+logging.debug("Showing the robot tree.")
+
+root_link = "root_link"
+comp = KinDynComputations(model_path, joints_name_list, root_link)
+robot_iDyn = idyntree.ModelLoader()
+robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list)
+
+kinDyn = idyntree.KinDynComputations()
+kinDyn.loadRobotModel(robot_iDyn.model())
+kinDyn.setFloatingBase(root_link)
+kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION)
+n_dofs = len(joints_name_list)
+
+xyz = (np.random.rand(3) - 0.5) * 5
+rpy = (np.random.rand(3) - 0.5) * 5
+
+H_b_idyn = H_from_PosRPY_idyn(xyz, rpy)
+vb = idyntree.Twist()
+vb.zero()
+s = idyntree.VectorDynSize(n_dofs)
+joints_val = (np.random.rand(n_dofs) - 0.5) * 5
+[s.setVal(i, joints_val[i]) for i in range(n_dofs)]
+s_dot = idyntree.VectorDynSize(n_dofs)
+[s_dot.setVal(i, 0) for i in range(n_dofs)]
+g = idyntree.Vector3()
+g.zero()
+kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g)
+H_b = utils.H_from_PosRPY(xyz, rpy)
+s_ = joints_val
+
+
+def test_mass_matrix():
+    M = comp.mass_matrix_fun()
+    mass_mx = idyntree.MatrixDynSize()
+    kinDyn.getFreeFloatingMassMatrix(mass_mx)
+    mass_mxNumpy = mass_mx.toNumPy()
+    mass_test = SX2DM(M(H_b, s_))
+    assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5)
+
+
+def test_CMM():
+    Jcm = comp.centroidal_momentum_matrix_fun()
+    cmm_idyntree = idyntree.MatrixDynSize()
+    kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree)
+    cmm_idyntreeNumpy = cmm_idyntree.toNumPy()
+    Jcm_test = SX2DM(Jcm(H_b, s_))
+    assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5)
+
+
+def test_CoM_pos():
+    com_f = comp.CoM_position_fun()
+    CoM_cs = SX2DM(com_f(H_b, s_))
+    CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy()
+    assert CoM_cs - CoM_iDynTree == pytest.approx(0.0, abs=1e-5)
+
+
+def test_total_mass():
+    assert comp.get_total_mass() - robot_iDyn.model().getTotalMass() == pytest.approx(
+        0.0, abs=1e-5
+    )
+
+
+def test_jacobian():
+    J_tot = comp.jacobian_fun("l_sole")
+    iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6)
+    kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_)
+    iDynNumpyJ_ = iDyntreeJ_.toNumPy()
+    s_ = joints_val
+    H_b = utils.H_from_PosRPY(xyz, rpy)
+    J_test = SX2DM(J_tot(H_b, s_))
+    assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5)
+
+
+def test_jacobian_non_actuated():
+    J_tot = comp.jacobian_fun("head")
+    iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6)
+    kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_)
+    iDynNumpyJ_ = iDyntreeJ_.toNumPy()
+    s_ = joints_val
+    H_b = utils.H_from_PosRPY(xyz, rpy)
+    J_test = SX2DM(J_tot(H_b, s_))
+    assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5)
+
+
+def test_fk():
+    H_idyntree = kinDyn.getWorldTransform("l_sole")
+    p_idy2np = H_idyntree.getPosition().toNumPy()
+    R_idy2np = H_idyntree.getRotation().toNumPy()
+    T = comp.forward_kinematics_fun("l_sole")
+    s_ = joints_val
+    H_b = utils.H_from_PosRPY(xyz, rpy)
+    H_test = SX2DM(T(H_b, s_))
+    assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
+    assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+
+
+def test_fk_non_actuated():
+    H_idyntree = kinDyn.getWorldTransform("head")
+    p_idy2np = H_idyntree.getPosition().toNumPy()
+    R_idy2np = H_idyntree.getRotation().toNumPy()
+    T = comp.forward_kinematics_fun("head")
+    s_ = joints_val
+    H_b = utils.H_from_PosRPY(xyz, rpy)
+    H_test = SX2DM(T(H_b, s_))
+    assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
+    assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)