diff --git a/src/adam/casadi/computations_parametric.py b/src/adam/casadi/computations_parametric.py
new file mode 100644
index 00000000..515e38a3
--- /dev/null
+++ b/src/adam/casadi/computations_parametric.py
@@ -0,0 +1,179 @@
+# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+import casadi as cs
+import numpy as np
+
+from adam.casadi.casadi_like import SpatialMath
+from adam.core import RBDAlgorithms
+from adam.model import Model, URDFModelFactory, URDFParametricModelFactory
+
+
+class KinDynComputationsParametric:
+    """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.
+    """
+
+    def __init__(
+        self,
+        urdfstring: str,
+        joints_name_list: list,
+        link_name_list:list,  
+        root_link: str = "root_link",
+        gravity: np.array = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]),
+        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'.
+        """
+        math = SpatialMath()
+        n_param_link = len(link_name_list)
+        self.density = cs.SX.sym("density", n_param_link)
+        self.length_multiplier = cs.SX.sym("length_multiplier",n_param_link)
+        factory = URDFParametricModelFactory(path=urdfstring, math=math,link_parametric_list=link_name_list,lenght_multiplier=lenght_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        self.NDoF = self.rbdalgos.NDoF
+        self.g = gravity
+        self.f_opts = f_opts
+
+    def mass_matrix_fun(self) -> cs.Function:
+        """Returns the Mass Matrix functions computed the CRBA
+
+        Returns:
+            M (casADi function): Mass Matrix
+        """
+        T_b = cs.SX.sym("T_b", 4, 4)
+        s = cs.SX.sym("s", self.NDoF)
+        [M, _] = self.rbdalgos.crba(T_b, s)
+        return cs.Function("M", [T_b, s, self.length_multiplier, self.density], [M.array], self.f_opts)
+
+    def centroidal_momentum_matrix_fun(self) -> cs.Function:
+        """Returns the Centroidal Momentum Matrix functions computed the CRBA
+
+        Returns:
+            Jcc (casADi function): Centroidal Momentum matrix
+        """
+        T_b = cs.SX.sym("T_b", 4, 4)
+        s = cs.SX.sym("s", self.NDoF)
+        [_, Jcm] = self.rbdalgos.crba(T_b, s)
+        return cs.Function("Jcm", [T_b, s,self.length_multiplier, self.density], [Jcm.array], self.f_opts)
+
+    def forward_kinematics_fun(self, frame: str) -> cs.Function:
+        """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
+        """
+        s = cs.SX.sym("s", self.NDoF)
+        T_b = cs.SX.sym("T_b", 4, 4)
+        T_fk = self.rbdalgos.forward_kinematics(frame, T_b, s)
+        return cs.Function("T_fk", [T_b, s,self.length_multiplier, self.density], [T_fk.array], self.f_opts)
+
+    def jacobian_fun(self, frame: str) -> cs.Function:
+        """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
+        """
+        s = cs.SX.sym("s", self.NDoF)
+        T_b = cs.SX.sym("T_b", 4, 4)
+        J_tot = self.rbdalgos.jacobian(frame, T_b, s)
+        return cs.Function("J_tot", [T_b, s,self.length_multiplier, self.density], [J_tot.array], self.f_opts)
+
+    def relative_jacobian_fun(self, frame: str) -> cs.Function:
+        """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
+        """
+        s = cs.SX.sym("s", self.NDoF)
+        J = self.rbdalgos.relative_jacobian(frame, s)
+        return cs.Function("J", [s,self.length_multiplier, self.density], [J.array], self.f_opts)
+
+    def CoM_position_fun(self) -> cs.Function:
+        """Returns the CoM positon
+
+        Returns:
+            com (casADi function): The CoM position
+        """
+        s = cs.SX.sym("s", self.NDoF)
+        T_b = cs.SX.sym("T_b", 4, 4)
+        com_pos = self.rbdalgos.CoM_position(T_b, s)
+        return cs.Function("CoM_pos", [T_b, s,self.length_multiplier, self.density], [com_pos.array], self.f_opts)
+
+    def bias_force_fun(self) -> cs.Function:
+        """Returns the bias force of the floating-base dynamics equation,
+        using a reduced RNEA (no acceleration and external forces)
+
+        Returns:
+            h (casADi function): the bias force
+        """
+        T_b = cs.SX.sym("T_b", 4, 4)
+        s = cs.SX.sym("s", self.NDoF)
+        v_b = cs.SX.sym("v_b", 6)
+        s_dot = cs.SX.sym("s_dot", self.NDoF)
+        h = self.rbdalgos.rnea(T_b, s, v_b, s_dot, self.g)
+        return cs.Function("h", [T_b, s, v_b, s_dot,self.length_multiplier, self.density], [h.array], self.f_opts)
+
+    def coriolis_term_fun(self) -> cs.Function:
+        """Returns the coriolis term of the floating-base dynamics equation,
+        using a reduced RNEA (no acceleration and external forces)
+
+        Returns:
+            C (casADi function): the Coriolis term
+        """
+        T_b = cs.SX.sym("T_b", 4, 4)
+        q = cs.SX.sym("q", self.NDoF)
+        v_b = cs.SX.sym("v_b", 6)
+        q_dot = cs.SX.sym("q_dot", self.NDoF)
+        # set in the bias force computation the gravity term to zero
+        C = self.rbdalgos.rnea(T_b, q, v_b, q_dot, np.zeros(6))
+        return cs.Function("C", [T_b, q, v_b, q_dot,self.length_multiplier, self.density], [C.array], self.f_opts)
+
+    def gravity_term_fun(self) -> cs.Function:
+        """Returns the gravity term of the floating-base dynamics equation,
+        using a reduced RNEA (no acceleration and external forces)
+
+        Returns:
+            G (casADi function): the gravity term
+        """
+        T_b = cs.SX.sym("T_b", 4, 4)
+        q = cs.SX.sym("q", self.NDoF)
+        # set in the bias force computation the velocity to zero
+        G = self.rbdalgos.rnea(T_b, q, np.zeros(6), np.zeros(self.NDoF), self.g)
+        return cs.Function("G", [T_b, q,self.length_multiplier, self.density], [G.array], self.f_opts)
+
+    def forward_kinematics(self, frame, T_b, s) -> cs.Function:
+        """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
+        """
+
+        return self.rbdalgos.forward_kinematics(frame, T_b, s,self.length_multiplier, self.density)
+
+    #TODO 
+    def get_total_mass(self) -> float:
+        """Returns the total mass of the robot
+
+        Returns:
+            mass: The total mass
+        """
+        return self.rbdalgos.get_total_mass()
diff --git a/src/adam/model/parametric_factories/__init__.py b/src/adam/model/parametric_factories/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/src/adam/model/parametric_factories/parametric_joint.py b/src/adam/model/parametric_factories/parametric_joint.py
new file mode 100644
index 00000000..4c086cbe
--- /dev/null
+++ b/src/adam/model/parametric_factories/parametric_joint.py
@@ -0,0 +1,129 @@
+from typing import Union
+
+import numpy.typing as npt
+import urdf_parser_py.urdf
+
+from adam.core.spatial_math import SpatialMath
+from adam.model import Joint
+from adam.model.parametric_factories import link_parametric
+
+class ParmetricJoint(Joint):
+    """Parametric Joint class"""
+
+    def __init__(
+        self,
+        joint: urdf_parser_py.urdf.Joint,
+        math: SpatialMath,
+        parent_link:link_parametric,
+        idx: Union[int, None] = None,
+    ) -> None:
+        self.math = math
+        self.name = joint.name
+        self.parent = jparent_link
+        self.child = joint.child
+        self.type = joint.joint_type
+        self.axis = joint.axis
+        self.limit = joint.limit
+        self.idx = idx
+
+        joint_offset = self.parent_link.compute_joint_offset(joint, self.parent_link.offset)
+        self.offset = joint_offset
+        self.origin = self.modify(self.parent_link.offset)
+
+    def modify(self, parent_joint_offset):
+        length = self.parent_link.get_principal_lenght_parametric()
+        # Ack for avoiding depending on casadi 
+        vo = self.parent_link.origin[2]
+        xyz_rpy = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        xyz_rpy[0] = self.joint.origin[0,3]
+        xyz_rpy[1] = self.joint.origin[1,3]
+        xyz_rpy[2] = self.joint.origin[2,3]
+        xyz_rpy_temp=  matrix_to_xyz_rpy(self.joint.origin)
+        xyz_rpy[3] = xyz_rpy_temp[3]
+        xyz_rpy[4] = xyz_rpy_temp[4]
+        xyz_rpy[5] = xyz_rpy_temp[5]
+        
+        if(xyz_rpy[2]<0): 
+            xyz_rpy[2] = -length +parent_joint_offset - self.offset   
+        else:
+            xyz_rpy[2] = vo+ length/2 - self.offset
+        return xyz_rpy
+
+    def homogeneous(self, q: npt.ArrayLike) -> npt.ArrayLike:
+        """
+        Args:
+            q (npt.ArrayLike): joint value
+
+        Returns:
+            npt.ArrayLike: the homogenous transform of a joint, given q
+        """
+
+        if self.type == "fixed":
+            xyz = self.origin.xyz
+            rpy = self.origin.rpy
+            return self.math.H_from_Pos_RPY(xyz, rpy)
+        elif self.type in ["revolute", "continuous"]:
+            return self.math.H_revolute_joint(
+                self.origin.xyz,
+                self.origin.rpy,
+                self.axis,
+                q,
+            )
+        elif self.type in ["prismatic"]:
+            return self.math.H_prismatic_joint(
+                self.origin.xyz,
+                self.origin.rpy,
+                self.axis,
+                q,
+            )
+
+    def spatial_transform(self, q: npt.ArrayLike) -> npt.ArrayLike:
+        """
+        Args:
+            q (npt.ArrayLike): joint motion
+
+        Returns:
+            npt.ArrayLike: spatial transform of the joint given q
+        """
+        if self.type == "fixed":
+            return self.math.X_fixed_joint(self.origin[:3], self.origin[3:])
+        elif self.type in ["revolute", "continuous"]:
+            return self.math.X_revolute_joint(
+                self.origin[:3], self.origin[3:], self.axis, q
+            )
+        elif self.type in ["prismatic"]:
+            return self.math.X_prismatic_joint(
+                self.origin[:3],
+                self.origin[3:],
+                self.axis,
+                q,
+            )
+
+    def motion_subspace(self) -> npt.ArrayLike:
+        """
+        Args:
+            joint (Joint): Joint
+
+        Returns:
+            npt.ArrayLike: motion subspace of the joint
+        """
+        if self.type == "fixed":
+            return self.math.vertcat(0, 0, 0, 0, 0, 0)
+        elif self.type in ["revolute", "continuous"]:
+            return self.math.vertcat(
+                0,
+                0,
+                0,
+                self.axis[0],
+                self.axis[1],
+                self.axis[2],
+            )
+        elif self.type in ["prismatic"]:
+            return self.math.vertcat(
+                self.axis[0],
+                self.axis[1],
+                self.axis[2],
+                0,
+                0,
+                0,
+            )
diff --git a/src/adam/model/parametric_factories/parametric_link.py b/src/adam/model/parametric_factories/parametric_link.py
new file mode 100644
index 00000000..ab4564fa
--- /dev/null
+++ b/src/adam/model/parametric_factories/parametric_link.py
@@ -0,0 +1,229 @@
+import numpy.typing as npt
+import urdf_parser_py.urdf
+from enum import Enum
+
+from adam.core.spatial_math import SpatialMath
+from adam.model import Link
+
+class I_parametric():
+    def __init__(self) -> None:
+        self.ixx = 0.0
+        self.ixy = 0.0
+        self.ixz = 0.0
+        self.iyy = 0.0
+        self.iyz = 0.0
+        self.izz = 0.0
+
+class Geometry(Enum):
+    """The different types of geometries that constitute the URDF"""
+    BOX = 1
+    CYLINDER = 2
+    SPHERE = 3
+
+class Side(Enum):
+    """The possible sides of a box geometry"""
+    WIDTH = 1
+    HEIGHT = 2
+    DEPTH = 3
+
+
+class ParametricLink(Link):
+    """Parametric Link class"""
+
+    def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath, length_multiplier, density):
+        self.math = math
+        self.name = link.name
+        self.length_multiplier = length_multiplier
+        self.density = density
+        self.visuals = link.visuals
+        self.geometry_type, self.visual_data = self.get_geometry(self.visuals)
+        self.link = link
+        self.link_offset = self.compute_offset()        
+        (self.volume, self.visual_data_new) = self.compute_volume()
+        self.mass = self.compute_mass()
+        self.I = self.compute_inertia_parametric()
+        self.origin = self.modify_origin()
+      
+        # if the link has inertial properties, but the origin is None, let's add it
+        if link.inertial is not None and link.inertial.origin is None:
+            link.inertial.origin.xyz = [0, 0, 0]
+            link.inertial.origin.rpy = [0, 0, 0]
+
+    def get_principal_lenght(self): 
+        visual = self.get_visual()
+        xyz_rpy = matrix_to_xyz_rpy(visual.origin) 
+        if self.geometry_type == Geometry.CYLINDER:
+            if(xyz_rpy[3] < 0.0 or xyz_rpy[4] > 0.0):
+                v_l = 2*self.visual_data.radius # returning the diameter, since the orientation of the shape is such that the radius is the principal lenght 
+            else: 
+                v_l=self.visual_data.length # returning the lenght, since the orientation of the shape is such that the radius is the principal lenght 
+        elif(self.geometry_type == Geometry.SPHERE): 
+            v_l = self.visual_data.radius
+        elif(self.geometry_type == Geometry.BOX): 
+            v_l= self.visual_data.size[2]
+        else:
+            raise Exception(f"THE GEOMETRY IS NOT SPECIFIED")
+        return v_l 
+
+    def get_principal_lenght_parametric(self): 
+        visual = self.get_visual()
+        xyz_rpy = matrix_to_xyz_rpy(visual.origin) 
+        if self.geometry_type == Geometry.CYLINDER:
+            if(xyz_rpy[3] < 0.0 or xyz_rpy[4] > 0.0):
+                v_l = 2*self.visual_data_new[1] # returning the diameter, since the orientation of the shape is such that the radius is the principal lenght 
+            else: 
+                v_l=self.visual_data_new[0] # returning the lenght, since the orientation of the shape is such that the radius is the principal lenght 
+        elif(self.geometry_type == Geometry.SPHERE): 
+            v_l = self.visual_data_new
+        elif(self.geometry_type == Geometry.BOX): 
+            v_l= self.visual_data_new[2]
+        else:
+            raise Exception(f"THE GEOMETRY IS NOT SPECIFIED")
+        return v_l 
+
+    def compute_offset(self): 
+        visual = self.get_visual()
+        xyz_rpy = matrix_to_xyz_rpy(visual.origin) 
+        v_l=  self.get_principal_lenght()
+        v_o = xyz_rpy[2]
+        if(v_o<0):
+            link_offset = v_l/2 + v_o
+        else:
+            link_offset = (v_o - v_l/2)
+        return link_offset
+ 
+    def compute_joint_offset(self,joint_i, parent_offset): 
+         # Taking the principal direction i.e. the length 
+        visual = self.get_visual()
+        xyz_rpy = matrix_to_xyz_rpy(visual.origin) 
+        v_l= self.get_principal_lenght()
+        j_0 = matrix_to_xyz_rpy(joint_i.origin)[2]
+        v_o = xyz_rpy[2]
+        if(j_0<0):
+            joint_offset_temp = -(v_l + j_0-parent_offset)
+            joint_offset = joint_offset_temp
+        else:
+            joint_offset_temp = v_l + parent_offset - j_0
+            joint_offset = joint_offset_temp
+        return joint_offset
+
+    @staticmethod
+    def get_geometry(visual_obj):
+        """Returns the geometry type and the corresponding geometry object for a given visual"""
+        if visual_obj.geometry.box is not None:
+            return [Geometry.BOX, visual_obj.geometry.box]
+        if visual_obj.geometry.cylinder is not None:
+            return [Geometry.CYLINDER, visual_obj.geometry.cylinder]
+        if visual_obj.geometry.sphere is not None:
+            return [Geometry.SPHERE, visual_obj.geometry.sphere]
+
+    """Function that starting from a multiplier and link visual characteristics computes the link volume"""
+    def compute_volume(self):
+        volume = 0.0
+        """Modifies a link's volume by a given multiplier, in a manner that is logical with the link's geometry"""
+        if self.geometry_type == Geometry.BOX:
+            visual_data_new =[0.0, 0.0, 0.0]
+            visual_data_new[0] = self.visual_data.size[0] * self.length_multiplier[0]
+            visual_data_new[1] = self.visual_data.size[1] * self.length_multiplier[1]
+            visual_data_new[2] = self.visual_data.size[2] * self.length_multiplier[2]
+            volume = visual_data_new[0] * visual_data_new[1] * visual_data_new[2]
+        elif self.geometry_type == Geometry.CYLINDER:
+            visual_data_new = [0.0, 0.0]
+            visual_data_new[0] = self.visual_data.length * self.length_multiplier[0]
+            visual_data_new[1] = self.visual_data.radius * self.length_multiplier[1]
+            volume = math.pi * visual_data_new[1] ** 2 * visual_data_new[0]
+        elif self.geometry_type == Geometry.SPHERE:
+            visual_data_new = 0.0
+            visual_data_new = self.visual_data.radius * self.length_multiplier[0]
+            volume = 4 * math.pi * visual_data_new ** 3 / 3
+        return volume, visual_data_new
+
+    """Function that computes the mass starting from the density, the length multiplier and the link"""
+    def compute_mass(self):
+        """Changes the mass of a link by preserving a given density."""
+        mass = 0.0
+        mass = self.volume * self.density
+        return mass
+
+    def modify_origin(self):
+        origin = [0.0,0.0,0.0,0.0,0.0,0.0]
+        visual = self.get_visual()
+        """Modifies the position of the origin by a given amount"""
+        xyz_rpy = matrix_to_xyz_rpy(visual.origin)
+        v_o = xyz_rpy[2] 
+        length = self.get_principal_lenght_parametric()
+        if(v_o<0):
+            origin[2] = self.offset-length/2
+            origin[0] = xyz_rpy[0]
+            origin[1] = xyz_rpy[1]
+            origin[3] = xyz_rpy[3]
+            origin[4] = xyz_rpy[4]
+            origin[5] = xyz_rpy[5]
+        else:
+            origin[2] = length/2 +self.offset
+            origin[0] = xyz_rpy[0]
+            origin[1] = xyz_rpy[1]
+            origin[3] = xyz_rpy[3]
+            origin[4] = xyz_rpy[4]
+            origin[5] = xyz_rpy[5]
+        if self.geometry_type == Geometry.SPHERE:
+            "in case of a sphere the origin of the framjoint_name_list[0]:link_parametric.JointCharacteristics(0.0295),e does not change"
+            origin = xyz_rpy 
+        return origin
+
+    def compute_inertia_parametric(self):
+        I = I_parametric
+        visual = self.get_visual()
+        xyz_rpy = matrix_to_xyz_rpy(visual.origin) 
+        """Calculates inertia (ixx, iyy and izz) with the formula that corresponds to the geometry
+        Formulas retrieved from https://en.wikipedia.org/wiki/List_of_moments_of_inertia"""
+        if self.geometry_type == Geometry.BOX:
+            I.ixx = self.mass * (self.visual_data_new[1] ** 2+ self.visual_data_new[2] ** 2)/12
+            I.iyy = self.mass* (self.visual_data_new[0]**2 + self.visual_data_new[2]**2)/12
+            I.izz = self.mass * (self.visual_data_new[0]**2 + self.visual_data_new[1]**2)/12
+        elif self.geometry_type == Geometry.CYLINDER:
+            i_xy_incomplete = (
+                3 *(self.visual_data_new[1] ** 2) + self.visual_data_new[0] ** 2
+            ) / 12
+            I.ixx = self.mass * i_xy_incomplete
+            I.iyy = self.mass * i_xy_incomplete
+            I.izz = self.mass * self.visual_data_new[1] ** 2 / 2
+
+            if(xyz_rpy[3]>0 and xyz_rpy[4] == 0.0 and xyz_rpy[5] == 0.0):
+                itemp = I.izz
+                I.iyy = itemp
+                I.izz = I.ixx
+            elif(xyz_rpy[4]>0.0):
+                itemp = I.izz
+                I.ixx = itemp
+                I.izz = I.iyy
+            return I
+        elif self.geometry_type == Geometry.SPHERE:
+            I.ixx = 2 * self.mass * self.visual_data_new** 2 / 5
+            I.iyy = I.ixx
+            I.izz = I.ixx
+        return I
+
+    def spatial_inertia(self) -> npt.ArrayLike:
+        """
+        Args:
+            link (Link): Link
+
+        Returns:
+            npt.ArrayLike: the 6x6 inertia matrix expressed at the origin of the link (with rotation)
+        """
+        I = self.I
+        mass = self.mass
+        o = self.origin[:3]
+        rpy = self.origin[3:]
+        return self.math.spatial_inertia(I, mass, o, rpy)
+
+    def homogeneous(self) -> npt.ArrayLike:
+        """
+        Returns:
+            npt.ArrayLike: the homogeneus transform of the link
+        """
+        return self.math.H_from_Pos_RPY(
+            self.inertial.origin.xyz,
+            self.inertial.origin.rpy,
+        )
diff --git a/src/adam/model/parametric_factories/parametric_model.py b/src/adam/model/parametric_factories/parametric_model.py
new file mode 100644
index 00000000..96556042
--- /dev/null
+++ b/src/adam/model/parametric_factories/parametric_model.py
@@ -0,0 +1,87 @@
+import pathlib
+from typing import List
+
+import urdf_parser_py.urdf
+
+from adam.core.spatial_math import SpatialMath
+from adam.model import ModelFactory, StdJoint, StdLink, Link, Joint, LinkParametric, JointParametric
+
+
+class URDFParametricModelFactory(ModelFactory):
+    """This factory generates robot elements from urdf_parser_py
+
+    Args:
+        ModelFactory: the Model factory
+    """
+
+    def __init__(self, path: str, math: SpatialMath, link_parametric_list:List, lenght_multiplier, density):
+        self.math = math
+        if type(path) is not pathlib.Path:
+            path = pathlib.Path(path)
+        if not path.exists():
+            raise FileExistsError(path)
+        self.link_parametric_list = list
+        self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_file(path)
+        self.name = self.urdf_desc.name
+        self.lenght_multiplier = lenght_multiplier
+        self.density  = density
+
+    def get_joints(self) -> List[Joint]:
+        """
+        Returns:
+            List[StdJoint]: build the list of the joints
+        """
+        return [self.build_joint(j) for j in self.urdf_desc.joints]
+
+    def get_links(self) -> List[Link]:
+        """
+        Returns:
+            List[StdLink]: build the list of the links
+        """
+        return [
+            self.build_link(l) for l in self.urdf_desc.links if l.inertial is not None
+        ]
+
+    def get_frames(self) -> List[StdLink]:
+        """
+        Returns:
+            List[StdLink]: build the list of the links
+        """
+        return [self.build_link(l) for l in self.urdf_desc.links if l.inertial is None]
+
+    def build_joint(self, joint: urdf_parser_py.urdf.Joint) -> Joint:
+        """
+        Args:
+            joint (Joint): the urdf_parser_py joint
+
+        Returns:
+            StdJoint: our joint representation
+        """
+        if(joint.parent in self.link_parametric_list): 
+            index_link = self.link_parametric_list.index(joint.parent)
+            link_parent = self.get_element_by_name(joint.parent)
+            parent_link_parametric = LinkParametric(link_parent, self.math,self.lenght_multiplier[index_link], self.density[index_link]) 
+            return JointParametric(joint, self.math, parent_link_parametric)
+
+        return StdJoint(joint, self.math)
+
+    def build_link(self, link: urdf_parser_py.urdf.Link) -> Link:
+        """
+        Args:
+            link (Link): the urdf_parser_py link
+
+        Returns:
+            Link: our link representation
+        """
+        if(link.name in self.link_parametric_list):
+            index_link = self.link_parametric_list.index(link.name) 
+            return LinkParametric(link, self.math, self.lenght_multiplier[index_link], self.density[index_link])
+        return StdLink(link, self.math)
+    
+    def get_element_by_name(self,link_name):
+        """Explores the robot looking for the link whose name matches the first argument"""
+        link_list = [corresponding_link for corresponding_link in self.urdf_desc.links if corresponding_link.name == link_name]
+        if len(link_list) != 0:
+            return link_list[0]
+        else:
+            return None
\ No newline at end of file