From 294d4c470113a020d9584910294fb23de9991a63 Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Wed, 11 Oct 2023 12:25:12 +0200
Subject: [PATCH 01/19] added model parametric and first version of casadi
 computation parametric

---
 src/adam/casadi/computations_parametric.py    | 179 ++++++++++++++
 .../model/parametric_factories/__init__.py    |   0
 .../parametric_factories/parametric_joint.py  | 129 ++++++++++
 .../parametric_factories/parametric_link.py   | 229 ++++++++++++++++++
 .../parametric_factories/parametric_model.py  |  87 +++++++
 5 files changed, 624 insertions(+)
 create mode 100644 src/adam/casadi/computations_parametric.py
 create mode 100644 src/adam/model/parametric_factories/__init__.py
 create mode 100644 src/adam/model/parametric_factories/parametric_joint.py
 create mode 100644 src/adam/model/parametric_factories/parametric_link.py
 create mode 100644 src/adam/model/parametric_factories/parametric_model.py

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

From 0686bed9755f5d7fd9bc0e97c237f332d0b2474f Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Thu, 19 Oct 2023 10:53:33 +0200
Subject: [PATCH 02/19] fix import error, fix missing methods

---
 src/adam/casadi/__init__.py                   |   2 +
 src/adam/casadi/computations_parametric.py    |   2 +-
 src/adam/model/__init__.py                    |   3 +
 .../parametric_factories/parametric_joint.py  |  29 ++-
 .../parametric_factories/parametric_link.py   |  67 +++---
 .../parametric_factories/parametric_model.py  |  11 +-
 tests/test_CasADi_computations_parametric.py  | 194 ++++++++++++++++++
 7 files changed, 256 insertions(+), 52 deletions(-)
 create mode 100644 tests/test_CasADi_computations_parametric.py

diff --git a/src/adam/casadi/__init__.py b/src/adam/casadi/__init__.py
index 1c3b0795..ee587263 100644
--- a/src/adam/casadi/__init__.py
+++ b/src/adam/casadi/__init__.py
@@ -4,3 +4,5 @@
 
 from .casadi_like import CasadiLike
 from .computations import KinDynComputations
+from .computations_parametric import KinDynComputationsParametric
+
diff --git a/src/adam/casadi/computations_parametric.py b/src/adam/casadi/computations_parametric.py
index 515e38a3..c94bb4e8 100644
--- a/src/adam/casadi/computations_parametric.py
+++ b/src/adam/casadi/computations_parametric.py
@@ -34,7 +34,7 @@ def __init__(
         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)
+        factory = URDFParametricModelFactory(path=urdfstring, math=math,link_parametric_list=link_name_list,lenght_multiplier=self.length_multiplier, density=self.density)
         model = Model.build(factory=factory, joints_name_list=joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=math)
         self.NDoF = self.rbdalgos.NDoF
diff --git a/src/adam/model/__init__.py b/src/adam/model/__init__.py
index 0dd4bf9e..2fb7d510 100644
--- a/src/adam/model/__init__.py
+++ b/src/adam/model/__init__.py
@@ -3,4 +3,7 @@
 from .std_factories.std_joint import StdJoint
 from .std_factories.std_link import StdLink
 from .std_factories.std_model import URDFModelFactory
+from .parametric_factories.parametric_joint import ParmetricJoint
+from .parametric_factories.parametric_link import ParametricLink
+from .parametric_factories.parametric_model import URDFParametricModelFactory
 from .tree import Node, Tree
diff --git a/src/adam/model/parametric_factories/parametric_joint.py b/src/adam/model/parametric_factories/parametric_joint.py
index 4c086cbe..bdefd2f0 100644
--- a/src/adam/model/parametric_factories/parametric_joint.py
+++ b/src/adam/model/parametric_factories/parametric_joint.py
@@ -5,7 +5,7 @@
 
 from adam.core.spatial_math import SpatialMath
 from adam.model import Joint
-from adam.model.parametric_factories import link_parametric
+from adam.model.parametric_factories.parametric_link import ParametricLink
 
 class ParmetricJoint(Joint):
     """Parametric Joint class"""
@@ -14,34 +14,33 @@ def __init__(
         self,
         joint: urdf_parser_py.urdf.Joint,
         math: SpatialMath,
-        parent_link:link_parametric,
+        parent_link:ParametricLink,
         idx: Union[int, None] = None,
     ) -> None:
         self.math = math
         self.name = joint.name
-        self.parent = jparent_link
+        self.parent = parent_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.joint = joint
+        joint_offset = self.parent.compute_joint_offset(joint, self.parent.link_offset)
         self.offset = joint_offset
-        self.origin = self.modify(self.parent_link.offset)
+        self.origin = self.modify(self.parent.link_offset)
 
     def modify(self, parent_joint_offset):
-        length = self.parent_link.get_principal_lenght_parametric()
+        length = self.parent.get_principal_lenght_parametric()
         # Ack for avoiding depending on casadi 
-        vo = self.parent_link.origin[2]
+        vo = self.parent.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]
+        xyz_rpy[0] = self.joint.origin.xyz[0]
+        xyz_rpy[1] = self.joint.origin.xyz[1]
+        xyz_rpy[2] = self.joint.origin.xyz[2]
+        xyz_rpy[3] = self.joint.origin.rpy[0]
+        xyz_rpy[4] = self.joint.origin.rpy[1]
+        xyz_rpy[5] = self.joint.origin.rpy[2]
         
         if(xyz_rpy[2]<0): 
             xyz_rpy[2] = -length +parent_joint_offset - self.offset   
diff --git a/src/adam/model/parametric_factories/parametric_link.py b/src/adam/model/parametric_factories/parametric_link.py
index ab4564fa..fcb70afb 100644
--- a/src/adam/model/parametric_factories/parametric_link.py
+++ b/src/adam/model/parametric_factories/parametric_link.py
@@ -5,6 +5,8 @@
 from adam.core.spatial_math import SpatialMath
 from adam.model import Link
 
+import math
+
 class I_parametric():
     def __init__(self) -> None:
         self.ixx = 0.0
@@ -25,6 +27,7 @@ class Side(Enum):
     WIDTH = 1
     HEIGHT = 2
     DEPTH = 3
+# ['XML_REFL', '_Link__get_collision', '_Link__get_visual', '_Link__set_collision', '_Link__set_visual', '__class__', '__delattr__', '__dict__', '__dir__', '__doc__', '__eq__', '__format__', '__ge__', '__getattribute__', '__gt__', '__hash__', '__init__', '__init_subclass__', '__le__', '__lt__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', 'add_aggregate', 'add_aggregates_to_xml', 'aggregate_init', 'aggregate_order', 'aggregate_type', 'check_valid', 'collision', 'collisions', 'from_xml', 'from_xml_file', 'from_xml_string', 'get_aggregate_list', 'get_refl_vars', 'inertial', 'lump_aggregates', 'name', 'origin', 'parse', 'post_read_xml', 'pre_write_xml', 'read_xml', 'remove_aggregate', 'to_xml', 'to_xml_string', 'to_yaml', 'visual', 'visuals', 'write_xml']
 
 
 class ParametricLink(Link):
@@ -35,7 +38,8 @@ def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath, length_mul
         self.name = link.name
         self.length_multiplier = length_multiplier
         self.density = density
-        self.visuals = link.visuals
+        self.visuals = link.visual
+        print(self.get_geometry(self.visuals))
         self.geometry_type, self.visual_data = self.get_geometry(self.visuals)
         self.link = link
         self.link_offset = self.compute_offset()        
@@ -49,9 +53,8 @@ def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath, length_mul
             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) 
+    def get_principal_lenght(self):  
+        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
         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 
@@ -66,8 +69,8 @@ def get_principal_lenght(self):
         return v_l 
 
     def get_principal_lenght_parametric(self): 
-        visual = self.get_visual()
-        xyz_rpy = matrix_to_xyz_rpy(visual.origin) 
+        
+        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
         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 
@@ -82,8 +85,7 @@ def get_principal_lenght_parametric(self):
         return v_l 
 
     def compute_offset(self): 
-        visual = self.get_visual()
-        xyz_rpy = matrix_to_xyz_rpy(visual.origin) 
+        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy] 
         v_l=  self.get_principal_lenght()
         v_o = xyz_rpy[2]
         if(v_o<0):
@@ -94,10 +96,10 @@ def compute_offset(self):
  
     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) 
+        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy]  
         v_l= self.get_principal_lenght()
-        j_0 = matrix_to_xyz_rpy(joint_i.origin)[2]
+        print(joint_i.origin.xyz)
+        j_0 = joint_i.origin.xyz[2]
         v_o = xyz_rpy[2]
         if(j_0<0):
             joint_offset_temp = -(v_l + j_0-parent_offset)
@@ -109,13 +111,19 @@ def compute_joint_offset(self,joint_i, parent_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]
+        print("visual obj",dir(visual_obj))
+        print(type(visual_obj.geometry))
+        print(visual_obj.name)
+        """Returns the geometry type and thez corresponding geometry object for a given visual"""
+        if type(visual_obj.geometry) is urdf_parser_py.urdf.Box:
+            print("IS A BOX")
+            return (Geometry.BOX, visual_obj.geometry)
+        if type(visual_obj.geometry) is urdf_parser_py.urdf.Cylinder:
+            print("IS A CYLINDER")
+            return (Geometry.CYLINDER, visual_obj.geometry)
+        if type(visual_obj.geometry) is urdf_parser_py.urdf.Sphere:
+            print("IS A SPHERE")
+            return (Geometry.SPHERE, visual_obj.geometry)
 
     """Function that starting from a multiplier and link visual characteristics computes the link volume"""
     def compute_volume(self):
@@ -123,18 +131,18 @@ def compute_volume(self):
         """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]
+            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
             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]
+            visual_data_new[0] = self.visual_data.length * self.length_multiplier
+            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]
+            visual_data_new = self.visual_data.radius * self.length_multiplier
             volume = 4 * math.pi * visual_data_new ** 3 / 3
         return volume, visual_data_new
 
@@ -147,20 +155,18 @@ def compute_mass(self):
 
     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)
+        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy] 
         v_o = xyz_rpy[2] 
         length = self.get_principal_lenght_parametric()
         if(v_o<0):
-            origin[2] = self.offset-length/2
+            origin[2] = self.link_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[2] = length/2 +self.link_offset
             origin[0] = xyz_rpy[0]
             origin[1] = xyz_rpy[1]
             origin[3] = xyz_rpy[3]
@@ -173,8 +179,7 @@ def modify_origin(self):
 
     def compute_inertia_parametric(self):
         I = I_parametric
-        visual = self.get_visual()
-        xyz_rpy = matrix_to_xyz_rpy(visual.origin) 
+        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy] 
         """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:
diff --git a/src/adam/model/parametric_factories/parametric_model.py b/src/adam/model/parametric_factories/parametric_model.py
index 96556042..e5cf0342 100644
--- a/src/adam/model/parametric_factories/parametric_model.py
+++ b/src/adam/model/parametric_factories/parametric_model.py
@@ -4,7 +4,8 @@
 import urdf_parser_py.urdf
 
 from adam.core.spatial_math import SpatialMath
-from adam.model import ModelFactory, StdJoint, StdLink, Link, Joint, LinkParametric, JointParametric
+from adam.model import ModelFactory, StdJoint, StdLink, Link, Joint
+from adam.model import ParmetricJoint, ParametricLink
 
 
 class URDFParametricModelFactory(ModelFactory):
@@ -20,7 +21,7 @@ def __init__(self, path: str, math: SpatialMath, link_parametric_list:List, leng
             path = pathlib.Path(path)
         if not path.exists():
             raise FileExistsError(path)
-        self.link_parametric_list = list
+        self.link_parametric_list = link_parametric_list
         self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_file(path)
         self.name = self.urdf_desc.name
         self.lenght_multiplier = lenght_multiplier
@@ -60,8 +61,8 @@ def build_joint(self, joint: urdf_parser_py.urdf.Joint) -> Joint:
         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)
+            parent_link_parametric = ParametricLink(link_parent, self.math,self.lenght_multiplier[index_link], self.density[index_link]) 
+            return ParmetricJoint(joint, self.math, parent_link_parametric)
 
         return StdJoint(joint, self.math)
 
@@ -75,7 +76,7 @@ def build_link(self, link: urdf_parser_py.urdf.Link) -> Link:
         """
         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 ParametricLink(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):
diff --git a/tests/test_CasADi_computations_parametric.py b/tests/test_CasADi_computations_parametric.py
new file mode 100644
index 00000000..6740d082
--- /dev/null
+++ b/tests/test_CasADi_computations_parametric.py
@@ -0,0 +1,194 @@
+# 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 logging
+from os import link
+
+import casadi as cs
+import numpy as np
+# import pytest
+import math 
+from adam.casadi.computations_parametric import KinDynComputationsParametric
+from adam.casadi import KinDynComputations
+
+from adam.geometry import utils
+
+model_path ="/home/carlotta/iit_ws/element_hardware-intelligence/Software/OptimizationControlAndHardware/models/model.urdf"
+
+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 ComputeOriginalDensity(kinDyn, link_name): 
+    link_original = kinDyn.rbdalgos.model.get_element_by_name(link_name, kinDyn.robot_desc)
+    mass = link_original.inertial.mass
+    volume = 0 
+    visual_obj = link_original.visuals[0]
+    if visual_obj.geometry.box is not None:
+        width = link_original.visuals[0].geometry.box.size[0]
+        depth = link_original.visuals[0].geometry.box.size[2]
+        height = link_original.visuals[0].geometry.box.size[1]
+        volume = width*depth*height
+    if visual_obj.geometry.cylinder is not None:
+        length = link_original.visuals[0].geometry.cylinder.length 
+        radius = link_original.visuals[0].geometry.cylinder.radius
+        volume = math.pi*radius**2*length
+    if visual_obj.geometry.sphere is not None:
+        radius = link_original.visuals[0].geometry.sphere.radius
+        volume = 4*(math.pi*radius**3)/3
+    return mass/volume
+
+def SX2DM(x):
+    return cs.DM(x)
+
+logging.basicConfig(level=logging.DEBUG)
+logging.debug("Showing the robot tree.")
+
+
+root_link = "root_link"
+comp = KinDynComputations(model_path, joints_name_list, root_link)
+
+link_name_list = ['r_upper_leg','r_lower_leg', 'r_hip_1', 'r_hip_2', 'r_ankle_1', 'r_ankle_2','l_upper_leg','l_lower_leg', 'l_hip_1', 'l_hip_2', 'l_ankle_1', 'l_ankle_2', 'l_shoulder_1', 'l_shoulder_2', 'l_shoulder_3', 'l_elbow_1','r_shoulder_1', 'r_shoulder_2', 'r_shoulder_3', 'r_elbow_1']
+comp_w_hardware = KinDynComputationsParametric(model_path, joints_name_list, link_name_list, root_link)
+original_density = []
+for item in link_name_list: 
+    original_density += [ComputeOriginalDensity(comp_w_hardware,item)]
+
+original_length = np.ones([len(link_name_list),3])
+
+joint_type = np.zeros(len(joints_name_list))
+
+n_dofs = len(joints_name_list)
+# base pose quantities
+xyz = (np.random.rand(3) - 0.5) * 5
+rpy = (np.random.rand(3) - 0.5) * 5
+base_vel = (np.random.rand(6) - 0.5) * 5
+# joints quantitites
+joints_val = (np.random.rand(n_dofs) - 0.5) * 5
+joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5
+
+H_b = utils.H_from_Pos_RPY(xyz, rpy)
+vb_ = base_vel
+s_ = joints_val
+s_dot_ = joints_dot_val
+
+# # def test_mass_matrix():
+M = comp.mass_matrix_fun()
+M_with_hardware = comp_w_hardware.mass_matrix_fun()
+mass_test = SX2DM(M(H_b, s_))
+mass_test_hardware = SX2DM(M_with_hardware(H_b,s_, original_density, original_length))
+print("M=",cs.sumsqr(mass_test - mass_test_hardware))
+    # assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+
+# def test_CMM():
+Jcm = comp.centroidal_momentum_matrix_fun()
+Jcm_with_hardware = comp_w_hardware.centroidal_momentum_matrix_fun()
+Jcm_test = SX2DM(Jcm(H_b, s_))
+Jcm_test_hardware = SX2DM(Jcm_with_hardware(H_b,s_, original_density, original_length))
+print("Jcmm=",cs.sumsqr(Jcm_test - Jcm_test_hardware))
+# assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+
+# def test_CoM_pos():
+com_f = comp.CoM_position_fun()
+com_with_hardware_f = comp_w_hardware.CoM_position_fun()
+CoM_cs = SX2DM(com_f(H_b, s_))
+CoM_hardware  = SX2DM(com_with_hardware_f(H_b,s_, original_density, original_length)) 
+print("CoM=",cs.sumsqr(CoM_cs - CoM_hardware))
+    # assert CoM_cs - CoM_hardware == pytest.approx(0.0, abs=1e-5)
+
+
+# def test_total_mass():
+mass = comp.get_total_mass(); 
+mass_hardware_fun = comp_w_hardware.get_total_mass()
+mass_hardware = SX2DM(mass_hardware_fun(original_density, original_length))
+print("mass=",cs.sumsqr(mass - mass_hardware))
+    # assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5)
+
+# # def test_jacobian():
+J_tot = comp.jacobian_fun("l_sole")
+J_tot_with_hardware = comp_w_hardware.jacobian_fun("l_sole")
+J_test = SX2DM(J_tot(H_b, s_))
+J_test_hardware = SX2DM(J_tot_with_hardware(H_b, s_, original_density, original_length))
+print("J=",cs.sumsqr(J_test- J_test_hardware))
+# assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+
+# def test_jacobian_non_actuated():
+J_tot = comp.jacobian_fun("head")
+J_test = SX2DM(J_tot(H_b, s_))
+J_tot_with_hardware = comp_w_hardware.jacobian_fun("head")
+J_tot_with_hardware_test = SX2DM(J_tot_with_hardware(H_b, s_, original_density, original_length))
+print("J=",cs.sumsqr(J_test - J_tot_with_hardware_test))    
+    # assert J_test - J_tot_with_hardware_test == pytest.approx(0.0, abs=1e-5)
+
+
+# def test_fk():
+T = comp.forward_kinematics_fun("l_sole")
+H_test = SX2DM(T(H_b, s_))
+T_with_hardware = comp_w_hardware.forward_kinematics_fun("l_sole")
+H_with_hardware_test = SX2DM(T_with_hardware(H_b, s_, original_density, original_length))
+print("fk=",cs.sumsqr(H_test - H_with_hardware_test))    
+    # assert H_with_hardware_test[:3,:3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
+    # assert H_with_hardware_test[:3,3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+
+
+# # def test_fk_non_actuated():
+T = comp.forward_kinematics_fun("head")
+H_test = SX2DM(T(H_b, s_))
+T_with_hardware = comp_w_hardware.forward_kinematics_fun("head")
+H_with_hardware_test = SX2DM(T_with_hardware(H_b,s_,original_density, original_length))
+print("fk=",cs.sumsqr(H_test - H_with_hardware_test))
+    # assert H_with_hardware_test[:3,:3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
+    # assert H_with_hardware_test[:3,3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+
+
+# def test_bias_force():
+h = comp.bias_force_fun()
+h_test = SX2DM(h(H_b, s_, vb_, s_dot_))
+
+h_with_hardware = comp_w_hardware.bias_force_fun()
+h_with_hardware_test = SX2DM(h_with_hardware(H_b, s_, vb_, s_dot_, original_density, original_length))
+print("h=",cs.sumsqr(h_with_hardware_test-h_test))   
+# assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4)
+
+
+# def test_coriolis_term():
+C = comp.coriolis_term_fun()
+C_test = SX2DM(C(H_b, s_, vb_, s_dot_))
+C_with_hardware = comp_w_hardware.coriolis_term_fun()
+C_with_hardware_test = SX2DM(C_with_hardware(H_b, s_, vb_, s_dot_, original_density, original_length))
+print("C=",cs.sumsqr(C_test - C_with_hardware_test)) 
+    # assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4)
+
+
+# def test_gravity_term():
+G = comp.gravity_term_fun()
+G_test = SX2DM(G(H_b, s_))
+G_with_hardware = comp_w_hardware.gravity_term_fun()
+G_with_hardware_test = G_with_hardware(H_b,s_, original_density, original_length)
+print("G=",cs.sumsqr(G_test- G_with_hardware_test))
+    # assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4)

From ed89e51ee9ea71a36d7aaa4c60575724a6d7a576 Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Thu, 19 Oct 2023 19:09:25 +0200
Subject: [PATCH 03/19] fix non ashable problem

---
 .../model/parametric_factories/parametric_joint.py    | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/adam/model/parametric_factories/parametric_joint.py b/src/adam/model/parametric_factories/parametric_joint.py
index bdefd2f0..e8147da1 100644
--- a/src/adam/model/parametric_factories/parametric_joint.py
+++ b/src/adam/model/parametric_factories/parametric_joint.py
@@ -19,21 +19,22 @@ def __init__(
     ) -> None:
         self.math = math
         self.name = joint.name
-        self.parent = parent_link
+        self.parent = parent_link.link
+        self.parent_parametric = parent_link
         self.child = joint.child
         self.type = joint.joint_type
         self.axis = joint.axis
         self.limit = joint.limit
         self.idx = idx
         self.joint = joint
-        joint_offset = self.parent.compute_joint_offset(joint, self.parent.link_offset)
+        joint_offset = self.parent_parametric.compute_joint_offset(joint, self.parent_parametric.link_offset)
         self.offset = joint_offset
-        self.origin = self.modify(self.parent.link_offset)
+        self.origin = self.modify(self.parent_parametric.link_offset)
 
     def modify(self, parent_joint_offset):
-        length = self.parent.get_principal_lenght_parametric()
+        length = self.parent_parametric.get_principal_lenght_parametric()
         # Ack for avoiding depending on casadi 
-        vo = self.parent.origin[2]
+        vo = self.parent_parametric.origin[2]
         xyz_rpy = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
         xyz_rpy[0] = self.joint.origin.xyz[0]
         xyz_rpy[1] = self.joint.origin.xyz[1]

From 43d2711bcdbcb7822e0bc6d23bd8641bf8eb96ec Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Fri, 20 Oct 2023 11:34:22 +0200
Subject: [PATCH 04/19] fix computations for casadi

---
 src/adam/casadi/casadi_like.py                | 28 +++++++++++++
 src/adam/core/spatial_math.py                 | 35 +++++++++++++++-
 .../parametric_factories/parametric_joint.py  | 22 ++++++----
 .../parametric_factories/parametric_link.py   | 41 ++++++++++---------
 4 files changed, 97 insertions(+), 29 deletions(-)

diff --git a/src/adam/casadi/casadi_like.py b/src/adam/casadi/casadi_like.py
index ebd9f3fe..c623c4d0 100644
--- a/src/adam/casadi/casadi_like.py
+++ b/src/adam/casadi/casadi_like.py
@@ -160,6 +160,22 @@ def sin(x: npt.ArrayLike) -> "CasadiLike":
         """
         return CasadiLike(cs.sin(x))
 
+
+    @staticmethod
+    def mtimes(x: npt.ArrayLike, y:npt.ArrayLike) -> "CasadiLike":
+        """
+        Args:
+            x (npt.ArrayLike): angle value
+
+        Returns:
+            CasadiLike: the sin value of x
+        """
+        if isinstance(x, CasadiLike) and isinstance(y, CasadiLike):
+            return CasadiLike(cs.mtimes(x.array, y.array))
+        else:
+            return CasadiLike(cs.mtimes(x,y))
+        # return CasadiLike(cs.mtimes(x, y))
+
     @staticmethod
     def cos(x: npt.ArrayLike) -> "CasadiLike":
         """
@@ -195,6 +211,18 @@ def vertcat(*x) -> "CasadiLike":
         # Then the list is unpacked with the * operator.
         y = [xi.array if isinstance(xi, CasadiLike) else xi for xi in x]
         return CasadiLike(cs.vertcat(*y))
+    @staticmethod
+    def horzcat(*x) -> "CasadiLike":
+        """
+        Returns:
+            CasadiLike:  vertical concatenation of elements
+        """
+        # here the logic is a bit convoluted: x is a tuple containing CasadiLike
+        # cs.vertcat accepts *args. A list of cs types is created extracting the value
+        # from the CasadiLike stored in the tuple x.
+        # Then the list is unpacked with the * operator.
+        y = [xi.array if isinstance(xi, CasadiLike) else xi for xi in x]
+        return CasadiLike(cs.horzcat(*y))
 
 
 if __name__ == "__main__":
diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py
index d74bc6d0..e87e8efe 100644
--- a/src/adam/core/spatial_math.py
+++ b/src/adam/core/spatial_math.py
@@ -117,6 +117,21 @@ def vertcat(x: npt.ArrayLike) -> npt.ArrayLike:
         """
         pass
 
+    @abc.abstractmethod
+    def horzcat(x: npt.ArrayLike) -> npt.ArrayLike:
+        """
+        Args:
+            x (npt.ArrayLike): elements
+
+        Returns:
+            npt.ArrayLike: horizontal concatenation of elements x
+        """
+        pass
+
+    @abc.abstractmethod
+    def mtimes(x: npt.ArrayLike, y:npt.ArrayLike) -> npt.ArrayLike:
+        pass
+
     @abc.abstractmethod
     def sin(x: npt.ArrayLike) -> npt.ArrayLike:
         """
@@ -227,7 +242,9 @@ def H_revolute_joint(
         T = self.factory.eye(4)
         R = self.R_from_RPY(rpy) @ self.R_from_axis_angle(axis, q)
         T[:3, :3] = R
-        T[:3, 3] = xyz
+        T[0,3] = xyz[0]
+        T[1,3] = xyz[1]
+        T[2,3] = xyz[2] 
         return T
 
     def H_prismatic_joint(
@@ -264,7 +281,9 @@ def H_from_Pos_RPY(self, xyz: npt.ArrayLike, rpy: npt.ArrayLike) -> npt.ArrayLik
         """
         T = self.factory.eye(4)
         T[:3, :3] = self.R_from_RPY(rpy)
-        T[:3, 3] = xyz
+        T[0,3] = xyz[0]
+        T[1,3] = xyz[1]
+        T[2,3] = xyz[2] 
         return T
 
     def R_from_RPY(self, rpy: npt.ArrayLike) -> npt.ArrayLike:
@@ -381,6 +400,18 @@ def spatial_inertia(
         IO[:3, :3] = self.factory.eye(3) * mass
         return IO
 
+    def spatial_inertial_with_parameter(self, I, mass, c , rpy):
+    # Returns the 6x6 inertia matrix expressed at the origin of the link (with rotation)"""
+        IO = self.factory.zeros(6,6)
+        Sc = self.skew(c)
+        R = self.factory.zeros(3,3)
+        R_temp = self.R_from_RPY(rpy)    
+        inertia_matrix =self.vertcat(self.horzcat(I.ixx,0.0, 0.0), self.horzcat(0.0, I.iyy, 0.0), self.horzcat(0.0, 0.0, I.izz))
+        IO[3:, 3:] = R_temp@inertia_matrix@R_temp.T + mass * self.mtimes(Sc,Sc.T)
+        IO[3:, :3] = mass * Sc
+        IO[:3, 3:] = mass * Sc.T
+        IO[:3, :3] = self.factory.eye(3)* mass
+        return IO
     def spatial_skew(self, v: npt.ArrayLike) -> npt.ArrayLike:
         """
         Args:
diff --git a/src/adam/model/parametric_factories/parametric_joint.py b/src/adam/model/parametric_factories/parametric_joint.py
index e8147da1..d6b5162a 100644
--- a/src/adam/model/parametric_factories/parametric_joint.py
+++ b/src/adam/model/parametric_factories/parametric_joint.py
@@ -19,7 +19,7 @@ def __init__(
     ) -> None:
         self.math = math
         self.name = joint.name
-        self.parent = parent_link.link
+        self.parent = parent_link.link.name
         self.parent_parametric = parent_link
         self.child = joint.child
         self.type = joint.joint_type
@@ -31,6 +31,7 @@ def __init__(
         self.offset = joint_offset
         self.origin = self.modify(self.parent_parametric.link_offset)
 
+
     def modify(self, parent_joint_offset):
         length = self.parent_parametric.get_principal_lenght_parametric()
         # Ack for avoiding depending on casadi 
@@ -58,21 +59,26 @@ def homogeneous(self, q: npt.ArrayLike) -> npt.ArrayLike:
             npt.ArrayLike: the homogenous transform of a joint, given q
         """
 
+
+        o = self.math.factory.zeros(3)
+        o[0] = self.origin[0]
+        o[1] = self.origin[1]
+        o[2] = self.origin[2]
+        rpy = self.origin[3:]
+
         if self.type == "fixed":
-            xyz = self.origin.xyz
-            rpy = self.origin.rpy
-            return self.math.H_from_Pos_RPY(xyz, rpy)
+            return self.math.H_from_Pos_RPY(o, rpy)
         elif self.type in ["revolute", "continuous"]:
             return self.math.H_revolute_joint(
-                self.origin.xyz,
-                self.origin.rpy,
+                o,
+                rpy,
                 self.axis,
                 q,
             )
         elif self.type in ["prismatic"]:
             return self.math.H_prismatic_joint(
-                self.origin.xyz,
-                self.origin.rpy,
+                o,
+                rpy,
                 self.axis,
                 q,
             )
diff --git a/src/adam/model/parametric_factories/parametric_link.py b/src/adam/model/parametric_factories/parametric_link.py
index fcb70afb..d6f87351 100644
--- a/src/adam/model/parametric_factories/parametric_link.py
+++ b/src/adam/model/parametric_factories/parametric_link.py
@@ -6,6 +6,7 @@
 from adam.model import Link
 
 import math
+from adam.model.abc_factories import Inertial
 
 class I_parametric():
     def __init__(self) -> None:
@@ -27,8 +28,6 @@ class Side(Enum):
     WIDTH = 1
     HEIGHT = 2
     DEPTH = 3
-# ['XML_REFL', '_Link__get_collision', '_Link__get_visual', '_Link__set_collision', '_Link__set_visual', '__class__', '__delattr__', '__dict__', '__dir__', '__doc__', '__eq__', '__format__', '__ge__', '__getattribute__', '__gt__', '__hash__', '__init__', '__init_subclass__', '__le__', '__lt__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', 'add_aggregate', 'add_aggregates_to_xml', 'aggregate_init', 'aggregate_order', 'aggregate_type', 'check_valid', 'collision', 'collisions', 'from_xml', 'from_xml_file', 'from_xml_string', 'get_aggregate_list', 'get_refl_vars', 'inertial', 'lump_aggregates', 'name', 'origin', 'parse', 'post_read_xml', 'pre_write_xml', 'read_xml', 'remove_aggregate', 'to_xml', 'to_xml_string', 'to_yaml', 'visual', 'visuals', 'write_xml']
-
 
 class ParametricLink(Link):
     """Parametric Link class"""
@@ -39,19 +38,20 @@ def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath, length_mul
         self.length_multiplier = length_multiplier
         self.density = density
         self.visuals = link.visual
-        print(self.get_geometry(self.visuals))
         self.geometry_type, self.visual_data = self.get_geometry(self.visuals)
         self.link = link
+        # self.parent = self.link.parent
         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]
+        self.inertial = Inertial(self.mass)
+        self.inertial.mass = self.mass
+        self.inertial.inertia = self.I
+        self.inertial.origin = self.origin
 
     def get_principal_lenght(self):  
         xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
@@ -98,7 +98,6 @@ def compute_joint_offset(self,joint_i, parent_offset):
          # Taking the principal direction i.e. the length 
         xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy]  
         v_l= self.get_principal_lenght()
-        print(joint_i.origin.xyz)
         j_0 = joint_i.origin.xyz[2]
         v_o = xyz_rpy[2]
         if(j_0<0):
@@ -111,18 +110,13 @@ def compute_joint_offset(self,joint_i, parent_offset):
 
     @staticmethod
     def get_geometry(visual_obj):
-        print("visual obj",dir(visual_obj))
-        print(type(visual_obj.geometry))
-        print(visual_obj.name)
+
         """Returns the geometry type and thez corresponding geometry object for a given visual"""
         if type(visual_obj.geometry) is urdf_parser_py.urdf.Box:
-            print("IS A BOX")
             return (Geometry.BOX, visual_obj.geometry)
         if type(visual_obj.geometry) is urdf_parser_py.urdf.Cylinder:
-            print("IS A CYLINDER")
             return (Geometry.CYLINDER, visual_obj.geometry)
         if type(visual_obj.geometry) is urdf_parser_py.urdf.Sphere:
-            print("IS A SPHERE")
             return (Geometry.SPHERE, visual_obj.geometry)
 
     """Function that starting from a multiplier and link visual characteristics computes the link volume"""
@@ -178,7 +172,7 @@ def modify_origin(self):
         return origin
 
     def compute_inertia_parametric(self):
-        I = I_parametric
+        I = I_parametric()
         xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy] 
         """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"""
@@ -219,16 +213,25 @@ def spatial_inertia(self) -> npt.ArrayLike:
         """
         I = self.I
         mass = self.mass
-        o = self.origin[:3]
+        o = self.math.factory.zeros(3)
+        o[0] = self.origin[0]
+        o[1] = self.origin[1]
+        o[2] = self.origin[2]
         rpy = self.origin[3:]
-        return self.math.spatial_inertia(I, mass, o, rpy)
+        return self.math.spatial_inertial_with_parameter(I, mass, o, rpy)
 
     def homogeneous(self) -> npt.ArrayLike:
         """
         Returns:
             npt.ArrayLike: the homogeneus transform of the link
         """
+
+        o = self.math.factory.zeros(3)
+        o[0] = self.origin[0]
+        o[1] = self.origin[1]
+        o[2] = self.origin[2]
+        rpy = self.origin[3:]
         return self.math.H_from_Pos_RPY(
-            self.inertial.origin.xyz,
-            self.inertial.origin.rpy,
+            o,
+            rpy,
         )

From c5560ede61037d1f4a7bf9348ba1bfebd25856cb Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Fri, 20 Oct 2023 12:38:18 +0200
Subject: [PATCH 05/19] working with casadi but for the head frame

---
 src/adam/casadi/computations_parametric.py   |  7 +-
 tests/test_CasADi_computations_parametric.py | 78 +++++++++++---------
 2 files changed, 48 insertions(+), 37 deletions(-)

diff --git a/src/adam/casadi/computations_parametric.py b/src/adam/casadi/computations_parametric.py
index c94bb4e8..fe1fb6be 100644
--- a/src/adam/casadi/computations_parametric.py
+++ b/src/adam/casadi/computations_parametric.py
@@ -34,8 +34,8 @@ def __init__(
         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=self.length_multiplier, density=self.density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math,link_parametric_list=link_name_list,lenght_multiplier=self.length_multiplier, density=self.density)
+        model = Model.build(factory=self.factory, joints_name_list=joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=math)
         self.NDoF = self.rbdalgos.NDoF
         self.g = gravity
@@ -169,11 +169,12 @@ def forward_kinematics(self, frame, T_b, s) -> cs.Function:
 
         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
         """
+        m = self.rbdalgos.get_total_mass()
+        return cs.Function("m", [self.density, self.length_multiplier], [m], self.f_opts)
         return self.rbdalgos.get_total_mass()
diff --git a/tests/test_CasADi_computations_parametric.py b/tests/test_CasADi_computations_parametric.py
index 6740d082..9d5abd42 100644
--- a/tests/test_CasADi_computations_parametric.py
+++ b/tests/test_CasADi_computations_parametric.py
@@ -4,7 +4,7 @@
 
 import logging
 from os import link
-
+import urdf_parser_py.urdf
 import casadi as cs
 import numpy as np
 # import pytest
@@ -13,8 +13,14 @@
 from adam.casadi import KinDynComputations
 
 from adam.geometry import utils
+import tempfile
+from git import Repo
 
-model_path ="/home/carlotta/iit_ws/element_hardware-intelligence/Software/OptimizationControlAndHardware/models/model.urdf"
+# Getting stickbot urdf file
+temp_dir = tempfile.TemporaryDirectory()
+git_url = "https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git"
+Repo.clone_from(git_url, temp_dir.name)
+model_path = temp_dir.name + "/models/stickBot/model.urdf"
 
 joints_name_list = [
     "torso_pitch",
@@ -41,22 +47,23 @@
     "r_ankle_pitch",
     "r_ankle_roll",
 ]
+
 def ComputeOriginalDensity(kinDyn, link_name): 
-    link_original = kinDyn.rbdalgos.model.get_element_by_name(link_name, kinDyn.robot_desc)
+    link_original = kinDyn.factory.get_element_by_name(link_name)
     mass = link_original.inertial.mass
     volume = 0 
     visual_obj = link_original.visuals[0]
-    if visual_obj.geometry.box is not None:
-        width = link_original.visuals[0].geometry.box.size[0]
-        depth = link_original.visuals[0].geometry.box.size[2]
-        height = link_original.visuals[0].geometry.box.size[1]
+    if type(visual_obj.geometry) is urdf_parser_py.urdf.Box:
+        width = link_original.visuals[0].geometry.size[0]
+        depth = link_original.visuals[0].geometry.size[2]
+        height = link_original.visuals[0].geometry.size[1]
         volume = width*depth*height
-    if visual_obj.geometry.cylinder is not None:
-        length = link_original.visuals[0].geometry.cylinder.length 
-        radius = link_original.visuals[0].geometry.cylinder.radius
+    if type(visual_obj.geometry) is urdf_parser_py.urdf.Cylinder:
+        length = link_original.visuals[0].geometry.length 
+        radius = link_original.visuals[0].geometry.radius
         volume = math.pi*radius**2*length
-    if visual_obj.geometry.sphere is not None:
-        radius = link_original.visuals[0].geometry.sphere.radius
+    if type(visual_obj.geometry) is urdf_parser_py.urdf.Sphere:
+        radius = link_original.visuals[0].geometry.radius
         volume = 4*(math.pi*radius**3)/3
     return mass/volume
 
@@ -70,15 +77,16 @@ def SX2DM(x):
 root_link = "root_link"
 comp = KinDynComputations(model_path, joints_name_list, root_link)
 
-link_name_list = ['r_upper_leg','r_lower_leg', 'r_hip_1', 'r_hip_2', 'r_ankle_1', 'r_ankle_2','l_upper_leg','l_lower_leg', 'l_hip_1', 'l_hip_2', 'l_ankle_1', 'l_ankle_2', 'l_shoulder_1', 'l_shoulder_2', 'l_shoulder_3', 'l_elbow_1','r_shoulder_1', 'r_shoulder_2', 'r_shoulder_3', 'r_elbow_1']
+# link_name_list = ['r_upper_leg','r_lower_leg', 'r_hip_1', 'r_hip_2', 'r_ankle_1', 'r_ankle_2','l_upper_leg','l_lower_leg', 'l_hip_1', 'l_hip_2', 'l_ankle_1', 'l_ankle_2', 'l_shoulder_1', 'l_shoulder_2', 'l_shoulder_3', 'l_elbow_1','r_shoulder_1', 'r_shoulder_2', 'r_shoulder_3', 'r_elbow_1']
+link_name_list = ['chest']
 comp_w_hardware = KinDynComputationsParametric(model_path, joints_name_list, link_name_list, root_link)
 original_density = []
 for item in link_name_list: 
     original_density += [ComputeOriginalDensity(comp_w_hardware,item)]
+print("original density", original_density)
+original_length = np.ones(len(link_name_list))
 
-original_length = np.ones([len(link_name_list),3])
-
-joint_type = np.zeros(len(joints_name_list))
+# joint_type = np.zeros(len(joints_name_list))
 
 n_dofs = len(joints_name_list)
 # base pose quantities
@@ -97,8 +105,10 @@ def SX2DM(x):
 # # def test_mass_matrix():
 M = comp.mass_matrix_fun()
 M_with_hardware = comp_w_hardware.mass_matrix_fun()
+print(M_with_hardware)
 mass_test = SX2DM(M(H_b, s_))
-mass_test_hardware = SX2DM(M_with_hardware(H_b,s_, original_density, original_length))
+mass_test_hardware = SX2DM(M_with_hardware(H_b,s_, original_length, original_density))
+# print("mass test hardware", mass_test_hardware)
 print("M=",cs.sumsqr(mass_test - mass_test_hardware))
     # assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5)
 
@@ -107,7 +117,7 @@ def SX2DM(x):
 Jcm = comp.centroidal_momentum_matrix_fun()
 Jcm_with_hardware = comp_w_hardware.centroidal_momentum_matrix_fun()
 Jcm_test = SX2DM(Jcm(H_b, s_))
-Jcm_test_hardware = SX2DM(Jcm_with_hardware(H_b,s_, original_density, original_length))
+Jcm_test_hardware = SX2DM(Jcm_with_hardware(H_b,s_, original_length, original_density))
 print("Jcmm=",cs.sumsqr(Jcm_test - Jcm_test_hardware))
 # assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5)
 
@@ -116,24 +126,24 @@ def SX2DM(x):
 com_f = comp.CoM_position_fun()
 com_with_hardware_f = comp_w_hardware.CoM_position_fun()
 CoM_cs = SX2DM(com_f(H_b, s_))
-CoM_hardware  = SX2DM(com_with_hardware_f(H_b,s_, original_density, original_length)) 
+CoM_hardware  = SX2DM(com_with_hardware_f(H_b,s_, original_length, original_density)) 
 print("CoM=",cs.sumsqr(CoM_cs - CoM_hardware))
     # assert CoM_cs - CoM_hardware == pytest.approx(0.0, abs=1e-5)
 
 
-# def test_total_mass():
+# # def test_total_mass():
 mass = comp.get_total_mass(); 
 mass_hardware_fun = comp_w_hardware.get_total_mass()
-mass_hardware = SX2DM(mass_hardware_fun(original_density, original_length))
+mass_hardware = SX2DM(mass_hardware_fun(original_length, original_density))
 print("mass=",cs.sumsqr(mass - mass_hardware))
-    # assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5)
+#     # assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5)
 
 # # def test_jacobian():
 J_tot = comp.jacobian_fun("l_sole")
 J_tot_with_hardware = comp_w_hardware.jacobian_fun("l_sole")
 J_test = SX2DM(J_tot(H_b, s_))
-J_test_hardware = SX2DM(J_tot_with_hardware(H_b, s_, original_density, original_length))
-print("J=",cs.sumsqr(J_test- J_test_hardware))
+J_test_hardware = SX2DM(J_tot_with_hardware(H_b, s_, original_length, original_density))
+print("J l sole=",cs.sumsqr(J_test- J_test_hardware))
 # assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
 
 
@@ -141,8 +151,8 @@ def SX2DM(x):
 J_tot = comp.jacobian_fun("head")
 J_test = SX2DM(J_tot(H_b, s_))
 J_tot_with_hardware = comp_w_hardware.jacobian_fun("head")
-J_tot_with_hardware_test = SX2DM(J_tot_with_hardware(H_b, s_, original_density, original_length))
-print("J=",cs.sumsqr(J_test - J_tot_with_hardware_test))    
+J_tot_with_hardware_test = SX2DM(J_tot_with_hardware(H_b, s_, original_length, original_density))
+print("J head =",cs.sumsqr(J_test - J_tot_with_hardware_test))    
     # assert J_test - J_tot_with_hardware_test == pytest.approx(0.0, abs=1e-5)
 
 
@@ -150,18 +160,18 @@ def SX2DM(x):
 T = comp.forward_kinematics_fun("l_sole")
 H_test = SX2DM(T(H_b, s_))
 T_with_hardware = comp_w_hardware.forward_kinematics_fun("l_sole")
-H_with_hardware_test = SX2DM(T_with_hardware(H_b, s_, original_density, original_length))
-print("fk=",cs.sumsqr(H_test - H_with_hardware_test))    
+H_with_hardware_test = SX2DM(T_with_hardware(H_b, s_, original_length, original_density))
+print("fk l sole =",cs.sumsqr(H_test - H_with_hardware_test))    
     # assert H_with_hardware_test[:3,:3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
     # assert H_with_hardware_test[:3,3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
 
 
 # # def test_fk_non_actuated():
-T = comp.forward_kinematics_fun("head")
+# T = comp.forward_kinematics_fun("head")
 H_test = SX2DM(T(H_b, s_))
 T_with_hardware = comp_w_hardware.forward_kinematics_fun("head")
-H_with_hardware_test = SX2DM(T_with_hardware(H_b,s_,original_density, original_length))
-print("fk=",cs.sumsqr(H_test - H_with_hardware_test))
+H_with_hardware_test = SX2DM(T_with_hardware(H_b,s_,original_length, original_density))
+print("fk head =",cs.sumsqr(H_test - H_with_hardware_test))
     # assert H_with_hardware_test[:3,:3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
     # assert H_with_hardware_test[:3,3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
 
@@ -171,7 +181,7 @@ def SX2DM(x):
 h_test = SX2DM(h(H_b, s_, vb_, s_dot_))
 
 h_with_hardware = comp_w_hardware.bias_force_fun()
-h_with_hardware_test = SX2DM(h_with_hardware(H_b, s_, vb_, s_dot_, original_density, original_length))
+h_with_hardware_test = SX2DM(h_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density))
 print("h=",cs.sumsqr(h_with_hardware_test-h_test))   
 # assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4)
 
@@ -180,7 +190,7 @@ def SX2DM(x):
 C = comp.coriolis_term_fun()
 C_test = SX2DM(C(H_b, s_, vb_, s_dot_))
 C_with_hardware = comp_w_hardware.coriolis_term_fun()
-C_with_hardware_test = SX2DM(C_with_hardware(H_b, s_, vb_, s_dot_, original_density, original_length))
+C_with_hardware_test = SX2DM(C_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density))
 print("C=",cs.sumsqr(C_test - C_with_hardware_test)) 
     # assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4)
 
@@ -189,6 +199,6 @@ def SX2DM(x):
 G = comp.gravity_term_fun()
 G_test = SX2DM(G(H_b, s_))
 G_with_hardware = comp_w_hardware.gravity_term_fun()
-G_with_hardware_test = G_with_hardware(H_b,s_, original_density, original_length)
+G_with_hardware_test = G_with_hardware(H_b,s_, original_length, original_density)
 print("G=",cs.sumsqr(G_test- G_with_hardware_test))
     # assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4)

From e584294eda13508a6cdef61396f3de0934217e56 Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Fri, 20 Oct 2023 19:12:54 +0200
Subject: [PATCH 06/19] Added computations parametric for numpy, jax, pythorch

---
 src/adam/jax/computations_parametric.py     | 258 ++++++++++++++++++
 src/adam/jax/jax_like.py                    |  20 ++
 src/adam/numpy/computations_parametric.py   | 262 ++++++++++++++++++
 src/adam/numpy/numpy_like.py                |  29 ++
 src/adam/pytorch/computations_parametric.py | 280 ++++++++++++++++++++
 src/adam/pytorch/torch_like.py              |  21 ++
 6 files changed, 870 insertions(+)
 create mode 100644 src/adam/jax/computations_parametric.py
 create mode 100644 src/adam/numpy/computations_parametric.py
 create mode 100644 src/adam/pytorch/computations_parametric.py

diff --git a/src/adam/jax/computations_parametric.py b/src/adam/jax/computations_parametric.py
new file mode 100644
index 00000000..8bf4246c
--- /dev/null
+++ b/src/adam/jax/computations_parametric.py
@@ -0,0 +1,258 @@
+# 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 jax.numpy as jnp
+import numpy as np
+from jax import grad, jit, vmap
+
+from adam.core.rbd_algorithms import RBDAlgorithms
+from adam.jax.jax_like import SpatialMath
+from adam.model import Model, URDFParametricModelFactory
+
+
+class KinDynComputationsParametric:
+    """This is a small class that retrieves robot quantities using Jax
+    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 = jnp.array([0, 0, -9.80665, 0, 0, 0]),
+    ) -> 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()
+        self.link_name_list = link_name_list
+        self.g = gravity
+
+    def mass_matrix(self, base_transform: jnp.array, joint_positions: jnp.array,length_multiplier:jnp.array, density: jnp.array):
+        """Returns the Mass Matrix functions computed the CRBA
+
+        Args:
+            base_transform (jnp.array): The homogenous transform from base to world frame
+            joint_positions (jnp.array): The joints position
+
+        Returns:
+            M (jax): Mass Matrix
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        [M, _] = self.rbdalgos.crba(base_transform, joint_positions)
+        return M.array
+
+    def centroidal_momentum_matrix(
+        self, base_transform: jnp.array, joint_positions: jnp.array,length_multiplier:jnp.array, density: jnp.array
+    ):
+        """Returns the Centroidal Momentum Matrix functions computed the CRBA
+
+        Args:
+            base_transform (jnp.array): The homogenous transform from base to world frame
+            joint_positions (jnp.array): The joints position
+
+        Returns:
+            Jcc (jnp.array): Centroidal Momentum matrix
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        [_, Jcm] = self.rbdalgos.crba(base_transform, joint_positions)
+        return Jcm.array
+
+    def relative_jacobian(self, frame: str, joint_positions: jnp.array,length_multiplier:jnp.array, density: jnp.array):
+        """Returns the Jacobian between the root link and a specified frame frames
+
+        Args:
+            frame (str): The tip of the chain
+            joint_positions (jnp.array): The joints position
+
+        Returns:
+            J (jnp.array): The Jacobian between the root and the frame
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        return self.rbdalgos.relative_jacobian(frame, joint_positions).array
+
+    def forward_kinematics(
+        self, frame: str, base_transform: jnp.array, joint_positions: jnp.array,length_multiplier:jnp.array, density: jnp.array
+    ):
+        """Computes the forward kinematics relative to the specified frame
+
+        Args:
+            frame (str): The frame to which the fk will be computed
+            base_transform (jnp.array): The homogenous transform from base to world frame
+            joint_positions (jnp.array): The joints position
+
+        Returns:
+            T_fk (jnp.array): The fk represented as Homogenous transformation matrix
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        return self.rbdalgos.forward_kinematics(
+            frame, base_transform, joint_positions
+        ).array
+
+    def forward_kinematics_fun(self, frame,length_multiplier:jnp.array, density: jnp.array):
+        return lambda T, joint_positions: self.forward_kinematics(
+            frame, T, joint_positions
+        )
+
+    def jacobian(self, frame: str, base_transform, joint_positions,length_multiplier:jnp.array, density: jnp.array):
+        """Returns the Jacobian relative to the specified frame
+
+        Args:
+            base_transform (jnp.array): The homogenous transform from base to world frame
+            s (jnp.array): The joints position
+            frame (str): The frame to which the jacobian will be computed
+
+        Returns:
+            J_tot (jnp.array): The Jacobian relative to the frame
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        return self.rbdalgos.jacobian(frame, base_transform, joint_positions).array
+
+    def bias_force(
+        self,
+        base_transform: jnp.array,
+        joint_positions: jnp.array,
+        base_velocity: jnp.array,
+        s_dot: jnp.array,
+        length_multiplier:jnp.array,
+        density: jnp.array
+    ) -> jnp.array:
+        """Returns the bias force of the floating-base dynamics ejoint_positionsuation,
+        using a reduced RNEA (no acceleration and external forces)
+
+        Args:
+            base_transform (jnp.array): The homogenous transform from base to world frame
+            joint_positions (jnp.array): The joints position
+            base_velocity (jnp.array): The base velocity in mixed representation
+            s_dot (jnp.array): The joints velocity
+
+        Returns:
+            h (jnp.array): the bias force
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        return self.rbdalgos.rnea(
+            base_transform, joint_positions, base_velocity, s_dot, self.g
+        ).array.squeeze()
+
+    def coriolis_term(
+        self,
+        base_transform: jnp.array,
+        joint_positions: jnp.array,
+        base_velocity: jnp.array,
+        s_dot: jnp.array,
+        length_multiplier:jnp.array,
+        density: jnp.array
+    ) -> jnp.array:
+        """Returns the coriolis term of the floating-base dynamics ejoint_positionsuation,
+        using a reduced RNEA (no acceleration and external forces)
+
+        Args:
+            base_transform (jnp.array): The homogenous transform from base to world frame
+            joint_positions (jnp.array): The joints position
+            base_velocity (jnp.array): The base velocity in mixed representation
+            s_dot (jnp.array): The joints velocity
+
+        Returns:
+            C (jnp.array): the Coriolis term
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        return self.rbdalgos.rnea(
+            base_transform,
+            joint_positions,
+            base_velocity.reshape(6, 1),
+            s_dot,
+            np.zeros(6),
+        ).array.squeeze()
+
+    def gravity_term(
+        self, base_transform: jnp.array, joint_positions: jnp.array,length_multiplier:jnp.array, density: jnp.array
+    ) -> jnp.array:
+        """Returns the gravity term of the floating-base dynamics ejoint_positionsuation,
+        using a reduced RNEA (no acceleration and external forces)
+
+        Args:
+            base_transform (jnp.array): The homogenous transform from base to world frame
+            joint_positions (jnp.array): The joints position
+
+        Returns:
+            G (jnp.array): the gravity term
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        return self.rbdalgos.rnea(
+            base_transform,
+            joint_positions,
+            np.zeros(6).reshape(6, 1),
+            np.zeros(self.NDoF),
+            self.g,
+        ).array.squeeze()
+
+    def CoM_position(
+        self, base_transform: jnp.array, joint_positions: jnp.array, length_multiplier:jnp.array, density: jnp.array
+    ) -> jnp.array:
+        """Returns the CoM positon
+
+        Args:
+            base_transform (jnp.array): The homogenous transform from base to world frame
+            joint_positions (jnp.array): The joints position
+
+        Returns:
+            com (jnp.array): The CoM position
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        return self.rbdalgos.CoM_position(
+            base_transform, joint_positions
+        ).array.squeeze()
+
+    def get_total_mass(self,length_multiplier:jnp.array, density: jnp.array) -> float:
+        """Returns the total mass of the robot
+
+        Returns:
+            mass: The total mass
+        """
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        return self.rbdalgos.get_total_mass()
diff --git a/src/adam/jax/jax_like.py b/src/adam/jax/jax_like.py
index 83df661f..74eeae76 100644
--- a/src/adam/jax/jax_like.py
+++ b/src/adam/jax/jax_like.py
@@ -188,6 +188,14 @@ def skew(x: Union["JaxLike", npt.ArrayLike]) -> "JaxLike":
         x = x.array
         return JaxLike(-jnp.cross(jnp.array(x), jnp.eye(3), axisa=0, axisb=0))
 
+    @staticmethod
+    def mtimes(x:npt.ArrayLike, y:npt.ArrayLike) -> "JaxLike":
+        if  isinstance(x, JaxLike) and isinstance(y, JaxLike):
+            return jnp.mtimes(jnp.array(x), jnp.array(y))
+        else: 
+            return jnp.mtimes(jnp.array(x.array), jnp.array(y.array))
+        return JaxLike(-jnp.cross(jnp.array(x), jnp.eye(3), axisa=0, axisb=0))
+
     @staticmethod
     def vertcat(*x) -> "JaxLike":
         """
@@ -199,3 +207,15 @@ def vertcat(*x) -> "JaxLike":
         else:
             v = jnp.vstack([x[i] for i in range(len(x))]).reshape(-1, 1)
         return JaxLike(v)
+
+    @staticmethod
+    def horzcat(*x) -> "JaxLike":
+        """
+        Returns:
+            JaxLike: Horizontal concatenation of elements
+        """
+        if isinstance(x[0], JaxLike):
+            v = jnp.hstack([x[i].array for i in range(len(x))]).reshape(1,-1)
+        else:
+            v = jnp.hstack([x[i] for i in range(len(x))]).reshape(1, -1)
+        return JaxLike(v)
diff --git a/src/adam/numpy/computations_parametric.py b/src/adam/numpy/computations_parametric.py
new file mode 100644
index 00000000..4b9a7790
--- /dev/null
+++ b/src/adam/numpy/computations_parametric.py
@@ -0,0 +1,262 @@
+# 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 numpy as np
+
+from adam.core.rbd_algorithms import RBDAlgorithms
+from adam.model import Model, URDFParametricModelFactory
+from adam.numpy.numpy_like import SpatialMath
+
+
+class KinDynComputationsParametric:
+    """This is a small class that retrieves robot quantities using NumPy
+    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, -9.80665, 0, 0, 0]),
+    ) -> 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.link_name_list = link_name_list
+        math = SpatialMath()
+        self.g = gravity
+
+    def mass_matrix(
+        self, base_transform: np.ndarray, joint_positions: np.ndarray, length_multiplier :np.ndarray, density: np.ndarray
+    ) -> np.ndarray:
+        """Returns the Mass Matrix functions computed the CRBA
+
+        Args:
+            base_transform (np.ndarray): The homogenous transform from base to world frame
+            joint_positions (np.ndarray): The joints position
+
+        Returns:
+            M (np.ndarray): Mass Matrix
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        self.NDoF = model.NDoF
+        [M, _] = self.rbdalgos.crba(base_transform, joint_positions)
+        return M.array
+
+    def centroidal_momentum_matrix(
+        self, base_transform: np.ndarray, s: np.ndarray,length_multiplier :np.ndarray, density: np.ndarray
+    ) -> np.ndarray:
+        """Returns the Centroidal Momentum Matrix functions computed the CRBA
+
+        Args:
+            base_transform (np.ndarray): The homogenous transform from base to world frame
+            joint_positions (np.ndarray): The joint positions
+
+        Returns:
+            Jcc (np.ndarray): Centroidal Momentum matrix
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        self.NDoF = model.NDoF
+        [_, Jcm] = self.rbdalgos.crba(base_transform, s)
+        return Jcm.array
+
+    def forward_kinematics(
+        self, frame: str, base_transform: np.ndarray, joint_positions: np.ndarray,length_multiplier :np.ndarray, density: np.ndarray
+    ) -> np.ndarray:
+        """Computes the forward kinematics relative to the specified frame
+
+        Args:
+            frame (str): The frame to which the fk will be computed
+            base_transform (np.ndarray): The homogenous transform from base to world frame
+            joint_positions (np.ndarray): The joints position
+
+        Returns:
+            T_fk (np.ndarray): The fk represented as Homogenous transformation matrix
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        self.NDoF = model.NDoF
+        return self.rbdalgos.forward_kinematics(
+            frame, base_transform, joint_positions
+        ).array.squeeze()
+
+    def jacobian(
+        self, frame: str, base_transform: np.ndarray, joint_positions: np.ndarray,length_multiplier :np.ndarray, density: np.ndarray
+    ) -> np.ndarray:
+        """Returns the Jacobian relative to the specified frame
+
+        Args:
+            frame (str): The frame to which the jacobian will be computed
+            base_transform (np.ndarray): The homogenous transform from base to world frame
+            joint_positions (np.ndarray): The joints position
+
+        Returns:
+            J_tot (np.ndarray): The Jacobian relative to the frame
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        self.NDoF = model.NDoF
+        return self.rbdalgos.jacobian(
+            frame, base_transform, joint_positions
+        ).array.squeeze()
+
+    def relative_jacobian(self, frame: str, joint_positions: np.ndarray,length_multiplier :np.ndarray, density: np.ndarray) -> np.ndarray:
+        """Returns the Jacobian between the root link and a specified frame frames
+
+        Args:
+            frame (str): The tip of the chain
+            joint_positions (np.ndarray): The joints position
+
+        Returns:
+            J (np.ndarray): The Jacobian between the root and the frame
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        self.NDoF = model.NDoF
+        return self.rbdalgos.relative_jacobian(frame, joint_positions).array
+
+    def CoM_position(
+        self, base_transform: np.ndarray, joint_positions: np.ndarray,length_multiplier :np.ndarray, density: np.ndarray
+    ) -> np.ndarray:
+        """Returns the CoM positon
+
+        Args:
+            base_transform (np.ndarray): The homogenous transform from base to world frame
+            joint_positions (np.ndarray): The joints position
+
+        Returns:
+            CoM (np.ndarray): The CoM position
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        self.NDoF = model.NDoF
+        return self.rbdalgos.CoM_position(
+            base_transform, joint_positions
+        ).array.squeeze()
+
+    def bias_force(
+        self,
+        base_transform: np.ndarray,
+        joint_positions: np.ndarray,
+        base_velocity: np.ndarray,
+        joint_velocities: np.ndarray,
+        length_multiplier :np.ndarray,
+         density: np.ndarray
+    ) -> np.ndarray:
+        """Returns the bias force of the floating-base dynamics equation,
+        using a reduced RNEA (no acceleration and external forces)
+
+        Args:
+            base_transform (np.ndarray): The homogenous transform from base to world frame
+            joint_positions (np.ndarray): The joints position
+            base_velocity (np.ndarray): The base velocity in mixed representation
+            joint_velocities (np.ndarray): The joint velocities
+
+        Returns:
+            h (np.ndarray): the bias force
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        self.NDoF = model.NDoF
+        return self.rbdalgos.rnea(
+            base_transform,
+            joint_positions,
+            base_velocity.reshape(6, 1),
+            joint_velocities,
+            self.g,
+        ).array.squeeze()
+
+    def coriolis_term(
+        self,
+        base_transform: np.ndarray,
+        joint_positions: np.ndarray,
+        base_velocity: np.ndarray,
+        joint_velocities: np.ndarray,
+        length_multiplier :np.ndarray,
+        density: np.ndarray
+    ) -> np.ndarray:
+        """Returns the coriolis term of the floating-base dynamics equation,
+        using a reduced RNEA (no acceleration and external forces)
+
+        Args:
+            base_transform (np.ndarray): The homogenous transform from base to world frame
+            joint_positions (np.ndarray): The joints position
+            base_velocity (np.ndarray): The base velocity in mixed representation
+            joint_velocities (np.ndarray): The joint velocities
+
+        Returns:
+            C (np.ndarray): the Coriolis term
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        self.NDoF = model.NDoF
+        # set in the bias force computation the gravity term to zero
+        return self.rbdalgos.rnea(
+            base_transform,
+            joint_positions,
+            base_velocity.reshape(6, 1),
+            joint_velocities,
+            np.zeros(6),
+        ).array.squeeze()
+
+    def gravity_term(
+        self, base_transform: np.ndarray, joint_positions: np.ndarray,length_multiplier :np.ndarray, density: np.ndarray
+    ) -> np.ndarray:
+        """Returns the gravity term of the floating-base dynamics equation,
+        using a reduced RNEA (no acceleration and external forces)
+
+        Args:
+            base_transform (np.ndarray): The homogenous transform from base to world frame
+            joint_positions (np.ndarray): The joints position
+
+        Returns:
+            G (np.ndarray): the gravity term
+        """
+
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        self.NDoF = model.NDoF
+        return self.rbdalgos.rnea(
+            base_transform,
+            joint_positions,
+            np.zeros(6).reshape(6, 1),
+            np.zeros(self.NDoF),
+            self.g,
+        ).array.squeeze()
+
+    def get_total_mass(self,length_multiplier :np.ndarray, density: np.ndarray) -> float:
+        """Returns the total mass of the robot
+
+        Returns:
+            mass: The total mass
+        """
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        self.NDoF = model.NDoF
+        return self.rbdalgos.get_total_mass()
diff --git a/src/adam/numpy/numpy_like.py b/src/adam/numpy/numpy_like.py
index 85ee2f78..c30afb8b 100644
--- a/src/adam/numpy/numpy_like.py
+++ b/src/adam/numpy/numpy_like.py
@@ -150,6 +150,22 @@ def sin(x: npt.ArrayLike) -> "NumpyLike":
             NumpyLike: sin value of x
         """
         return NumpyLike(np.sin(x))
+    
+    @staticmethod
+    def mtimes(x: npt.ArrayLike, y:npt.ArrayLike) -> "NumpyLike":
+        """
+        Args:
+            x (npt.ArrayLike): angle value
+
+        Returns:
+            CasadiLike: the sin value of x
+        """
+        if isinstance(x, NumpyLike) and isinstance(y, NumpyLike):
+            return NumpyLike((x.array@y.array))
+        else:
+            return NumpyLike(x@y)
+        # return CasadiLike(cs.mtimes(x, y))
+
 
     @staticmethod
     def cos(x: npt.ArrayLike) -> "NumpyLike":
@@ -188,6 +204,19 @@ def vertcat(*x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike":
             v = np.vstack([x[i] for i in range(len(x))]).reshape(-1, 1)
         return NumpyLike(v)
 
+    
+    @staticmethod
+    def horzcat(*x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike":
+        """
+        Returns:
+            NumpyLike: horrizontal concatenation of x
+        """
+        if isinstance(x[0], NumpyLike):
+            v = np.hstack([x[i].array for i in range(len(x))]).reshape(1, -1)
+        else:
+            v = np.hstack([x[i] for i in range(len(x))]).reshape(1, -1)
+        return NumpyLike(v)
+
     @staticmethod
     def skew(x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike":
         """
diff --git a/src/adam/pytorch/computations_parametric.py b/src/adam/pytorch/computations_parametric.py
new file mode 100644
index 00000000..bae03145
--- /dev/null
+++ b/src/adam/pytorch/computations_parametric.py
@@ -0,0 +1,280 @@
+# 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 numpy as np
+import torch
+
+from adam.core.rbd_algorithms import RBDAlgorithms
+from adam.model import Model, URDFModelFactory, URDFParametricModelFactory
+from adam.pytorch.torch_like import SpatialMath
+
+
+class KinDynComputations:
+    """This is a small class that retrieves robot quantities using Pytorch
+    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 = torch.FloatTensor([0, 0, -9.80665, 0, 0, 0]),
+    ) -> 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.g = gravity
+
+    def mass_matrix(
+        self, base_transform: torch.Tensor, s: torch.Tensor, length_multiplier:torch.Tensor, density:torch.Tensor
+    ) -> torch.Tensor:
+        """Returns the Mass Matrix functions computed the CRBA
+
+        Args:
+            base_transform (torch.tensor): The homogenous transform from base to world frame
+            s (torch.tensor): The joints position
+
+        Returns:
+            M (torch.tensor): Mass Matrix
+        """
+
+        math = SpatialMath()
+        self.link_name_list = link_name_list
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        [M, _] = self.rbdalgos.crba(base_transform, s)
+        return M.array
+
+    def centroidal_momentum_matrix(
+        self, base_transform: torch.Tensor, s: torch.Tensor, length_multiplier:torch.Tensor, density:torch.Tensor
+    ) -> torch.Tensor:
+        """Returns the Centroidal Momentum Matrix functions computed the CRBA
+
+        Args:
+            base_transform (torch.tensor): The homogenous transform from base to world frame
+            s (torch.tensor): The joints position
+
+        Returns:
+            Jcc (torch.tensor): Centroidal Momentum matrix
+        """
+
+        math = SpatialMath()
+        self.link_name_list = link_name_list
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        [_, Jcm] = self.rbdalgos.crba(base_transform, s)
+        return Jcm.array
+
+    def forward_kinematics(
+        self, frame, base_transform: torch.Tensor, s: torch.Tensor, length_multiplier:torch.Tensor, density:torch.Tensor
+    ) -> torch.Tensor:
+        """Computes the forward kinematics relative to the specified frame
+
+        Args:
+            frame (str): The frame to which the fk will be computed
+            base_transform (torch.tensor): The homogenous transform from base to world frame
+            s (torch.tensor): The joints position
+
+        Returns:
+            T_fk (torch.tensor): The fk represented as Homogenous transformation matrix
+        """
+
+        math = SpatialMath()
+        self.link_name_list = link_name_list
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        return (
+            self.rbdalgos.forward_kinematics(
+                frame, torch.FloatTensor(base_transform), torch.FloatTensor(s)
+            )
+        ).array
+
+    def jacobian(
+        self, frame: str, base_transform: torch.Tensor, joint_positions: torch.Tensor, length_multiplier:torch.Tensor, density:torch.Tensor
+    ) -> torch.Tensor:
+        """Returns the Jacobian relative to the specified frame
+
+        Args:
+            frame (str): The frame to which the jacobian will be computed
+            base_transform (torch.tensor): The homogenous transform from base to world frame
+            joint_positions (torch.tensor): The joints position
+
+        Returns:
+            J_tot (torch.tensor): The Jacobian relative to the frame
+        """
+
+        math = SpatialMath()
+        self.link_name_list = link_name_list
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        return self.rbdalgos.jacobian(frame, base_transform, joint_positions).array
+
+    def relative_jacobian(self, frame, joint_positions: torch.Tensor, length_multiplier:torch.Tensor, density:torch.Tensor) -> torch.Tensor:
+        """Returns the Jacobian between the root link and a specified frame frames
+
+        Args:
+            frame (str): The tip of the chain
+            joint_positions (torch.tensor): The joints position
+
+        Returns:
+            J (torch.tensor): The Jacobian between the root and the frame
+        """
+
+        math = SpatialMath()
+        self.link_name_list = link_name_list
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        return self.rbdalgos.relative_jacobian(frame, joint_positions).array
+
+    def CoM_position(
+        self, base_transform: torch.Tensor, joint_positions: torch.Tensor, length_multiplier:torch.Tensor, density:torch.Tensor
+    ) -> torch.Tensor:
+        """Returns the CoM positon
+
+        Args:
+            base_transform (torch.tensor): The homogenous transform from base to world frame
+            joint_positions (torch.tensor): The joints position
+
+        Returns:
+            com (torch.tensor): The CoM position
+        """
+
+        math = SpatialMath()
+        self.link_name_list = link_name_list
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        return self.rbdalgos.CoM_position(
+            base_transform, joint_positions
+        ).array.squeeze()
+
+    def bias_force(
+        self,
+        base_transform: torch.Tensor,
+        s: torch.Tensor,
+        base_velocity: torch.Tensor,
+        joint_velocities: torch.Tensor,
+        length_multiplier:torch.Tensor,
+        density:torch.Tensor
+    ) -> torch.Tensor:
+        """Returns the bias force of the floating-base dynamics ejoint_positionsuation,
+        using a reduced RNEA (no acceleration and external forces)
+
+        Args:
+            base_transform (torch.tensor): The homogenous transform from base to world frame
+            s (torch.tensor): The joints position
+            base_velocity (torch.tensor): The base velocity in mixed representation
+            joint_velocities (torch.tensor): The joints velocity
+
+        Returns:
+            h (torch.tensor): the bias force
+        """
+
+        math = SpatialMath()
+        self.link_name_list = link_name_list
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        return self.rbdalgos.rnea(
+            base_transform,
+            s,
+            base_velocity.reshape(6, 1),
+            joint_velocities,
+            self.g,
+        ).array.squeeze()
+
+    def coriolis_term(
+        self,
+        base_transform: torch.Tensor,
+        joint_positions: torch.Tensor,
+        base_velocity: torch.Tensor,
+        joint_velocities: torch.Tensor,
+        length_multiplier:torch.Tensor,
+        density:torch.Tensor
+    ) -> torch.Tensor:
+        """Returns the coriolis term of the floating-base dynamics ejoint_positionsuation,
+        using a reduced RNEA (no acceleration and external forces)
+
+        Args:
+            base_transform (torch.tensor): The homogenous transform from base to world frame
+            joint_positions (torch.tensor): The joints position
+            base_velocity (torch.tensor): The base velocity in mixed representation
+            joint_velocities (torch.tensor): The joints velocity
+
+        Returns:
+            C (torch.tensor): the Coriolis term
+        """
+
+        math = SpatialMath()
+        self.link_name_list = link_name_list
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        # set in the bias force computation the gravity term to zero
+        return self.rbdalgos.rnea(
+            base_transform,
+            joint_positions,
+            base_velocity.reshape(6, 1),
+            joint_velocities,
+            torch.zeros(6),
+        ).array.squeeze()
+
+    def gravity_term(
+        self, base_transform: torch.Tensor, base_positions: torch.Tensor, length_multiplier:torch.Tensor, density:torch.Tensor
+    ) -> torch.Tensor:
+        """Returns the gravity term of the floating-base dynamics ejoint_positionsuation,
+        using a reduced RNEA (no acceleration and external forces)
+
+        Args:
+            base_transform (torch.tensor): The homogenous transform from base to world frame
+            base_positions (torch.tensor): The joints position
+
+        Returns:
+            G (torch.tensor): the gravity term
+        """
+        math = SpatialMath()
+        self.link_name_list = link_name_list
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        return self.rbdalgos.rnea(
+            base_transform,
+            base_positions,
+            torch.zeros(6).reshape(6, 1),
+            torch.zeros(self.NDoF),
+            torch.FloatTensor(self.g),
+        ).array.squeeze()
+
+    def get_total_mass(self, length_multiplier:torch.Tensor, density:torch.Tensor) -> float:
+        """Returns the total mass of the robot
+
+        Returns:
+            mass: The total mass
+        """
+        math = SpatialMath()
+        self.link_name_list = link_name_list
+        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_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
+        return self.rbdalgos.get_total_mass()
diff --git a/src/adam/pytorch/torch_like.py b/src/adam/pytorch/torch_like.py
index a1afde00..1cbf05d7 100644
--- a/src/adam/pytorch/torch_like.py
+++ b/src/adam/pytorch/torch_like.py
@@ -202,6 +202,14 @@ def skew(x: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike":
             torch.FloatTensor([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]])
         )
 
+
+    @staticmethod
+    def skew(x: ntp.ArrayLike, y:np.ArrayLike) -> "TorchLike":
+        if isinstance(x, TorchLike) and isinstance(y, TorchLike):
+            return TorchLike(torch.mtimes(TorchLike(x), TorchLike(y)))
+        else:
+            return TorchLike(torch.mtimes(x.array, y.array))
+
     @staticmethod
     def vertcat(*x: ntp.ArrayLike) -> "TorchLike":
         """
@@ -213,3 +221,16 @@ def vertcat(*x: ntp.ArrayLike) -> "TorchLike":
         else:
             v = torch.FloatTensor(x).reshape(-1, 1)
         return TorchLike(v)
+
+
+    @staticmethod
+    def horzcat(*x: ntp.ArrayLike) -> "TorchLike":
+        """
+        Returns:
+            TorchLike: horizontal concatenation of x
+        """
+        if isinstance(x[0], TorchLike):
+            v = torch.hstack([x[i].array for i in range(len(x))]).reshape(1,-1)
+        else:
+            v = torch.FloatTensor(x).reshape(1,-1)
+        return TorchLike(v)

From cb2eec7c6791a753d9ed2ca4b6cd77841ad95ed4 Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Mon, 6 Nov 2023 17:16:07 +0100
Subject: [PATCH 07/19] no regression tests working for all backend

---
 src/adam/jax/jax_like.py                     |   1 -
 src/adam/pytorch/torch_like.py               |   9 +-
 tests/test_CasADi_computations_parametric.py | 183 ++++++++-----------
 3 files changed, 80 insertions(+), 113 deletions(-)

diff --git a/src/adam/jax/jax_like.py b/src/adam/jax/jax_like.py
index 74eeae76..5adee423 100644
--- a/src/adam/jax/jax_like.py
+++ b/src/adam/jax/jax_like.py
@@ -194,7 +194,6 @@ def mtimes(x:npt.ArrayLike, y:npt.ArrayLike) -> "JaxLike":
             return jnp.mtimes(jnp.array(x), jnp.array(y))
         else: 
             return jnp.mtimes(jnp.array(x.array), jnp.array(y.array))
-        return JaxLike(-jnp.cross(jnp.array(x), jnp.eye(3), axisa=0, axisb=0))
 
     @staticmethod
     def vertcat(*x) -> "JaxLike":
diff --git a/src/adam/pytorch/torch_like.py b/src/adam/pytorch/torch_like.py
index 1cbf05d7..71a6f15f 100644
--- a/src/adam/pytorch/torch_like.py
+++ b/src/adam/pytorch/torch_like.py
@@ -23,7 +23,7 @@ def __setitem__(self, idx, value: Union["TorchLike", ntp.ArrayLike]) -> "TorchLi
         if type(self) is type(value):
             self.array[idx] = value.array.reshape(self.array[idx].shape)
         else:
-            self.array[idx] = torch.FloatTensor(value)
+            self.array[idx] =torch.tensor(value)
 
     def __getitem__(self, idx):
         """Overrides get item operator"""
@@ -203,13 +203,6 @@ def skew(x: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike":
         )
 
 
-    @staticmethod
-    def skew(x: ntp.ArrayLike, y:np.ArrayLike) -> "TorchLike":
-        if isinstance(x, TorchLike) and isinstance(y, TorchLike):
-            return TorchLike(torch.mtimes(TorchLike(x), TorchLike(y)))
-        else:
-            return TorchLike(torch.mtimes(x.array, y.array))
-
     @staticmethod
     def vertcat(*x: ntp.ArrayLike) -> "TorchLike":
         """
diff --git a/tests/test_CasADi_computations_parametric.py b/tests/test_CasADi_computations_parametric.py
index 9d5abd42..4fe23ad1 100644
--- a/tests/test_CasADi_computations_parametric.py
+++ b/tests/test_CasADi_computations_parametric.py
@@ -7,7 +7,7 @@
 import urdf_parser_py.urdf
 import casadi as cs
 import numpy as np
-# import pytest
+import pytest
 import math 
 from adam.casadi.computations_parametric import KinDynComputationsParametric
 from adam.casadi import KinDynComputations
@@ -77,16 +77,13 @@ def SX2DM(x):
 root_link = "root_link"
 comp = KinDynComputations(model_path, joints_name_list, root_link)
 
-# link_name_list = ['r_upper_leg','r_lower_leg', 'r_hip_1', 'r_hip_2', 'r_ankle_1', 'r_ankle_2','l_upper_leg','l_lower_leg', 'l_hip_1', 'l_hip_2', 'l_ankle_1', 'l_ankle_2', 'l_shoulder_1', 'l_shoulder_2', 'l_shoulder_3', 'l_elbow_1','r_shoulder_1', 'r_shoulder_2', 'r_shoulder_3', 'r_elbow_1']
 link_name_list = ['chest']
 comp_w_hardware = KinDynComputationsParametric(model_path, joints_name_list, link_name_list, root_link)
 original_density = []
 for item in link_name_list: 
     original_density += [ComputeOriginalDensity(comp_w_hardware,item)]
-print("original density", original_density)
 original_length = np.ones(len(link_name_list))
 
-# joint_type = np.zeros(len(joints_name_list))
 
 n_dofs = len(joints_name_list)
 # base pose quantities
@@ -102,103 +99,81 @@ def SX2DM(x):
 s_ = joints_val
 s_dot_ = joints_dot_val
 
-# # def test_mass_matrix():
-M = comp.mass_matrix_fun()
-M_with_hardware = comp_w_hardware.mass_matrix_fun()
-print(M_with_hardware)
-mass_test = SX2DM(M(H_b, s_))
-mass_test_hardware = SX2DM(M_with_hardware(H_b,s_, original_length, original_density))
-# print("mass test hardware", mass_test_hardware)
-print("M=",cs.sumsqr(mass_test - mass_test_hardware))
-    # assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5)
-
-
-# def test_CMM():
-Jcm = comp.centroidal_momentum_matrix_fun()
-Jcm_with_hardware = comp_w_hardware.centroidal_momentum_matrix_fun()
-Jcm_test = SX2DM(Jcm(H_b, s_))
-Jcm_test_hardware = SX2DM(Jcm_with_hardware(H_b,s_, original_length, original_density))
-print("Jcmm=",cs.sumsqr(Jcm_test - Jcm_test_hardware))
-# assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5)
-
-
-# def test_CoM_pos():
-com_f = comp.CoM_position_fun()
-com_with_hardware_f = comp_w_hardware.CoM_position_fun()
-CoM_cs = SX2DM(com_f(H_b, s_))
-CoM_hardware  = SX2DM(com_with_hardware_f(H_b,s_, original_length, original_density)) 
-print("CoM=",cs.sumsqr(CoM_cs - CoM_hardware))
-    # assert CoM_cs - CoM_hardware == pytest.approx(0.0, abs=1e-5)
-
-
-# # def test_total_mass():
-mass = comp.get_total_mass(); 
-mass_hardware_fun = comp_w_hardware.get_total_mass()
-mass_hardware = SX2DM(mass_hardware_fun(original_length, original_density))
-print("mass=",cs.sumsqr(mass - mass_hardware))
-#     # assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5)
-
-# # def test_jacobian():
-J_tot = comp.jacobian_fun("l_sole")
-J_tot_with_hardware = comp_w_hardware.jacobian_fun("l_sole")
-J_test = SX2DM(J_tot(H_b, s_))
-J_test_hardware = SX2DM(J_tot_with_hardware(H_b, s_, original_length, original_density))
-print("J l sole=",cs.sumsqr(J_test- J_test_hardware))
-# assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
-
-
-# def test_jacobian_non_actuated():
-J_tot = comp.jacobian_fun("head")
-J_test = SX2DM(J_tot(H_b, s_))
-J_tot_with_hardware = comp_w_hardware.jacobian_fun("head")
-J_tot_with_hardware_test = SX2DM(J_tot_with_hardware(H_b, s_, original_length, original_density))
-print("J head =",cs.sumsqr(J_test - J_tot_with_hardware_test))    
-    # assert J_test - J_tot_with_hardware_test == pytest.approx(0.0, abs=1e-5)
-
-
-# def test_fk():
-T = comp.forward_kinematics_fun("l_sole")
-H_test = SX2DM(T(H_b, s_))
-T_with_hardware = comp_w_hardware.forward_kinematics_fun("l_sole")
-H_with_hardware_test = SX2DM(T_with_hardware(H_b, s_, original_length, original_density))
-print("fk l sole =",cs.sumsqr(H_test - H_with_hardware_test))    
-    # assert H_with_hardware_test[:3,:3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
-    # assert H_with_hardware_test[:3,3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
-
-
-# # def test_fk_non_actuated():
-# T = comp.forward_kinematics_fun("head")
-H_test = SX2DM(T(H_b, s_))
-T_with_hardware = comp_w_hardware.forward_kinematics_fun("head")
-H_with_hardware_test = SX2DM(T_with_hardware(H_b,s_,original_length, original_density))
-print("fk head =",cs.sumsqr(H_test - H_with_hardware_test))
-    # assert H_with_hardware_test[:3,:3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
-    # assert H_with_hardware_test[:3,3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
-
-
-# def test_bias_force():
-h = comp.bias_force_fun()
-h_test = SX2DM(h(H_b, s_, vb_, s_dot_))
-
-h_with_hardware = comp_w_hardware.bias_force_fun()
-h_with_hardware_test = SX2DM(h_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density))
-print("h=",cs.sumsqr(h_with_hardware_test-h_test))   
-# assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4)
-
-
-# def test_coriolis_term():
-C = comp.coriolis_term_fun()
-C_test = SX2DM(C(H_b, s_, vb_, s_dot_))
-C_with_hardware = comp_w_hardware.coriolis_term_fun()
-C_with_hardware_test = SX2DM(C_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density))
-print("C=",cs.sumsqr(C_test - C_with_hardware_test)) 
-    # assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4)
-
-
-# def test_gravity_term():
-G = comp.gravity_term_fun()
-G_test = SX2DM(G(H_b, s_))
-G_with_hardware = comp_w_hardware.gravity_term_fun()
-G_with_hardware_test = G_with_hardware(H_b,s_, original_length, original_density)
-print("G=",cs.sumsqr(G_test- G_with_hardware_test))
-    # assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4)
+def test_mass_matrix():
+    M = comp.mass_matrix_fun()
+    M_with_hardware = comp_w_hardware.mass_matrix_fun()
+    mass_test = SX2DM(M(H_b, s_))
+    mass_test_hardware = SX2DM(M_with_hardware(H_b,s_, original_length, original_density))
+    assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+def test_CMM():
+    Jcm = comp.centroidal_momentum_matrix_fun()
+    Jcm_with_hardware = comp_w_hardware.centroidal_momentum_matrix_fun()
+    Jcm_test = SX2DM(Jcm(H_b, s_))
+    Jcm_test_hardware = SX2DM(Jcm_with_hardware(H_b,s_, original_length, original_density))
+    assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+def test_CoM_pos():
+    com_f = comp.CoM_position_fun()
+    com_with_hardware_f = comp_w_hardware.CoM_position_fun()
+    CoM_cs = SX2DM(com_f(H_b, s_))
+    CoM_hardware  = SX2DM(com_with_hardware_f(H_b,s_, original_length, original_density)) 
+    assert CoM_cs - CoM_hardware == pytest.approx(0.0, abs=1e-5)
+
+def test_total_mass():
+    mass = comp.get_total_mass(); 
+    mass_hardware_fun = comp_w_hardware.get_total_mass()
+    mass_hardware = SX2DM(mass_hardware_fun(original_length, original_density))
+    assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5)
+
+def test_jacobian():
+    J_tot = comp.jacobian_fun("l_sole")
+    J_tot_with_hardware = comp_w_hardware.jacobian_fun("l_sole")
+    J_test = SX2DM(J_tot(H_b, s_))
+    J_test_hardware = SX2DM(J_tot_with_hardware(H_b, s_, original_length, original_density))
+    assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+def test_jacobian_non_actuated():
+    J_tot = comp.jacobian_fun("head")
+    J_test = SX2DM(J_tot(H_b, s_))
+    J_tot_with_hardware = comp_w_hardware.jacobian_fun("head")
+    J_tot_with_hardware_test = SX2DM(J_tot_with_hardware(H_b, s_, original_length, original_density))
+    assert J_test - J_tot_with_hardware_test == pytest.approx(0.0, abs=1e-5)
+
+def test_fk():
+    T = comp.forward_kinematics_fun("l_sole")
+    H_test = SX2DM(T(H_b, s_))
+    T_with_hardware = comp_w_hardware.forward_kinematics_fun("l_sole")
+    H_with_hardware_test = SX2DM(T_with_hardware(H_b, s_, original_length, original_density))
+    assert H_with_hardware_test[:3,:3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
+    assert H_with_hardware_test[:3,3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+
+def test_fk_non_actuated():
+    T = comp.forward_kinematics_fun("head")
+    H_test = SX2DM(T(H_b, s_))
+    T_with_hardware = comp_w_hardware.forward_kinematics_fun("head")
+    H_with_hardware_test = SX2DM(T_with_hardware(H_b,s_,original_length, original_density))
+    assert H_with_hardware_test[:3,:3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
+    assert H_with_hardware_test[:3,3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+
+def test_bias_force():
+    h = comp.bias_force_fun()
+    h_test = SX2DM(h(H_b, s_, vb_, s_dot_))
+
+    h_with_hardware = comp_w_hardware.bias_force_fun()
+    h_with_hardware_test = SX2DM(h_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density))
+    assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4)
+
+def test_coriolis_term():
+    C = comp.coriolis_term_fun()
+    C_test = SX2DM(C(H_b, s_, vb_, s_dot_))
+    C_with_hardware = comp_w_hardware.coriolis_term_fun()
+    C_with_hardware_test = SX2DM(C_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density))
+    assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4)
+
+def test_gravity_term():
+    G = comp.gravity_term_fun()
+    G_test = SX2DM(G(H_b, s_))
+    G_with_hardware = comp_w_hardware.gravity_term_fun()
+    G_with_hardware_test = G_with_hardware(H_b,s_, original_length, original_density)
+    assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4)

From 6386f7dbe75c28a46fe90943665692fd0e0b37cd Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Mon, 6 Nov 2023 18:08:12 +0100
Subject: [PATCH 08/19] fix tests jax with hardware parameters

---
 src/adam/jax/computations_parametric.py   |  86 +++++++-------
 src/adam/jax/jax_like.py                  |  15 +--
 tests/test_Jax_computations_parametric.py | 138 ++++++++++++++++++++++
 3 files changed, 188 insertions(+), 51 deletions(-)
 create mode 100644 tests/test_Jax_computations_parametric.py

diff --git a/src/adam/jax/computations_parametric.py b/src/adam/jax/computations_parametric.py
index 8bf4246c..6a03d701 100644
--- a/src/adam/jax/computations_parametric.py
+++ b/src/adam/jax/computations_parametric.py
@@ -30,11 +30,13 @@ def __init__(
             joints_name_list (list): list of the actuated joints
             root_link (str, optional): the first link. Defaults to 'root_link'.
         """
-        math = SpatialMath()
+        self.math = SpatialMath()
         self.link_name_list = link_name_list
         self.g = gravity
+        self.urdfstring = urdfstring
+        self.joints_name_list = joints_name_list
 
-    def mass_matrix(self, base_transform: jnp.array, joint_positions: jnp.array,length_multiplier:jnp.array, density: jnp.array):
+    def mass_matrix(self, base_transform: jnp.array, joint_positions: jnp.array,lenght_multiplier:jnp.array, density: jnp.array):
         """Returns the Mass Matrix functions computed the CRBA
 
         Args:
@@ -45,15 +47,15 @@ def mass_matrix(self, base_transform: jnp.array, joint_positions: jnp.array,leng
             M (jax): Mass Matrix
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         [M, _] = self.rbdalgos.crba(base_transform, joint_positions)
         return M.array
 
     def centroidal_momentum_matrix(
-        self, base_transform: jnp.array, joint_positions: jnp.array,length_multiplier:jnp.array, density: jnp.array
+        self, base_transform: jnp.array, joint_positions: jnp.array,lenght_multiplier:jnp.array, density: jnp.array
     ):
         """Returns the Centroidal Momentum Matrix functions computed the CRBA
 
@@ -65,14 +67,14 @@ def centroidal_momentum_matrix(
             Jcc (jnp.array): Centroidal Momentum matrix
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         [_, Jcm] = self.rbdalgos.crba(base_transform, joint_positions)
         return Jcm.array
 
-    def relative_jacobian(self, frame: str, joint_positions: jnp.array,length_multiplier:jnp.array, density: jnp.array):
+    def relative_jacobian(self, frame: str, joint_positions: jnp.array,lenght_multiplier:jnp.array, density: jnp.array):
         """Returns the Jacobian between the root link and a specified frame frames
 
         Args:
@@ -83,14 +85,14 @@ def relative_jacobian(self, frame: str, joint_positions: jnp.array,length_multip
             J (jnp.array): The Jacobian between the root and the frame
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.relative_jacobian(frame, joint_positions).array
 
     def forward_kinematics(
-        self, frame: str, base_transform: jnp.array, joint_positions: jnp.array,length_multiplier:jnp.array, density: jnp.array
+        self, frame: str, base_transform: jnp.array, joint_positions: jnp.array,lenght_multiplier:jnp.array, density: jnp.array
     ):
         """Computes the forward kinematics relative to the specified frame
 
@@ -103,20 +105,20 @@ def forward_kinematics(
             T_fk (jnp.array): The fk represented as Homogenous transformation matrix
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.forward_kinematics(
             frame, base_transform, joint_positions
         ).array
 
-    def forward_kinematics_fun(self, frame,length_multiplier:jnp.array, density: jnp.array):
+    def forward_kinematics_fun(self, frame,lenght_multiplier:jnp.array, density: jnp.array):
         return lambda T, joint_positions: self.forward_kinematics(
             frame, T, joint_positions
         )
 
-    def jacobian(self, frame: str, base_transform, joint_positions,length_multiplier:jnp.array, density: jnp.array):
+    def jacobian(self, frame: str, base_transform, joint_positions,lenght_multiplier:jnp.array, density: jnp.array):
         """Returns the Jacobian relative to the specified frame
 
         Args:
@@ -128,9 +130,9 @@ def jacobian(self, frame: str, base_transform, joint_positions,length_multiplier
             J_tot (jnp.array): The Jacobian relative to the frame
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.jacobian(frame, base_transform, joint_positions).array
 
@@ -140,7 +142,7 @@ def bias_force(
         joint_positions: jnp.array,
         base_velocity: jnp.array,
         s_dot: jnp.array,
-        length_multiplier:jnp.array,
+        lenght_multiplier:jnp.array,
         density: jnp.array
     ) -> jnp.array:
         """Returns the bias force of the floating-base dynamics ejoint_positionsuation,
@@ -156,9 +158,9 @@ def bias_force(
             h (jnp.array): the bias force
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.rnea(
             base_transform, joint_positions, base_velocity, s_dot, self.g
@@ -170,7 +172,7 @@ def coriolis_term(
         joint_positions: jnp.array,
         base_velocity: jnp.array,
         s_dot: jnp.array,
-        length_multiplier:jnp.array,
+        lenght_multiplier:jnp.array,
         density: jnp.array
     ) -> jnp.array:
         """Returns the coriolis term of the floating-base dynamics ejoint_positionsuation,
@@ -186,9 +188,9 @@ def coriolis_term(
             C (jnp.array): the Coriolis term
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.rnea(
             base_transform,
@@ -199,7 +201,7 @@ def coriolis_term(
         ).array.squeeze()
 
     def gravity_term(
-        self, base_transform: jnp.array, joint_positions: jnp.array,length_multiplier:jnp.array, density: jnp.array
+        self, base_transform: jnp.array, joint_positions: jnp.array,lenght_multiplier:jnp.array, density: jnp.array
     ) -> jnp.array:
         """Returns the gravity term of the floating-base dynamics ejoint_positionsuation,
         using a reduced RNEA (no acceleration and external forces)
@@ -212,9 +214,9 @@ def gravity_term(
             G (jnp.array): the gravity term
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.rnea(
             base_transform,
@@ -225,7 +227,7 @@ def gravity_term(
         ).array.squeeze()
 
     def CoM_position(
-        self, base_transform: jnp.array, joint_positions: jnp.array, length_multiplier:jnp.array, density: jnp.array
+        self, base_transform: jnp.array, joint_positions: jnp.array, lenght_multiplier:jnp.array, density: jnp.array
     ) -> jnp.array:
         """Returns the CoM positon
 
@@ -237,22 +239,22 @@ def CoM_position(
             com (jnp.array): The CoM position
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.CoM_position(
             base_transform, joint_positions
         ).array.squeeze()
 
-    def get_total_mass(self,length_multiplier:jnp.array, density: jnp.array) -> float:
+    def get_total_mass(self,lenght_multiplier:jnp.array, density: jnp.array) -> float:
         """Returns the total mass of the robot
 
         Returns:
             mass: The total mass
         """
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.get_total_mass()
diff --git a/src/adam/jax/jax_like.py b/src/adam/jax/jax_like.py
index 5adee423..17edd32b 100644
--- a/src/adam/jax/jax_like.py
+++ b/src/adam/jax/jax_like.py
@@ -190,11 +190,8 @@ def skew(x: Union["JaxLike", npt.ArrayLike]) -> "JaxLike":
 
     @staticmethod
     def mtimes(x:npt.ArrayLike, y:npt.ArrayLike) -> "JaxLike":
-        if  isinstance(x, JaxLike) and isinstance(y, JaxLike):
-            return jnp.mtimes(jnp.array(x), jnp.array(y))
-        else: 
-            return jnp.mtimes(jnp.array(x.array), jnp.array(y.array))
-
+        return x@y
+        
     @staticmethod
     def vertcat(*x) -> "JaxLike":
         """
@@ -202,9 +199,9 @@ def vertcat(*x) -> "JaxLike":
             JaxLike: Vertical concatenation of elements
         """
         if isinstance(x[0], JaxLike):
-            v = jnp.vstack([x[i].array for i in range(len(x))]).reshape(-1, 1)
+            v = jnp.vstack([x[i].array for i in range(len(x))])
         else:
-            v = jnp.vstack([x[i] for i in range(len(x))]).reshape(-1, 1)
+            v = jnp.vstack([x[i] for i in range(len(x))])
         return JaxLike(v)
 
     @staticmethod
@@ -214,7 +211,7 @@ def horzcat(*x) -> "JaxLike":
             JaxLike: Horizontal concatenation of elements
         """
         if isinstance(x[0], JaxLike):
-            v = jnp.hstack([x[i].array for i in range(len(x))]).reshape(1,-1)
+            v = jnp.hstack([x[i].array for i in range(len(x))])
         else:
-            v = jnp.hstack([x[i] for i in range(len(x))]).reshape(1, -1)
+            v = jnp.hstack([x[i] for i in range(len(x))])
         return JaxLike(v)
diff --git a/tests/test_Jax_computations_parametric.py b/tests/test_Jax_computations_parametric.py
new file mode 100644
index 00000000..371bf7de
--- /dev/null
+++ b/tests/test_Jax_computations_parametric.py
@@ -0,0 +1,138 @@
+# 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 logging
+from os import link
+import urdf_parser_py.urdf
+import jax.numpy as jnp
+from jax import config
+import numpy as np
+import pytest
+import math 
+from adam.jax.computations_parametric import KinDynComputationsParametric
+from adam.jax import KinDynComputations
+
+from adam.geometry import utils
+import tempfile
+from git import Repo
+
+np.random.seed(42)
+config.update("jax_enable_x64", True)
+
+# Getting stickbot urdf file
+temp_dir = tempfile.TemporaryDirectory()
+git_url = "https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git"
+Repo.clone_from(git_url, temp_dir.name)
+model_path = temp_dir.name + "/models/stickBot/model.urdf"
+
+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)
+
+logging.basicConfig(level=logging.DEBUG)
+logging.debug("Showing the robot tree.")
+
+
+root_link = "root_link"
+comp = KinDynComputations(model_path, joints_name_list, root_link)
+
+link_name_list = ['chest']
+comp_w_hardware = KinDynComputationsParametric(model_path, joints_name_list, link_name_list, root_link)
+original_density = [628.0724496264945]
+original_length = np.ones(len(link_name_list))
+
+n_dofs = len(joints_name_list)
+# base pose quantities
+xyz = (np.random.rand(3) - 0.5) * 5
+rpy = (np.random.rand(3) - 0.5) * 5
+base_vel = (np.random.rand(6) - 0.5) * 5
+# joints quantitites
+joints_val = (np.random.rand(n_dofs) - 0.5) * 5
+joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5
+
+H_b = utils.H_from_Pos_RPY(xyz, rpy)
+vb_ = base_vel
+s_ = joints_val
+s_dot_ = joints_dot_val
+
+def test_mass_matrix():
+    mass_test = comp.mass_matrix(H_b, s_)
+    mass_test_hardware = np.array(comp_w_hardware.mass_matrix(H_b,s_, original_length, original_density))
+    assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+def test_CMM():
+    Jcm_test = comp.centroidal_momentum_matrix(H_b, s_)
+    Jcm_test_hardware = np.array(comp_w_hardware.centroidal_momentum_matrix(H_b,s_, original_length, original_density))
+    assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+def test_CoM_pos():
+    CoM_test = comp.CoM_position(H_b, s_)
+    CoM_hardware  = np.array(comp_w_hardware.CoM_position(H_b,s_, original_length, original_density)) 
+    assert CoM_test - CoM_hardware == pytest.approx(0.0, abs=1e-5)
+
+def test_total_mass():
+    mass = comp.get_total_mass(); 
+    mass_hardware = comp_w_hardware.get_total_mass(original_length, original_density)
+    assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5)
+
+def test_jacobian():
+    J_test = comp.jacobian("l_sole", H_b, s_)
+    J_test_hardware = np.array(comp_w_hardware.jacobian("l_sole",H_b, s_, original_length, original_density))
+    assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+def test_jacobian_non_actuated():
+    J_test = comp.jacobian("head", H_b, s_)
+    J_test_hardware = np.array(comp_w_hardware.jacobian("head",H_b, s_, original_length, original_density))
+    assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+def test_fk():
+    H_test = comp.forward_kinematics("l_sole", H_b, s_)
+    H_with_hardware_test = np.array(comp_w_hardware.forward_kinematics("l_sole",H_b, s_, original_length, original_density))
+    assert H_with_hardware_test[:3,:3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
+    assert H_with_hardware_test[:3,3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+
+def test_fk_non_actuated():
+    H_test = comp.forward_kinematics("head", H_b, s_)
+    H_with_hardware_test = np.array(comp_w_hardware.forward_kinematics("head",H_b, s_, original_length, original_density))
+    assert H_with_hardware_test[:3,:3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
+    assert H_with_hardware_test[:3,3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+
+def test_bias_force():
+    h_test = comp.bias_force(H_b, s_, vb_, s_dot_)
+    h_with_hardware_test = np.array(comp_w_hardware.bias_force(H_b, s_, vb_, s_dot_, original_length, original_density))
+    assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4)
+
+def test_coriolis_term():
+    C_test = comp.coriolis_term(H_b, s_, vb_, s_dot_)
+    C_with_hardware_test = np.array(comp_w_hardware.coriolis_term(H_b, s_, vb_, s_dot_, original_length, original_density))
+    assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4)
+
+def test_gravity_term():
+    G_test = comp.gravity_term(H_b, s_)
+    G_with_hardware_test = comp_w_hardware.gravity_term(H_b,s_, original_length, original_density)
+    assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4)

From 83fd338ec3121b398add01773efe00b9732d914e Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Mon, 6 Nov 2023 18:48:10 +0100
Subject: [PATCH 09/19] tests for computations with hardware parameters and
 formatted

---
 src/adam/casadi/__init__.py                   |   1 -
 src/adam/casadi/casadi_like.py                |   6 +-
 src/adam/casadi/computations_parametric.py    |  74 +++++--
 src/adam/core/spatial_math.py                 |  35 +--
 src/adam/jax/computations_parametric.py       | 146 +++++++++++--
 src/adam/jax/jax_like.py                      |   6 +-
 .../parametric_factories/parametric_joint.py  |  19 +-
 .../parametric_factories/parametric_link.py   | 150 ++++++++-----
 .../parametric_factories/parametric_model.py  |  43 +++-
 src/adam/numpy/computations_parametric.py     | 184 ++++++++++++----
 src/adam/numpy/numpy_like.py                  |  18 +-
 src/adam/pytorch/computations_parametric.py   | 206 ++++++++++++------
 src/adam/pytorch/torch_like.py                |  17 +-
 tests/test_CasADi_computations_parametric.py  |  88 +++++---
 tests/test_Jax_computations_parametric.py     |  82 +++++--
 tests/test_NumPy_computations_parametric.py   | 181 +++++++++++++++
 tests/test_pytorch_computations_parametric.py | 183 ++++++++++++++++
 17 files changed, 1131 insertions(+), 308 deletions(-)
 create mode 100644 tests/test_NumPy_computations_parametric.py
 create mode 100644 tests/test_pytorch_computations_parametric.py

diff --git a/src/adam/casadi/__init__.py b/src/adam/casadi/__init__.py
index ee587263..b6dc7fb6 100644
--- a/src/adam/casadi/__init__.py
+++ b/src/adam/casadi/__init__.py
@@ -5,4 +5,3 @@
 from .casadi_like import CasadiLike
 from .computations import KinDynComputations
 from .computations_parametric import KinDynComputationsParametric
-
diff --git a/src/adam/casadi/casadi_like.py b/src/adam/casadi/casadi_like.py
index c623c4d0..7de2b875 100644
--- a/src/adam/casadi/casadi_like.py
+++ b/src/adam/casadi/casadi_like.py
@@ -160,9 +160,8 @@ def sin(x: npt.ArrayLike) -> "CasadiLike":
         """
         return CasadiLike(cs.sin(x))
 
-
     @staticmethod
-    def mtimes(x: npt.ArrayLike, y:npt.ArrayLike) -> "CasadiLike":
+    def mtimes(x: npt.ArrayLike, y: npt.ArrayLike) -> "CasadiLike":
         """
         Args:
             x (npt.ArrayLike): angle value
@@ -173,7 +172,7 @@ def mtimes(x: npt.ArrayLike, y:npt.ArrayLike) -> "CasadiLike":
         if isinstance(x, CasadiLike) and isinstance(y, CasadiLike):
             return CasadiLike(cs.mtimes(x.array, y.array))
         else:
-            return CasadiLike(cs.mtimes(x,y))
+            return CasadiLike(cs.mtimes(x, y))
         # return CasadiLike(cs.mtimes(x, y))
 
     @staticmethod
@@ -211,6 +210,7 @@ def vertcat(*x) -> "CasadiLike":
         # Then the list is unpacked with the * operator.
         y = [xi.array if isinstance(xi, CasadiLike) else xi for xi in x]
         return CasadiLike(cs.vertcat(*y))
+
     @staticmethod
     def horzcat(*x) -> "CasadiLike":
         """
diff --git a/src/adam/casadi/computations_parametric.py b/src/adam/casadi/computations_parametric.py
index fe1fb6be..a999dbbf 100644
--- a/src/adam/casadi/computations_parametric.py
+++ b/src/adam/casadi/computations_parametric.py
@@ -19,7 +19,7 @@ def __init__(
         self,
         urdfstring: str,
         joints_name_list: list,
-        link_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")),
@@ -33,8 +33,14 @@ def __init__(
         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)
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math,link_parametric_list=link_name_list,lenght_multiplier=self.length_multiplier, density=self.density)
+        self.length_multiplier = cs.SX.sym("length_multiplier", n_param_link)
+        self.factory = URDFParametricModelFactory(
+            path=urdfstring,
+            math=math,
+            link_parametric_list=link_name_list,
+            lenght_multiplier=self.length_multiplier,
+            density=self.density,
+        )
         model = Model.build(factory=self.factory, joints_name_list=joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=math)
         self.NDoF = self.rbdalgos.NDoF
@@ -50,7 +56,9 @@ def mass_matrix_fun(self) -> cs.Function:
         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)
+        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
@@ -61,7 +69,12 @@ def centroidal_momentum_matrix_fun(self) -> cs.Function:
         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)
+        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
@@ -75,7 +88,12 @@ def forward_kinematics_fun(self, frame: str) -> cs.Function:
         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)
+        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
@@ -89,7 +107,12 @@ def jacobian_fun(self, frame: str) -> cs.Function:
         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)
+        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
@@ -102,7 +125,9 @@ def relative_jacobian_fun(self, frame: str) -> cs.Function:
         """
         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)
+        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
@@ -113,7 +138,12 @@ def CoM_position_fun(self) -> cs.Function:
         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)
+        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,
@@ -127,7 +157,12 @@ def bias_force_fun(self) -> cs.Function:
         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)
+        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,
@@ -142,7 +177,12 @@ def coriolis_term_fun(self) -> cs.Function:
         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)
+        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,
@@ -155,7 +195,9 @@ def gravity_term_fun(self) -> cs.Function:
         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)
+        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
@@ -167,7 +209,9 @@ def forward_kinematics(self, frame, T_b, s) -> cs.Function:
             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)
+        return self.rbdalgos.forward_kinematics(
+            frame, T_b, s, self.length_multiplier, self.density
+        )
 
     def get_total_mass(self) -> float:
         """Returns the total mass of the robot
@@ -176,5 +220,7 @@ def get_total_mass(self) -> float:
             mass: The total mass
         """
         m = self.rbdalgos.get_total_mass()
-        return cs.Function("m", [self.density, self.length_multiplier], [m], self.f_opts)
+        return cs.Function(
+            "m", [self.density, self.length_multiplier], [m], self.f_opts
+        )
         return self.rbdalgos.get_total_mass()
diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py
index e87e8efe..b9200b5f 100644
--- a/src/adam/core/spatial_math.py
+++ b/src/adam/core/spatial_math.py
@@ -129,7 +129,7 @@ def horzcat(x: npt.ArrayLike) -> npt.ArrayLike:
         pass
 
     @abc.abstractmethod
-    def mtimes(x: npt.ArrayLike, y:npt.ArrayLike) -> npt.ArrayLike:
+    def mtimes(x: npt.ArrayLike, y: npt.ArrayLike) -> npt.ArrayLike:
         pass
 
     @abc.abstractmethod
@@ -242,9 +242,9 @@ def H_revolute_joint(
         T = self.factory.eye(4)
         R = self.R_from_RPY(rpy) @ self.R_from_axis_angle(axis, q)
         T[:3, :3] = R
-        T[0,3] = xyz[0]
-        T[1,3] = xyz[1]
-        T[2,3] = xyz[2] 
+        T[0, 3] = xyz[0]
+        T[1, 3] = xyz[1]
+        T[2, 3] = xyz[2]
         return T
 
     def H_prismatic_joint(
@@ -281,9 +281,9 @@ def H_from_Pos_RPY(self, xyz: npt.ArrayLike, rpy: npt.ArrayLike) -> npt.ArrayLik
         """
         T = self.factory.eye(4)
         T[:3, :3] = self.R_from_RPY(rpy)
-        T[0,3] = xyz[0]
-        T[1,3] = xyz[1]
-        T[2,3] = xyz[2] 
+        T[0, 3] = xyz[0]
+        T[1, 3] = xyz[1]
+        T[2, 3] = xyz[2]
         return T
 
     def R_from_RPY(self, rpy: npt.ArrayLike) -> npt.ArrayLike:
@@ -400,18 +400,23 @@ def spatial_inertia(
         IO[:3, :3] = self.factory.eye(3) * mass
         return IO
 
-    def spatial_inertial_with_parameter(self, I, mass, c , rpy):
-    # Returns the 6x6 inertia matrix expressed at the origin of the link (with rotation)"""
-        IO = self.factory.zeros(6,6)
+    def spatial_inertial_with_parameter(self, I, mass, c, rpy):
+        # Returns the 6x6 inertia matrix expressed at the origin of the link (with rotation)"""
+        IO = self.factory.zeros(6, 6)
         Sc = self.skew(c)
-        R = self.factory.zeros(3,3)
-        R_temp = self.R_from_RPY(rpy)    
-        inertia_matrix =self.vertcat(self.horzcat(I.ixx,0.0, 0.0), self.horzcat(0.0, I.iyy, 0.0), self.horzcat(0.0, 0.0, I.izz))
-        IO[3:, 3:] = R_temp@inertia_matrix@R_temp.T + mass * self.mtimes(Sc,Sc.T)
+        R = self.factory.zeros(3, 3)
+        R_temp = self.R_from_RPY(rpy)
+        inertia_matrix = self.vertcat(
+            self.horzcat(I.ixx, 0.0, 0.0),
+            self.horzcat(0.0, I.iyy, 0.0),
+            self.horzcat(0.0, 0.0, I.izz),
+        )
+        IO[3:, 3:] = R_temp @ inertia_matrix @ R_temp.T + mass * self.mtimes(Sc, Sc.T)
         IO[3:, :3] = mass * Sc
         IO[:3, 3:] = mass * Sc.T
-        IO[:3, :3] = self.factory.eye(3)* mass
+        IO[:3, :3] = self.factory.eye(3) * mass
         return IO
+
     def spatial_skew(self, v: npt.ArrayLike) -> npt.ArrayLike:
         """
         Args:
diff --git a/src/adam/jax/computations_parametric.py b/src/adam/jax/computations_parametric.py
index 6a03d701..c1be7b16 100644
--- a/src/adam/jax/computations_parametric.py
+++ b/src/adam/jax/computations_parametric.py
@@ -20,7 +20,7 @@ def __init__(
         self,
         urdfstring: str,
         joints_name_list: list,
-        link_name_list:list,
+        link_name_list: list,
         root_link: str = "root_link",
         gravity: np.array = jnp.array([0, 0, -9.80665, 0, 0, 0]),
     ) -> None:
@@ -36,7 +36,13 @@ def __init__(
         self.urdfstring = urdfstring
         self.joints_name_list = joints_name_list
 
-    def mass_matrix(self, base_transform: jnp.array, joint_positions: jnp.array,lenght_multiplier:jnp.array, density: jnp.array):
+    def mass_matrix(
+        self,
+        base_transform: jnp.array,
+        joint_positions: jnp.array,
+        lenght_multiplier: jnp.array,
+        density: jnp.array,
+    ):
         """Returns the Mass Matrix functions computed the CRBA
 
         Args:
@@ -47,7 +53,13 @@ def mass_matrix(self, base_transform: jnp.array, joint_positions: jnp.array,leng
             M (jax): Mass Matrix
         """
 
-        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
@@ -55,7 +67,11 @@ def mass_matrix(self, base_transform: jnp.array, joint_positions: jnp.array,leng
         return M.array
 
     def centroidal_momentum_matrix(
-        self, base_transform: jnp.array, joint_positions: jnp.array,lenght_multiplier:jnp.array, density: jnp.array
+        self,
+        base_transform: jnp.array,
+        joint_positions: jnp.array,
+        lenght_multiplier: jnp.array,
+        density: jnp.array,
     ):
         """Returns the Centroidal Momentum Matrix functions computed the CRBA
 
@@ -67,14 +83,26 @@ def centroidal_momentum_matrix(
             Jcc (jnp.array): Centroidal Momentum matrix
         """
 
-        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         [_, Jcm] = self.rbdalgos.crba(base_transform, joint_positions)
         return Jcm.array
 
-    def relative_jacobian(self, frame: str, joint_positions: jnp.array,lenght_multiplier:jnp.array, density: jnp.array):
+    def relative_jacobian(
+        self,
+        frame: str,
+        joint_positions: jnp.array,
+        lenght_multiplier: jnp.array,
+        density: jnp.array,
+    ):
         """Returns the Jacobian between the root link and a specified frame frames
 
         Args:
@@ -85,14 +113,25 @@ def relative_jacobian(self, frame: str, joint_positions: jnp.array,lenght_multip
             J (jnp.array): The Jacobian between the root and the frame
         """
 
-        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.relative_jacobian(frame, joint_positions).array
 
     def forward_kinematics(
-        self, frame: str, base_transform: jnp.array, joint_positions: jnp.array,lenght_multiplier:jnp.array, density: jnp.array
+        self,
+        frame: str,
+        base_transform: jnp.array,
+        joint_positions: jnp.array,
+        lenght_multiplier: jnp.array,
+        density: jnp.array,
     ):
         """Computes the forward kinematics relative to the specified frame
 
@@ -105,7 +144,13 @@ def forward_kinematics(
             T_fk (jnp.array): The fk represented as Homogenous transformation matrix
         """
 
-        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
@@ -113,12 +158,21 @@ def forward_kinematics(
             frame, base_transform, joint_positions
         ).array
 
-    def forward_kinematics_fun(self, frame,lenght_multiplier:jnp.array, density: jnp.array):
+    def forward_kinematics_fun(
+        self, frame, lenght_multiplier: jnp.array, density: jnp.array
+    ):
         return lambda T, joint_positions: self.forward_kinematics(
             frame, T, joint_positions
         )
 
-    def jacobian(self, frame: str, base_transform, joint_positions,lenght_multiplier:jnp.array, density: jnp.array):
+    def jacobian(
+        self,
+        frame: str,
+        base_transform,
+        joint_positions,
+        lenght_multiplier: jnp.array,
+        density: jnp.array,
+    ):
         """Returns the Jacobian relative to the specified frame
 
         Args:
@@ -130,7 +184,13 @@ def jacobian(self, frame: str, base_transform, joint_positions,lenght_multiplier
             J_tot (jnp.array): The Jacobian relative to the frame
         """
 
-        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
@@ -142,8 +202,8 @@ def bias_force(
         joint_positions: jnp.array,
         base_velocity: jnp.array,
         s_dot: jnp.array,
-        lenght_multiplier:jnp.array,
-        density: jnp.array
+        lenght_multiplier: jnp.array,
+        density: jnp.array,
     ) -> jnp.array:
         """Returns the bias force of the floating-base dynamics ejoint_positionsuation,
         using a reduced RNEA (no acceleration and external forces)
@@ -158,7 +218,13 @@ def bias_force(
             h (jnp.array): the bias force
         """
 
-        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
@@ -172,8 +238,8 @@ def coriolis_term(
         joint_positions: jnp.array,
         base_velocity: jnp.array,
         s_dot: jnp.array,
-        lenght_multiplier:jnp.array,
-        density: jnp.array
+        lenght_multiplier: jnp.array,
+        density: jnp.array,
     ) -> jnp.array:
         """Returns the coriolis term of the floating-base dynamics ejoint_positionsuation,
         using a reduced RNEA (no acceleration and external forces)
@@ -188,7 +254,13 @@ def coriolis_term(
             C (jnp.array): the Coriolis term
         """
 
-        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
@@ -201,7 +273,11 @@ def coriolis_term(
         ).array.squeeze()
 
     def gravity_term(
-        self, base_transform: jnp.array, joint_positions: jnp.array,lenght_multiplier:jnp.array, density: jnp.array
+        self,
+        base_transform: jnp.array,
+        joint_positions: jnp.array,
+        lenght_multiplier: jnp.array,
+        density: jnp.array,
     ) -> jnp.array:
         """Returns the gravity term of the floating-base dynamics ejoint_positionsuation,
         using a reduced RNEA (no acceleration and external forces)
@@ -214,7 +290,13 @@ def gravity_term(
             G (jnp.array): the gravity term
         """
 
-        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
@@ -227,7 +309,11 @@ def gravity_term(
         ).array.squeeze()
 
     def CoM_position(
-        self, base_transform: jnp.array, joint_positions: jnp.array, lenght_multiplier:jnp.array, density: jnp.array
+        self,
+        base_transform: jnp.array,
+        joint_positions: jnp.array,
+        lenght_multiplier: jnp.array,
+        density: jnp.array,
     ) -> jnp.array:
         """Returns the CoM positon
 
@@ -239,7 +325,13 @@ def CoM_position(
             com (jnp.array): The CoM position
         """
 
-        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
@@ -247,13 +339,19 @@ def CoM_position(
             base_transform, joint_positions
         ).array.squeeze()
 
-    def get_total_mass(self,lenght_multiplier:jnp.array, density: jnp.array) -> float:
+    def get_total_mass(self, lenght_multiplier: jnp.array, density: jnp.array) -> float:
         """Returns the total mass of the robot
 
         Returns:
             mass: The total mass
         """
-        factory = URDFParametricModelFactory(path=self.urdfstring, math=self.math, link_parametric_list=self.link_name_list, lenght_multiplier= lenght_multiplier, density=density)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
diff --git a/src/adam/jax/jax_like.py b/src/adam/jax/jax_like.py
index 17edd32b..bb88f457 100644
--- a/src/adam/jax/jax_like.py
+++ b/src/adam/jax/jax_like.py
@@ -189,9 +189,9 @@ def skew(x: Union["JaxLike", npt.ArrayLike]) -> "JaxLike":
         return JaxLike(-jnp.cross(jnp.array(x), jnp.eye(3), axisa=0, axisb=0))
 
     @staticmethod
-    def mtimes(x:npt.ArrayLike, y:npt.ArrayLike) -> "JaxLike":
-        return x@y
-        
+    def mtimes(x: npt.ArrayLike, y: npt.ArrayLike) -> "JaxLike":
+        return x @ y
+
     @staticmethod
     def vertcat(*x) -> "JaxLike":
         """
diff --git a/src/adam/model/parametric_factories/parametric_joint.py b/src/adam/model/parametric_factories/parametric_joint.py
index d6b5162a..88583a48 100644
--- a/src/adam/model/parametric_factories/parametric_joint.py
+++ b/src/adam/model/parametric_factories/parametric_joint.py
@@ -7,6 +7,7 @@
 from adam.model import Joint
 from adam.model.parametric_factories.parametric_link import ParametricLink
 
+
 class ParmetricJoint(Joint):
     """Parametric Joint class"""
 
@@ -14,7 +15,7 @@ def __init__(
         self,
         joint: urdf_parser_py.urdf.Joint,
         math: SpatialMath,
-        parent_link:ParametricLink,
+        parent_link: ParametricLink,
         idx: Union[int, None] = None,
     ) -> None:
         self.math = math
@@ -27,14 +28,15 @@ def __init__(
         self.limit = joint.limit
         self.idx = idx
         self.joint = joint
-        joint_offset = self.parent_parametric.compute_joint_offset(joint, self.parent_parametric.link_offset)
+        joint_offset = self.parent_parametric.compute_joint_offset(
+            joint, self.parent_parametric.link_offset
+        )
         self.offset = joint_offset
         self.origin = self.modify(self.parent_parametric.link_offset)
 
-
     def modify(self, parent_joint_offset):
         length = self.parent_parametric.get_principal_lenght_parametric()
-        # Ack for avoiding depending on casadi 
+        # Ack for avoiding depending on casadi
         vo = self.parent_parametric.origin[2]
         xyz_rpy = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
         xyz_rpy[0] = self.joint.origin.xyz[0]
@@ -43,11 +45,11 @@ def modify(self, parent_joint_offset):
         xyz_rpy[3] = self.joint.origin.rpy[0]
         xyz_rpy[4] = self.joint.origin.rpy[1]
         xyz_rpy[5] = self.joint.origin.rpy[2]
-        
-        if(xyz_rpy[2]<0): 
-            xyz_rpy[2] = -length +parent_joint_offset - self.offset   
+
+        if xyz_rpy[2] < 0:
+            xyz_rpy[2] = -length + parent_joint_offset - self.offset
         else:
-            xyz_rpy[2] = vo+ length/2 - self.offset
+            xyz_rpy[2] = vo + length / 2 - self.offset
         return xyz_rpy
 
     def homogeneous(self, q: npt.ArrayLike) -> npt.ArrayLike:
@@ -59,7 +61,6 @@ def homogeneous(self, q: npt.ArrayLike) -> npt.ArrayLike:
             npt.ArrayLike: the homogenous transform of a joint, given q
         """
 
-
         o = self.math.factory.zeros(3)
         o[0] = self.origin[0]
         o[1] = self.origin[1]
diff --git a/src/adam/model/parametric_factories/parametric_link.py b/src/adam/model/parametric_factories/parametric_link.py
index d6f87351..a6cbc1f3 100644
--- a/src/adam/model/parametric_factories/parametric_link.py
+++ b/src/adam/model/parametric_factories/parametric_link.py
@@ -8,7 +8,8 @@
 import math
 from adam.model.abc_factories import Inertial
 
-class I_parametric():
+
+class I_parametric:
     def __init__(self) -> None:
         self.ixx = 0.0
         self.ixy = 0.0
@@ -17,22 +18,33 @@ def __init__(self) -> None:
         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):
+    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
@@ -41,67 +53,75 @@ def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath, length_mul
         self.geometry_type, self.visual_data = self.get_geometry(self.visuals)
         self.link = link
         # self.parent = self.link.parent
-        self.link_offset = self.compute_offset()        
+        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()
-      
+
         self.inertial = Inertial(self.mass)
         self.inertial.mass = self.mass
         self.inertial.inertia = self.I
         self.inertial.origin = self.origin
 
-    def get_principal_lenght(self):  
-        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
+    def get_principal_lenght(self):
+        xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
         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): 
+            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]
+        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 
+        return v_l
+
+    def get_principal_lenght_parametric(self):
 
-    def get_principal_lenght_parametric(self): 
-        
-        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
+        xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
         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): 
+            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]
+        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 
+        return v_l
 
-    def compute_offset(self): 
-        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy] 
-        v_l=  self.get_principal_lenght()
+    def compute_offset(self):
+        xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
+        v_l = self.get_principal_lenght()
         v_o = xyz_rpy[2]
-        if(v_o<0):
-            link_offset = v_l/2 + v_o
+        if v_o < 0:
+            link_offset = v_l / 2 + v_o
         else:
-            link_offset = (v_o - v_l/2)
+            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 
-        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy]  
-        v_l= self.get_principal_lenght()
+
+    def compute_joint_offset(self, joint_i, parent_offset):
+        # Taking the principal direction i.e. the length
+        xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
+        v_l = self.get_principal_lenght()
         j_0 = joint_i.origin.xyz[2]
         v_o = xyz_rpy[2]
-        if(j_0<0):
-            joint_offset_temp = -(v_l + j_0-parent_offset)
+        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
@@ -120,19 +140,20 @@ def get_geometry(visual_obj):
             return (Geometry.SPHERE, visual_obj.geometry)
 
     """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 = [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
             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
-            visual_data_new[1] = self.visual_data.radius #* self.length_multiplier[1]
+            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
@@ -141,6 +162,7 @@ def compute_volume(self):
         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
@@ -148,19 +170,19 @@ def compute_mass(self):
         return mass
 
     def modify_origin(self):
-        origin = [0.0,0.0,0.0,0.0,0.0,0.0]
-        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy] 
-        v_o = xyz_rpy[2] 
+        origin = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
+        v_o = xyz_rpy[2]
         length = self.get_principal_lenght_parametric()
-        if(v_o<0):
-            origin[2] = self.link_offset-length/2
+        if v_o < 0:
+            origin[2] = self.link_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.link_offset
+            origin[2] = length / 2 + self.link_offset
             origin[0] = xyz_rpy[0]
             origin[1] = xyz_rpy[1]
             origin[3] = xyz_rpy[3]
@@ -168,37 +190,49 @@ def modify_origin(self):
             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 
+            origin = xyz_rpy
         return origin
 
     def compute_inertia_parametric(self):
         I = I_parametric()
-        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy] 
+        xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
         """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
+            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
+                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):
+            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):
+            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.ixx = 2 * self.mass * self.visual_data_new ** 2 / 5
             I.iyy = I.ixx
             I.izz = I.ixx
         return I
diff --git a/src/adam/model/parametric_factories/parametric_model.py b/src/adam/model/parametric_factories/parametric_model.py
index e5cf0342..5cb19203 100644
--- a/src/adam/model/parametric_factories/parametric_model.py
+++ b/src/adam/model/parametric_factories/parametric_model.py
@@ -15,7 +15,14 @@ class URDFParametricModelFactory(ModelFactory):
         ModelFactory: the Model factory
     """
 
-    def __init__(self, path: str, math: SpatialMath, link_parametric_list:List, lenght_multiplier, density):
+    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)
@@ -25,7 +32,7 @@ def __init__(self, path: str, math: SpatialMath, link_parametric_list:List, leng
         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
+        self.density = density
 
     def get_joints(self) -> List[Joint]:
         """
@@ -58,10 +65,15 @@ def build_joint(self, joint: urdf_parser_py.urdf.Joint) -> Joint:
         Returns:
             StdJoint: our joint representation
         """
-        if(joint.parent in self.link_parametric_list): 
+        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 = ParametricLink(link_parent, self.math,self.lenght_multiplier[index_link], self.density[index_link]) 
+            parent_link_parametric = ParametricLink(
+                link_parent,
+                self.math,
+                self.lenght_multiplier[index_link],
+                self.density[index_link],
+            )
             return ParmetricJoint(joint, self.math, parent_link_parametric)
 
         return StdJoint(joint, self.math)
@@ -74,15 +86,24 @@ def build_link(self, link: urdf_parser_py.urdf.Link) -> Link:
         Returns:
             Link: our link representation
         """
-        if(link.name in self.link_parametric_list):
-            index_link = self.link_parametric_list.index(link.name) 
-            return ParametricLink(link, self.math, self.lenght_multiplier[index_link], self.density[index_link])
+        if link.name in self.link_parametric_list:
+            index_link = self.link_parametric_list.index(link.name)
+            return ParametricLink(
+                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):
+
+    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]
+        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
+            return None
diff --git a/src/adam/numpy/computations_parametric.py b/src/adam/numpy/computations_parametric.py
index 4b9a7790..d53de5fe 100644
--- a/src/adam/numpy/computations_parametric.py
+++ b/src/adam/numpy/computations_parametric.py
@@ -18,7 +18,7 @@ def __init__(
         self,
         urdfstring: str,
         joints_name_list: list,
-        link_name_list:list,
+        link_name_list: list,
         root_link: str = "root_link",
         gravity: np.array = np.array([0, 0, -9.80665, 0, 0, 0]),
     ) -> None:
@@ -29,11 +29,17 @@ def __init__(
             root_link (str, optional): the first link. Defaults to 'root_link'.
         """
         self.link_name_list = link_name_list
-        math = SpatialMath()
+        self.math = SpatialMath()
         self.g = gravity
+        self.urdfstring = urdfstring
+        self.joints_name_list = joints_name_list
 
     def mass_matrix(
-        self, base_transform: np.ndarray, joint_positions: np.ndarray, length_multiplier :np.ndarray, density: np.ndarray
+        self,
+        base_transform: np.ndarray,
+        joint_positions: np.ndarray,
+        lenght_multiplier: np.ndarray,
+        density: np.ndarray,
     ) -> np.ndarray:
         """Returns the Mass Matrix functions computed the CRBA
 
@@ -45,15 +51,25 @@ def mass_matrix(
             M (np.ndarray): Mass Matrix
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = model.NDoF
         [M, _] = self.rbdalgos.crba(base_transform, joint_positions)
         return M.array
 
     def centroidal_momentum_matrix(
-        self, base_transform: np.ndarray, s: np.ndarray,length_multiplier :np.ndarray, density: np.ndarray
+        self,
+        base_transform: np.ndarray,
+        s: np.ndarray,
+        lenght_multiplier: np.ndarray,
+        density: np.ndarray,
     ) -> np.ndarray:
         """Returns the Centroidal Momentum Matrix functions computed the CRBA
 
@@ -65,15 +81,26 @@ def centroidal_momentum_matrix(
             Jcc (np.ndarray): Centroidal Momentum matrix
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = model.NDoF
         [_, Jcm] = self.rbdalgos.crba(base_transform, s)
         return Jcm.array
 
     def forward_kinematics(
-        self, frame: str, base_transform: np.ndarray, joint_positions: np.ndarray,length_multiplier :np.ndarray, density: np.ndarray
+        self,
+        frame: str,
+        base_transform: np.ndarray,
+        joint_positions: np.ndarray,
+        lenght_multiplier: np.ndarray,
+        density: np.ndarray,
     ) -> np.ndarray:
         """Computes the forward kinematics relative to the specified frame
 
@@ -86,16 +113,27 @@ def forward_kinematics(
             T_fk (np.ndarray): The fk represented as Homogenous transformation matrix
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = model.NDoF
         return self.rbdalgos.forward_kinematics(
             frame, base_transform, joint_positions
         ).array.squeeze()
 
     def jacobian(
-        self, frame: str, base_transform: np.ndarray, joint_positions: np.ndarray,length_multiplier :np.ndarray, density: np.ndarray
+        self,
+        frame: str,
+        base_transform: np.ndarray,
+        joint_positions: np.ndarray,
+        lenght_multiplier: np.ndarray,
+        density: np.ndarray,
     ) -> np.ndarray:
         """Returns the Jacobian relative to the specified frame
 
@@ -108,15 +146,27 @@ def jacobian(
             J_tot (np.ndarray): The Jacobian relative to the frame
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = model.NDoF
         return self.rbdalgos.jacobian(
             frame, base_transform, joint_positions
         ).array.squeeze()
 
-    def relative_jacobian(self, frame: str, joint_positions: np.ndarray,length_multiplier :np.ndarray, density: np.ndarray) -> np.ndarray:
+    def relative_jacobian(
+        self,
+        frame: str,
+        joint_positions: np.ndarray,
+        lenght_multiplier: np.ndarray,
+        density: np.ndarray,
+    ) -> np.ndarray:
         """Returns the Jacobian between the root link and a specified frame frames
 
         Args:
@@ -127,14 +177,24 @@ def relative_jacobian(self, frame: str, joint_positions: np.ndarray,length_multi
             J (np.ndarray): The Jacobian between the root and the frame
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = model.NDoF
         return self.rbdalgos.relative_jacobian(frame, joint_positions).array
 
     def CoM_position(
-        self, base_transform: np.ndarray, joint_positions: np.ndarray,length_multiplier :np.ndarray, density: np.ndarray
+        self,
+        base_transform: np.ndarray,
+        joint_positions: np.ndarray,
+        lenght_multiplier: np.ndarray,
+        density: np.ndarray,
     ) -> np.ndarray:
         """Returns the CoM positon
 
@@ -146,9 +206,15 @@ def CoM_position(
             CoM (np.ndarray): The CoM position
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = model.NDoF
         return self.rbdalgos.CoM_position(
             base_transform, joint_positions
@@ -160,8 +226,8 @@ def bias_force(
         joint_positions: np.ndarray,
         base_velocity: np.ndarray,
         joint_velocities: np.ndarray,
-        length_multiplier :np.ndarray,
-         density: np.ndarray
+        lenght_multiplier: np.ndarray,
+        density: np.ndarray,
     ) -> np.ndarray:
         """Returns the bias force of the floating-base dynamics equation,
         using a reduced RNEA (no acceleration and external forces)
@@ -176,9 +242,15 @@ def bias_force(
             h (np.ndarray): the bias force
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = model.NDoF
         return self.rbdalgos.rnea(
             base_transform,
@@ -194,8 +266,8 @@ def coriolis_term(
         joint_positions: np.ndarray,
         base_velocity: np.ndarray,
         joint_velocities: np.ndarray,
-        length_multiplier :np.ndarray,
-        density: np.ndarray
+        lenght_multiplier: np.ndarray,
+        density: np.ndarray,
     ) -> np.ndarray:
         """Returns the coriolis term of the floating-base dynamics equation,
         using a reduced RNEA (no acceleration and external forces)
@@ -210,9 +282,15 @@ def coriolis_term(
             C (np.ndarray): the Coriolis term
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = model.NDoF
         # set in the bias force computation the gravity term to zero
         return self.rbdalgos.rnea(
@@ -224,7 +302,11 @@ def coriolis_term(
         ).array.squeeze()
 
     def gravity_term(
-        self, base_transform: np.ndarray, joint_positions: np.ndarray,length_multiplier :np.ndarray, density: np.ndarray
+        self,
+        base_transform: np.ndarray,
+        joint_positions: np.ndarray,
+        lenght_multiplier: np.ndarray,
+        density: np.ndarray,
     ) -> np.ndarray:
         """Returns the gravity term of the floating-base dynamics equation,
         using a reduced RNEA (no acceleration and external forces)
@@ -237,9 +319,15 @@ def gravity_term(
             G (np.ndarray): the gravity term
         """
 
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = model.NDoF
         return self.rbdalgos.rnea(
             base_transform,
@@ -249,14 +337,22 @@ def gravity_term(
             self.g,
         ).array.squeeze()
 
-    def get_total_mass(self,length_multiplier :np.ndarray, density: np.ndarray) -> float:
+    def get_total_mass(
+        self, lenght_multiplier: np.ndarray, density: np.ndarray
+    ) -> float:
         """Returns the total mass of the robot
 
         Returns:
             mass: The total mass
         """
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density)
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = model.NDoF
         return self.rbdalgos.get_total_mass()
diff --git a/src/adam/numpy/numpy_like.py b/src/adam/numpy/numpy_like.py
index c30afb8b..4c77ddb0 100644
--- a/src/adam/numpy/numpy_like.py
+++ b/src/adam/numpy/numpy_like.py
@@ -150,9 +150,9 @@ def sin(x: npt.ArrayLike) -> "NumpyLike":
             NumpyLike: sin value of x
         """
         return NumpyLike(np.sin(x))
-    
+
     @staticmethod
-    def mtimes(x: npt.ArrayLike, y:npt.ArrayLike) -> "NumpyLike":
+    def mtimes(x: npt.ArrayLike, y: npt.ArrayLike) -> "NumpyLike":
         """
         Args:
             x (npt.ArrayLike): angle value
@@ -161,12 +161,11 @@ def mtimes(x: npt.ArrayLike, y:npt.ArrayLike) -> "NumpyLike":
             CasadiLike: the sin value of x
         """
         if isinstance(x, NumpyLike) and isinstance(y, NumpyLike):
-            return NumpyLike((x.array@y.array))
+            return NumpyLike((x.array @ y.array))
         else:
-            return NumpyLike(x@y)
+            return NumpyLike(x @ y)
         # return CasadiLike(cs.mtimes(x, y))
 
-
     @staticmethod
     def cos(x: npt.ArrayLike) -> "NumpyLike":
         """
@@ -199,12 +198,11 @@ def vertcat(*x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike":
             NumpyLike: vertical concatenation of x
         """
         if isinstance(x[0], NumpyLike):
-            v = np.vstack([x[i].array for i in range(len(x))]).reshape(-1, 1)
+            v = np.vstack([x[i].array for i in range(len(x))])
         else:
-            v = np.vstack([x[i] for i in range(len(x))]).reshape(-1, 1)
+            v = np.vstack([x[i] for i in range(len(x))])
         return NumpyLike(v)
 
-    
     @staticmethod
     def horzcat(*x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike":
         """
@@ -212,9 +210,9 @@ def horzcat(*x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike":
             NumpyLike: horrizontal concatenation of x
         """
         if isinstance(x[0], NumpyLike):
-            v = np.hstack([x[i].array for i in range(len(x))]).reshape(1, -1)
+            v = np.hstack([x[i].array for i in range(len(x))])
         else:
-            v = np.hstack([x[i] for i in range(len(x))]).reshape(1, -1)
+            v = np.hstack([x[i] for i in range(len(x))])
         return NumpyLike(v)
 
     @staticmethod
diff --git a/src/adam/pytorch/computations_parametric.py b/src/adam/pytorch/computations_parametric.py
index bae03145..c3007e1d 100644
--- a/src/adam/pytorch/computations_parametric.py
+++ b/src/adam/pytorch/computations_parametric.py
@@ -10,7 +10,7 @@
 from adam.pytorch.torch_like import SpatialMath
 
 
-class KinDynComputations:
+class KinDynComputationsParametric:
     """This is a small class that retrieves robot quantities using Pytorch
     in mixed representation, for Floating Base systems - as humanoid robots.
     """
@@ -19,7 +19,7 @@ def __init__(
         self,
         urdfstring: str,
         joints_name_list: list,
-        link_name_list:list,
+        link_name_list: list,
         root_link: str = "root_link",
         gravity: np.array = torch.FloatTensor([0, 0, -9.80665, 0, 0, 0]),
     ) -> None:
@@ -29,10 +29,18 @@ def __init__(
             joints_name_list (list): list of the actuated joints
             root_link (str, optional): the first link. Defaults to 'root_link'.
         """
+        self.math = SpatialMath()
         self.g = gravity
+        self.link_name_list = link_name_list
+        self.joints_name_list = joints_name_list
+        self.urdfstring = urdfstring
 
     def mass_matrix(
-        self, base_transform: torch.Tensor, s: torch.Tensor, length_multiplier:torch.Tensor, density:torch.Tensor
+        self,
+        base_transform: torch.Tensor,
+        s: torch.Tensor,
+        lenght_multiplier: torch.Tensor,
+        density: torch.Tensor,
     ) -> torch.Tensor:
         """Returns the Mass Matrix functions computed the CRBA
 
@@ -44,17 +52,25 @@ def mass_matrix(
             M (torch.tensor): Mass Matrix
         """
 
-        math = SpatialMath()
-        self.link_name_list = link_name_list
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density )
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         [M, _] = self.rbdalgos.crba(base_transform, s)
         return M.array
 
     def centroidal_momentum_matrix(
-        self, base_transform: torch.Tensor, s: torch.Tensor, length_multiplier:torch.Tensor, density:torch.Tensor
+        self,
+        base_transform: torch.Tensor,
+        s: torch.Tensor,
+        lenght_multiplier: torch.Tensor,
+        density: torch.Tensor,
     ) -> torch.Tensor:
         """Returns the Centroidal Momentum Matrix functions computed the CRBA
 
@@ -66,17 +82,26 @@ def centroidal_momentum_matrix(
             Jcc (torch.tensor): Centroidal Momentum matrix
         """
 
-        math = SpatialMath()
-        self.link_name_list = link_name_list
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density )
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         [_, Jcm] = self.rbdalgos.crba(base_transform, s)
         return Jcm.array
 
     def forward_kinematics(
-        self, frame, base_transform: torch.Tensor, s: torch.Tensor, length_multiplier:torch.Tensor, density:torch.Tensor
+        self,
+        frame,
+        base_transform: torch.Tensor,
+        s: torch.Tensor,
+        lenght_multiplier: torch.Tensor,
+        density: torch.Tensor,
     ) -> torch.Tensor:
         """Computes the forward kinematics relative to the specified frame
 
@@ -89,11 +114,15 @@ def forward_kinematics(
             T_fk (torch.tensor): The fk represented as Homogenous transformation matrix
         """
 
-        math = SpatialMath()
-        self.link_name_list = link_name_list
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density )
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         return (
             self.rbdalgos.forward_kinematics(
@@ -102,7 +131,12 @@ def forward_kinematics(
         ).array
 
     def jacobian(
-        self, frame: str, base_transform: torch.Tensor, joint_positions: torch.Tensor, length_multiplier:torch.Tensor, density:torch.Tensor
+        self,
+        frame: str,
+        base_transform: torch.Tensor,
+        joint_positions: torch.Tensor,
+        lenght_multiplier: torch.Tensor,
+        density: torch.Tensor,
     ) -> torch.Tensor:
         """Returns the Jacobian relative to the specified frame
 
@@ -115,15 +149,25 @@ def jacobian(
             J_tot (torch.tensor): The Jacobian relative to the frame
         """
 
-        math = SpatialMath()
-        self.link_name_list = link_name_list
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density )
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.jacobian(frame, base_transform, joint_positions).array
 
-    def relative_jacobian(self, frame, joint_positions: torch.Tensor, length_multiplier:torch.Tensor, density:torch.Tensor) -> torch.Tensor:
+    def relative_jacobian(
+        self,
+        frame,
+        joint_positions: torch.Tensor,
+        lenght_multiplier: torch.Tensor,
+        density: torch.Tensor,
+    ) -> torch.Tensor:
         """Returns the Jacobian between the root link and a specified frame frames
 
         Args:
@@ -134,16 +178,24 @@ def relative_jacobian(self, frame, joint_positions: torch.Tensor, length_multipl
             J (torch.tensor): The Jacobian between the root and the frame
         """
 
-        math = SpatialMath()
-        self.link_name_list = link_name_list
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density )
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.relative_jacobian(frame, joint_positions).array
 
     def CoM_position(
-        self, base_transform: torch.Tensor, joint_positions: torch.Tensor, length_multiplier:torch.Tensor, density:torch.Tensor
+        self,
+        base_transform: torch.Tensor,
+        joint_positions: torch.Tensor,
+        lenght_multiplier: torch.Tensor,
+        density: torch.Tensor,
     ) -> torch.Tensor:
         """Returns the CoM positon
 
@@ -155,11 +207,15 @@ def CoM_position(
             com (torch.tensor): The CoM position
         """
 
-        math = SpatialMath()
-        self.link_name_list = link_name_list
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density )
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.CoM_position(
             base_transform, joint_positions
@@ -171,8 +227,8 @@ def bias_force(
         s: torch.Tensor,
         base_velocity: torch.Tensor,
         joint_velocities: torch.Tensor,
-        length_multiplier:torch.Tensor,
-        density:torch.Tensor
+        lenght_multiplier: torch.Tensor,
+        density: torch.Tensor,
     ) -> torch.Tensor:
         """Returns the bias force of the floating-base dynamics ejoint_positionsuation,
         using a reduced RNEA (no acceleration and external forces)
@@ -187,11 +243,15 @@ def bias_force(
             h (torch.tensor): the bias force
         """
 
-        math = SpatialMath()
-        self.link_name_list = link_name_list
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density )
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.rnea(
             base_transform,
@@ -207,8 +267,8 @@ def coriolis_term(
         joint_positions: torch.Tensor,
         base_velocity: torch.Tensor,
         joint_velocities: torch.Tensor,
-        length_multiplier:torch.Tensor,
-        density:torch.Tensor
+        lenght_multiplier: torch.Tensor,
+        density: torch.Tensor,
     ) -> torch.Tensor:
         """Returns the coriolis term of the floating-base dynamics ejoint_positionsuation,
         using a reduced RNEA (no acceleration and external forces)
@@ -223,11 +283,15 @@ def coriolis_term(
             C (torch.tensor): the Coriolis term
         """
 
-        math = SpatialMath()
-        self.link_name_list = link_name_list
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density )
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         # set in the bias force computation the gravity term to zero
         return self.rbdalgos.rnea(
@@ -239,7 +303,11 @@ def coriolis_term(
         ).array.squeeze()
 
     def gravity_term(
-        self, base_transform: torch.Tensor, base_positions: torch.Tensor, length_multiplier:torch.Tensor, density:torch.Tensor
+        self,
+        base_transform: torch.Tensor,
+        base_positions: torch.Tensor,
+        lenght_multiplier: torch.Tensor,
+        density: torch.Tensor,
     ) -> torch.Tensor:
         """Returns the gravity term of the floating-base dynamics ejoint_positionsuation,
         using a reduced RNEA (no acceleration and external forces)
@@ -251,11 +319,15 @@ def gravity_term(
         Returns:
             G (torch.tensor): the gravity term
         """
-        math = SpatialMath()
-        self.link_name_list = link_name_list
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density )
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.rnea(
             base_transform,
@@ -265,16 +337,22 @@ def gravity_term(
             torch.FloatTensor(self.g),
         ).array.squeeze()
 
-    def get_total_mass(self, length_multiplier:torch.Tensor, density:torch.Tensor) -> float:
+    def get_total_mass(
+        self, lenght_multiplier: torch.Tensor, density: torch.Tensor
+    ) -> float:
         """Returns the total mass of the robot
 
         Returns:
             mass: The total mass
         """
-        math = SpatialMath()
-        self.link_name_list = link_name_list
-        self.factory = URDFParametricModelFactory(path=urdfstring, math=math, link_parametric_list=self.link_name_list, length_multiplier= length_multiplier, density=density )
-        model = Model.build(factory=factory, joints_name_list=joints_name_list)
-        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            link_parametric_list=self.link_name_list,
+            lenght_multiplier=lenght_multiplier,
+            density=density,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.get_total_mass()
diff --git a/src/adam/pytorch/torch_like.py b/src/adam/pytorch/torch_like.py
index 71a6f15f..71991bf7 100644
--- a/src/adam/pytorch/torch_like.py
+++ b/src/adam/pytorch/torch_like.py
@@ -23,7 +23,7 @@ def __setitem__(self, idx, value: Union["TorchLike", ntp.ArrayLike]) -> "TorchLi
         if type(self) is type(value):
             self.array[idx] = value.array.reshape(self.array[idx].shape)
         else:
-            self.array[idx] =torch.tensor(value)
+            self.array[idx] = torch.tensor(value)
 
     def __getitem__(self, idx):
         """Overrides get item operator"""
@@ -170,6 +170,11 @@ def cos(x: ntp.ArrayLike) -> "TorchLike":
         x = torch.tensor(x)
         return TorchLike(torch.cos(x))
 
+    @staticmethod
+    def mtimes(x: ntp.ArrayLike, y: ntp.ArrayLike) -> "TorchLike":
+
+        return x @ y
+
     @staticmethod
     def outer(x: ntp.ArrayLike, y: ntp.ArrayLike) -> "TorchLike":
         """
@@ -202,7 +207,6 @@ def skew(x: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike":
             torch.FloatTensor([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]])
         )
 
-
     @staticmethod
     def vertcat(*x: ntp.ArrayLike) -> "TorchLike":
         """
@@ -210,12 +214,11 @@ def vertcat(*x: ntp.ArrayLike) -> "TorchLike":
             TorchLike: vertical concatenation of x
         """
         if isinstance(x[0], TorchLike):
-            v = torch.vstack([x[i].array for i in range(len(x))]).reshape(-1, 1)
+            v = torch.vstack([x[i].array for i in range(len(x))])
         else:
-            v = torch.FloatTensor(x).reshape(-1, 1)
+            v = torch.FloatTensor(x)
         return TorchLike(v)
 
-
     @staticmethod
     def horzcat(*x: ntp.ArrayLike) -> "TorchLike":
         """
@@ -223,7 +226,7 @@ def horzcat(*x: ntp.ArrayLike) -> "TorchLike":
             TorchLike: horizontal concatenation of x
         """
         if isinstance(x[0], TorchLike):
-            v = torch.hstack([x[i].array for i in range(len(x))]).reshape(1,-1)
+            v = torch.hstack([x[i].array for i in range(len(x))])
         else:
-            v = torch.FloatTensor(x).reshape(1,-1)
+            v = torch.FloatTensor(x)
         return TorchLike(v)
diff --git a/tests/test_CasADi_computations_parametric.py b/tests/test_CasADi_computations_parametric.py
index 4fe23ad1..42d42e54 100644
--- a/tests/test_CasADi_computations_parametric.py
+++ b/tests/test_CasADi_computations_parametric.py
@@ -8,7 +8,7 @@
 import casadi as cs
 import numpy as np
 import pytest
-import math 
+import math
 from adam.casadi.computations_parametric import KinDynComputationsParametric
 from adam.casadi import KinDynComputations
 
@@ -48,28 +48,31 @@
     "r_ankle_roll",
 ]
 
-def ComputeOriginalDensity(kinDyn, link_name): 
+
+def ComputeOriginalDensity(kinDyn, link_name):
     link_original = kinDyn.factory.get_element_by_name(link_name)
     mass = link_original.inertial.mass
-    volume = 0 
+    volume = 0
     visual_obj = link_original.visuals[0]
     if type(visual_obj.geometry) is urdf_parser_py.urdf.Box:
         width = link_original.visuals[0].geometry.size[0]
         depth = link_original.visuals[0].geometry.size[2]
         height = link_original.visuals[0].geometry.size[1]
-        volume = width*depth*height
+        volume = width * depth * height
     if type(visual_obj.geometry) is urdf_parser_py.urdf.Cylinder:
-        length = link_original.visuals[0].geometry.length 
+        length = link_original.visuals[0].geometry.length
         radius = link_original.visuals[0].geometry.radius
-        volume = math.pi*radius**2*length
+        volume = math.pi * radius ** 2 * length
     if type(visual_obj.geometry) is urdf_parser_py.urdf.Sphere:
         radius = link_original.visuals[0].geometry.radius
-        volume = 4*(math.pi*radius**3)/3
-    return mass/volume
+        volume = 4 * (math.pi * radius ** 3) / 3
+    return mass / volume
+
 
 def SX2DM(x):
     return cs.DM(x)
 
+
 logging.basicConfig(level=logging.DEBUG)
 logging.debug("Showing the robot tree.")
 
@@ -77,11 +80,13 @@ def SX2DM(x):
 root_link = "root_link"
 comp = KinDynComputations(model_path, joints_name_list, root_link)
 
-link_name_list = ['chest']
-comp_w_hardware = KinDynComputationsParametric(model_path, joints_name_list, link_name_list, root_link)
+link_name_list = ["chest"]
+comp_w_hardware = KinDynComputationsParametric(
+    model_path, joints_name_list, link_name_list, root_link
+)
 original_density = []
-for item in link_name_list: 
-    original_density += [ComputeOriginalDensity(comp_w_hardware,item)]
+for item in link_name_list:
+    original_density += [ComputeOriginalDensity(comp_w_hardware, item)]
 original_length = np.ones(len(link_name_list))
 
 
@@ -99,81 +104,110 @@ def SX2DM(x):
 s_ = joints_val
 s_dot_ = joints_dot_val
 
+
 def test_mass_matrix():
     M = comp.mass_matrix_fun()
     M_with_hardware = comp_w_hardware.mass_matrix_fun()
     mass_test = SX2DM(M(H_b, s_))
-    mass_test_hardware = SX2DM(M_with_hardware(H_b,s_, original_length, original_density))
+    mass_test_hardware = SX2DM(
+        M_with_hardware(H_b, s_, original_length, original_density)
+    )
     assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5)
 
+
 def test_CMM():
     Jcm = comp.centroidal_momentum_matrix_fun()
     Jcm_with_hardware = comp_w_hardware.centroidal_momentum_matrix_fun()
     Jcm_test = SX2DM(Jcm(H_b, s_))
-    Jcm_test_hardware = SX2DM(Jcm_with_hardware(H_b,s_, original_length, original_density))
+    Jcm_test_hardware = SX2DM(
+        Jcm_with_hardware(H_b, s_, original_length, original_density)
+    )
     assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5)
 
+
 def test_CoM_pos():
     com_f = comp.CoM_position_fun()
     com_with_hardware_f = comp_w_hardware.CoM_position_fun()
     CoM_cs = SX2DM(com_f(H_b, s_))
-    CoM_hardware  = SX2DM(com_with_hardware_f(H_b,s_, original_length, original_density)) 
+    CoM_hardware = SX2DM(
+        com_with_hardware_f(H_b, s_, original_length, original_density)
+    )
     assert CoM_cs - CoM_hardware == pytest.approx(0.0, abs=1e-5)
 
+
 def test_total_mass():
-    mass = comp.get_total_mass(); 
+    mass = comp.get_total_mass()
     mass_hardware_fun = comp_w_hardware.get_total_mass()
     mass_hardware = SX2DM(mass_hardware_fun(original_length, original_density))
     assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5)
 
+
 def test_jacobian():
     J_tot = comp.jacobian_fun("l_sole")
     J_tot_with_hardware = comp_w_hardware.jacobian_fun("l_sole")
     J_test = SX2DM(J_tot(H_b, s_))
-    J_test_hardware = SX2DM(J_tot_with_hardware(H_b, s_, original_length, original_density))
+    J_test_hardware = SX2DM(
+        J_tot_with_hardware(H_b, s_, original_length, original_density)
+    )
     assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
 
+
 def test_jacobian_non_actuated():
     J_tot = comp.jacobian_fun("head")
     J_test = SX2DM(J_tot(H_b, s_))
     J_tot_with_hardware = comp_w_hardware.jacobian_fun("head")
-    J_tot_with_hardware_test = SX2DM(J_tot_with_hardware(H_b, s_, original_length, original_density))
+    J_tot_with_hardware_test = SX2DM(
+        J_tot_with_hardware(H_b, s_, original_length, original_density)
+    )
     assert J_test - J_tot_with_hardware_test == pytest.approx(0.0, abs=1e-5)
 
+
 def test_fk():
     T = comp.forward_kinematics_fun("l_sole")
     H_test = SX2DM(T(H_b, s_))
     T_with_hardware = comp_w_hardware.forward_kinematics_fun("l_sole")
-    H_with_hardware_test = SX2DM(T_with_hardware(H_b, s_, original_length, original_density))
-    assert H_with_hardware_test[:3,:3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
-    assert H_with_hardware_test[:3,3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+    H_with_hardware_test = SX2DM(
+        T_with_hardware(H_b, s_, original_length, original_density)
+    )
+    assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
+    assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+
 
 def test_fk_non_actuated():
     T = comp.forward_kinematics_fun("head")
     H_test = SX2DM(T(H_b, s_))
     T_with_hardware = comp_w_hardware.forward_kinematics_fun("head")
-    H_with_hardware_test = SX2DM(T_with_hardware(H_b,s_,original_length, original_density))
-    assert H_with_hardware_test[:3,:3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
-    assert H_with_hardware_test[:3,3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+    H_with_hardware_test = SX2DM(
+        T_with_hardware(H_b, s_, original_length, original_density)
+    )
+    assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
+    assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+
 
 def test_bias_force():
     h = comp.bias_force_fun()
     h_test = SX2DM(h(H_b, s_, vb_, s_dot_))
 
     h_with_hardware = comp_w_hardware.bias_force_fun()
-    h_with_hardware_test = SX2DM(h_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density))
+    h_with_hardware_test = SX2DM(
+        h_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density)
+    )
     assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4)
 
+
 def test_coriolis_term():
     C = comp.coriolis_term_fun()
     C_test = SX2DM(C(H_b, s_, vb_, s_dot_))
     C_with_hardware = comp_w_hardware.coriolis_term_fun()
-    C_with_hardware_test = SX2DM(C_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density))
+    C_with_hardware_test = SX2DM(
+        C_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density)
+    )
     assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4)
 
+
 def test_gravity_term():
     G = comp.gravity_term_fun()
     G_test = SX2DM(G(H_b, s_))
     G_with_hardware = comp_w_hardware.gravity_term_fun()
-    G_with_hardware_test = G_with_hardware(H_b,s_, original_length, original_density)
+    G_with_hardware_test = G_with_hardware(H_b, s_, original_length, original_density)
     assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4)
diff --git a/tests/test_Jax_computations_parametric.py b/tests/test_Jax_computations_parametric.py
index 371bf7de..2d245055 100644
--- a/tests/test_Jax_computations_parametric.py
+++ b/tests/test_Jax_computations_parametric.py
@@ -9,7 +9,7 @@
 from jax import config
 import numpy as np
 import pytest
-import math 
+import math
 from adam.jax.computations_parametric import KinDynComputationsParametric
 from adam.jax import KinDynComputations
 
@@ -51,9 +51,12 @@
     "r_ankle_pitch",
     "r_ankle_roll",
 ]
+
+
 def SX2DM(x):
     return cs.DM(x)
 
+
 logging.basicConfig(level=logging.DEBUG)
 logging.debug("Showing the robot tree.")
 
@@ -61,8 +64,10 @@ def SX2DM(x):
 root_link = "root_link"
 comp = KinDynComputations(model_path, joints_name_list, root_link)
 
-link_name_list = ['chest']
-comp_w_hardware = KinDynComputationsParametric(model_path, joints_name_list, link_name_list, root_link)
+link_name_list = ["chest"]
+comp_w_hardware = KinDynComputationsParametric(
+    model_path, joints_name_list, link_name_list, root_link
+)
 original_density = [628.0724496264945]
 original_length = np.ones(len(link_name_list))
 
@@ -80,59 +85,100 @@ def SX2DM(x):
 s_ = joints_val
 s_dot_ = joints_dot_val
 
+
 def test_mass_matrix():
     mass_test = comp.mass_matrix(H_b, s_)
-    mass_test_hardware = np.array(comp_w_hardware.mass_matrix(H_b,s_, original_length, original_density))
+    mass_test_hardware = np.array(
+        comp_w_hardware.mass_matrix(H_b, s_, original_length, original_density)
+    )
     assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5)
 
+
 def test_CMM():
     Jcm_test = comp.centroidal_momentum_matrix(H_b, s_)
-    Jcm_test_hardware = np.array(comp_w_hardware.centroidal_momentum_matrix(H_b,s_, original_length, original_density))
+    Jcm_test_hardware = np.array(
+        comp_w_hardware.centroidal_momentum_matrix(
+            H_b, s_, original_length, original_density
+        )
+    )
     assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5)
 
+
 def test_CoM_pos():
     CoM_test = comp.CoM_position(H_b, s_)
-    CoM_hardware  = np.array(comp_w_hardware.CoM_position(H_b,s_, original_length, original_density)) 
+    CoM_hardware = np.array(
+        comp_w_hardware.CoM_position(H_b, s_, original_length, original_density)
+    )
     assert CoM_test - CoM_hardware == pytest.approx(0.0, abs=1e-5)
 
+
 def test_total_mass():
-    mass = comp.get_total_mass(); 
+    mass = comp.get_total_mass()
     mass_hardware = comp_w_hardware.get_total_mass(original_length, original_density)
     assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5)
 
+
 def test_jacobian():
     J_test = comp.jacobian("l_sole", H_b, s_)
-    J_test_hardware = np.array(comp_w_hardware.jacobian("l_sole",H_b, s_, original_length, original_density))
+    J_test_hardware = np.array(
+        comp_w_hardware.jacobian("l_sole", H_b, s_, original_length, original_density)
+    )
     assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
 
+
 def test_jacobian_non_actuated():
     J_test = comp.jacobian("head", H_b, s_)
-    J_test_hardware = np.array(comp_w_hardware.jacobian("head",H_b, s_, original_length, original_density))
+    J_test_hardware = np.array(
+        comp_w_hardware.jacobian("head", H_b, s_, original_length, original_density)
+    )
     assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
 
+
 def test_fk():
     H_test = comp.forward_kinematics("l_sole", H_b, s_)
-    H_with_hardware_test = np.array(comp_w_hardware.forward_kinematics("l_sole",H_b, s_, original_length, original_density))
-    assert H_with_hardware_test[:3,:3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
-    assert H_with_hardware_test[:3,3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+    H_with_hardware_test = np.array(
+        comp_w_hardware.forward_kinematics(
+            "l_sole", H_b, s_, original_length, original_density
+        )
+    )
+    assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
+    assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+
 
 def test_fk_non_actuated():
     H_test = comp.forward_kinematics("head", H_b, s_)
-    H_with_hardware_test = np.array(comp_w_hardware.forward_kinematics("head",H_b, s_, original_length, original_density))
-    assert H_with_hardware_test[:3,:3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
-    assert H_with_hardware_test[:3,3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+    H_with_hardware_test = np.array(
+        comp_w_hardware.forward_kinematics(
+            "head", H_b, s_, original_length, original_density
+        )
+    )
+    assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
+    assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+
 
 def test_bias_force():
     h_test = comp.bias_force(H_b, s_, vb_, s_dot_)
-    h_with_hardware_test = np.array(comp_w_hardware.bias_force(H_b, s_, vb_, s_dot_, original_length, original_density))
+    h_with_hardware_test = np.array(
+        comp_w_hardware.bias_force(
+            H_b, s_, vb_, s_dot_, original_length, original_density
+        )
+    )
     assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4)
 
+
 def test_coriolis_term():
     C_test = comp.coriolis_term(H_b, s_, vb_, s_dot_)
-    C_with_hardware_test = np.array(comp_w_hardware.coriolis_term(H_b, s_, vb_, s_dot_, original_length, original_density))
+    C_with_hardware_test = np.array(
+        comp_w_hardware.coriolis_term(
+            H_b, s_, vb_, s_dot_, original_length, original_density
+        )
+    )
     assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4)
 
+
 def test_gravity_term():
     G_test = comp.gravity_term(H_b, s_)
-    G_with_hardware_test = comp_w_hardware.gravity_term(H_b,s_, original_length, original_density)
+    G_with_hardware_test = comp_w_hardware.gravity_term(
+        H_b, s_, original_length, original_density
+    )
     assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4)
diff --git a/tests/test_NumPy_computations_parametric.py b/tests/test_NumPy_computations_parametric.py
new file mode 100644
index 00000000..8bfddf38
--- /dev/null
+++ b/tests/test_NumPy_computations_parametric.py
@@ -0,0 +1,181 @@
+# 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 logging
+from os import link
+import urdf_parser_py.urdf
+import pytest
+import math
+import numpy as np
+from adam.numpy.computations_parametric import KinDynComputationsParametric
+from adam.numpy import KinDynComputations
+
+from adam.geometry import utils
+import tempfile
+from git import Repo
+
+np.random.seed(42)
+
+# Getting stickbot urdf file
+temp_dir = tempfile.TemporaryDirectory()
+git_url = "https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git"
+Repo.clone_from(git_url, temp_dir.name)
+model_path = temp_dir.name + "/models/stickBot/model.urdf"
+
+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)
+
+
+logging.basicConfig(level=logging.DEBUG)
+logging.debug("Showing the robot tree.")
+
+
+root_link = "root_link"
+comp = KinDynComputations(model_path, joints_name_list, root_link)
+
+link_name_list = ["chest"]
+comp_w_hardware = KinDynComputationsParametric(
+    model_path, joints_name_list, link_name_list, root_link
+)
+original_density = [628.0724496264945]
+original_length = np.ones(len(link_name_list))
+
+n_dofs = len(joints_name_list)
+# base pose quantities
+xyz = (np.random.rand(3) - 0.5) * 5
+rpy = (np.random.rand(3) - 0.5) * 5
+base_vel = (np.random.rand(6) - 0.5) * 5
+# joints quantitites
+joints_val = (np.random.rand(n_dofs) - 0.5) * 5
+joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5
+
+H_b = utils.H_from_Pos_RPY(xyz, rpy)
+vb_ = base_vel
+s_ = joints_val
+s_dot_ = joints_dot_val
+
+
+def test_mass_matrix():
+    mass_test = comp.mass_matrix(H_b, s_)
+    mass_test_hardware = np.array(
+        comp_w_hardware.mass_matrix(H_b, s_, original_length, original_density)
+    )
+    assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+
+def test_CMM():
+    Jcm_test = comp.centroidal_momentum_matrix(H_b, s_)
+    Jcm_test_hardware = np.array(
+        comp_w_hardware.centroidal_momentum_matrix(
+            H_b, s_, original_length, original_density
+        )
+    )
+    assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+
+def test_CoM_pos():
+    CoM_test = comp.CoM_position(H_b, s_)
+    CoM_hardware = np.array(
+        comp_w_hardware.CoM_position(H_b, s_, original_length, original_density)
+    )
+    assert CoM_test - CoM_hardware == pytest.approx(0.0, abs=1e-5)
+
+
+def test_total_mass():
+    mass = comp.get_total_mass()
+    mass_hardware = comp_w_hardware.get_total_mass(original_length, original_density)
+    assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5)
+
+
+def test_jacobian():
+    J_test = comp.jacobian("l_sole", H_b, s_)
+    J_test_hardware = np.array(
+        comp_w_hardware.jacobian("l_sole", H_b, s_, original_length, original_density)
+    )
+    assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+
+def test_jacobian_non_actuated():
+    J_test = comp.jacobian("head", H_b, s_)
+    J_test_hardware = np.array(
+        comp_w_hardware.jacobian("head", H_b, s_, original_length, original_density)
+    )
+    assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+
+def test_fk():
+    H_test = comp.forward_kinematics("l_sole", H_b, s_)
+    H_with_hardware_test = np.array(
+        comp_w_hardware.forward_kinematics(
+            "l_sole", H_b, s_, original_length, original_density
+        )
+    )
+    assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
+    assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+
+
+def test_fk_non_actuated():
+    H_test = comp.forward_kinematics("head", H_b, s_)
+    H_with_hardware_test = np.array(
+        comp_w_hardware.forward_kinematics(
+            "head", H_b, s_, original_length, original_density
+        )
+    )
+    assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
+    assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+
+
+def test_bias_force():
+    h_test = comp.bias_force(H_b, s_, vb_, s_dot_)
+    h_with_hardware_test = np.array(
+        comp_w_hardware.bias_force(
+            H_b, s_, vb_, s_dot_, original_length, original_density
+        )
+    )
+    assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4)
+
+
+def test_coriolis_term():
+    C_test = comp.coriolis_term(H_b, s_, vb_, s_dot_)
+    C_with_hardware_test = np.array(
+        comp_w_hardware.coriolis_term(
+            H_b, s_, vb_, s_dot_, original_length, original_density
+        )
+    )
+    assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4)
+
+
+def test_gravity_term():
+    G_test = comp.gravity_term(H_b, s_)
+    G_with_hardware_test = comp_w_hardware.gravity_term(
+        H_b, s_, original_length, original_density
+    )
+    assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4)
diff --git a/tests/test_pytorch_computations_parametric.py b/tests/test_pytorch_computations_parametric.py
new file mode 100644
index 00000000..e085f336
--- /dev/null
+++ b/tests/test_pytorch_computations_parametric.py
@@ -0,0 +1,183 @@
+# 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 logging
+from os import link
+import urdf_parser_py.urdf
+import pytest
+import math
+import torch
+import numpy as np
+from adam.pytorch.computations_parametric import KinDynComputationsParametric
+from adam.pytorch import KinDynComputations
+
+from adam.geometry import utils
+import tempfile
+from git import Repo
+
+np.random.seed(42)
+torch.set_default_dtype(torch.float64)
+
+# Getting stickbot urdf file
+temp_dir = tempfile.TemporaryDirectory()
+git_url = "https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git"
+Repo.clone_from(git_url, temp_dir.name)
+model_path = temp_dir.name + "/models/stickBot/model.urdf"
+
+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)
+
+
+logging.basicConfig(level=logging.DEBUG)
+logging.debug("Showing the robot tree.")
+
+
+root_link = "root_link"
+comp = KinDynComputations(model_path, joints_name_list, root_link)
+
+link_name_list = ["chest"]
+comp_w_hardware = KinDynComputationsParametric(
+    model_path, joints_name_list, link_name_list, root_link
+)
+original_density = [628.0724496264945]
+original_length = np.ones(len(link_name_list))
+
+n_dofs = len(joints_name_list)
+# base pose quantities
+xyz = (np.random.rand(3) - 0.5) * 5
+rpy = (np.random.rand(3) - 0.5) * 5
+base_vel = (np.random.rand(6) - 0.5) * 5
+# joints quantitites
+joints_val = (np.random.rand(n_dofs) - 0.5) * 5
+joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5
+
+H_b = torch.FloatTensor(utils.H_from_Pos_RPY(xyz, rpy))
+vb_ = torch.FloatTensor(base_vel)
+s_ = torch.FloatTensor(joints_val)
+s_dot_ = torch.FloatTensor(joints_dot_val)
+
+
+def test_mass_matrix():
+    mass_test = comp.mass_matrix(H_b, s_)
+    mass_test_hardware = np.array(
+        comp_w_hardware.mass_matrix(H_b, s_, original_length, original_density)
+    )
+    assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+
+def test_CMM():
+    Jcm_test = comp.centroidal_momentum_matrix(H_b, s_)
+    Jcm_test_hardware = np.array(
+        comp_w_hardware.centroidal_momentum_matrix(
+            H_b, s_, original_length, original_density
+        )
+    )
+    assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+
+def test_CoM_pos():
+    CoM_test = comp.CoM_position(H_b, s_)
+    CoM_hardware = np.array(
+        comp_w_hardware.CoM_position(H_b, s_, original_length, original_density)
+    )
+    assert CoM_test - CoM_hardware == pytest.approx(0.0, abs=1e-5)
+
+
+def test_total_mass():
+    mass = comp.get_total_mass()
+    mass_hardware = comp_w_hardware.get_total_mass(original_length, original_density)
+    assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5)
+
+
+def test_jacobian():
+    J_test = comp.jacobian("l_sole", H_b, s_)
+    J_test_hardware = np.array(
+        comp_w_hardware.jacobian("l_sole", H_b, s_, original_length, original_density)
+    )
+    assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+
+def test_jacobian_non_actuated():
+    J_test = comp.jacobian("head", H_b, s_)
+    J_test_hardware = np.array(
+        comp_w_hardware.jacobian("head", H_b, s_, original_length, original_density)
+    )
+    assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
+
+
+def test_fk():
+    H_test = np.array(comp.forward_kinematics("l_sole", H_b, s_))
+    H_with_hardware_test = np.array(
+        comp_w_hardware.forward_kinematics(
+            "l_sole", H_b, s_, original_length, original_density
+        )
+    )
+    assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
+    assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+
+
+def test_fk_non_actuated():
+    H_test = np.array(comp.forward_kinematics("head", H_b, s_))
+    H_with_hardware_test = np.array(
+        comp_w_hardware.forward_kinematics(
+            "head", H_b, s_, original_length, original_density
+        )
+    )
+    assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
+    assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
+
+
+def test_bias_force():
+    h_test = np.array(comp.bias_force(H_b, s_, vb_, s_dot_))
+    h_with_hardware_test = np.array(
+        comp_w_hardware.bias_force(
+            H_b, s_, vb_, s_dot_, original_length, original_density
+        )
+    )
+    assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4)
+
+
+def test_coriolis_term():
+    C_test = np.array(comp.coriolis_term(H_b, s_, vb_, s_dot_))
+    C_with_hardware_test = np.array(
+        comp_w_hardware.coriolis_term(
+            H_b, s_, vb_, s_dot_, original_length, original_density
+        )
+    )
+    assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4)
+
+
+def test_gravity_term():
+    G_test = comp.gravity_term(H_b, s_)
+    G_with_hardware_test = comp_w_hardware.gravity_term(
+        H_b, s_, original_length, original_density
+    )
+    assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4)

From 4b1263fe780069715d9fa81cae5be071d702551f Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Mon, 6 Nov 2023 19:15:34 +0100
Subject: [PATCH 10/19] remove useless method, add first documentation

---
 src/adam/casadi/casadi_like.py                | 21 ++-------
 src/adam/core/spatial_math.py                 | 20 ++++++--
 src/adam/jax/jax_like.py                      |  4 --
 .../parametric_factories/parametric_joint.py  | 10 +++-
 .../parametric_factories/parametric_link.py   | 46 ++++++++++++++-----
 .../parametric_factories/parametric_model.py  | 20 +++++---
 src/adam/numpy/numpy_like.py                  | 15 ------
 src/adam/pytorch/torch_like.py                |  5 --
 8 files changed, 75 insertions(+), 66 deletions(-)

diff --git a/src/adam/casadi/casadi_like.py b/src/adam/casadi/casadi_like.py
index 7de2b875..5bfeaa41 100644
--- a/src/adam/casadi/casadi_like.py
+++ b/src/adam/casadi/casadi_like.py
@@ -21,9 +21,9 @@ class CasadiLike(ArrayLike):
     def __matmul__(self, other: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike":
         """Overrides @ operator"""
         if type(other) in [CasadiLike, NumpyLike]:
-            return CasadiLike(self.array @ other.array)
+            return CasadiLike(cs.mtimes(self.array, other.array))
         else:
-            return CasadiLike(self.array @ other)
+            return CasadiLike(cs.mtimes(self.array, other))
 
     def __rmatmul__(self, other: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike":
         """Overrides @ operator"""
@@ -160,21 +160,6 @@ def sin(x: npt.ArrayLike) -> "CasadiLike":
         """
         return CasadiLike(cs.sin(x))
 
-    @staticmethod
-    def mtimes(x: npt.ArrayLike, y: npt.ArrayLike) -> "CasadiLike":
-        """
-        Args:
-            x (npt.ArrayLike): angle value
-
-        Returns:
-            CasadiLike: the sin value of x
-        """
-        if isinstance(x, CasadiLike) and isinstance(y, CasadiLike):
-            return CasadiLike(cs.mtimes(x.array, y.array))
-        else:
-            return CasadiLike(cs.mtimes(x, y))
-        # return CasadiLike(cs.mtimes(x, y))
-
     @staticmethod
     def cos(x: npt.ArrayLike) -> "CasadiLike":
         """
@@ -215,7 +200,7 @@ def vertcat(*x) -> "CasadiLike":
     def horzcat(*x) -> "CasadiLike":
         """
         Returns:
-            CasadiLike:  vertical concatenation of elements
+            CasadiLike:  horizontal concatenation of elements
         """
         # here the logic is a bit convoluted: x is a tuple containing CasadiLike
         # cs.vertcat accepts *args. A list of cs types is created extracting the value
diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py
index b9200b5f..01515d74 100644
--- a/src/adam/core/spatial_math.py
+++ b/src/adam/core/spatial_math.py
@@ -401,17 +401,27 @@ def spatial_inertia(
         return IO
 
     def spatial_inertial_with_parameter(self, I, mass, c, rpy):
-        # Returns the 6x6 inertia matrix expressed at the origin of the link (with rotation)"""
+        """
+        Args:
+            I (npt.ArrayLike): inertia values parametric
+            mass (npt.ArrayLike): mass value parametric
+            c (npt.ArrayLike): origin of the link parametric
+            rpy (npt.ArrayLike): orientation of the link from urdf
+
+        Returns:
+            npt.ArrayLike: the 6x6 inertia matrix parametric expressed at the origin of the link (with rotation)
+        """
         IO = self.factory.zeros(6, 6)
         Sc = self.skew(c)
         R = self.factory.zeros(3, 3)
         R_temp = self.R_from_RPY(rpy)
         inertia_matrix = self.vertcat(
-            self.horzcat(I.ixx, 0.0, 0.0),
-            self.horzcat(0.0, I.iyy, 0.0),
-            self.horzcat(0.0, 0.0, I.izz),
+            self.horzcat(I.ixx, I.ixy, I.ixz),
+            self.horzcat(I.iyx, I.iyy, I.iyz),
+            self.horzcat(I.ixz, I.iyz, I.izz),
         )
-        IO[3:, 3:] = R_temp @ inertia_matrix @ R_temp.T + mass * self.mtimes(Sc, Sc.T)
+
+        IO[3:, 3:] = R_temp @ inertia_matrix @ R_temp.T + mass * Sc @ Sc.T
         IO[3:, :3] = mass * Sc
         IO[:3, 3:] = mass * Sc.T
         IO[:3, :3] = self.factory.eye(3) * mass
diff --git a/src/adam/jax/jax_like.py b/src/adam/jax/jax_like.py
index bb88f457..23ddc2a1 100644
--- a/src/adam/jax/jax_like.py
+++ b/src/adam/jax/jax_like.py
@@ -188,10 +188,6 @@ def skew(x: Union["JaxLike", npt.ArrayLike]) -> "JaxLike":
         x = x.array
         return JaxLike(-jnp.cross(jnp.array(x), jnp.eye(3), axisa=0, axisb=0))
 
-    @staticmethod
-    def mtimes(x: npt.ArrayLike, y: npt.ArrayLike) -> "JaxLike":
-        return x @ y
-
     @staticmethod
     def vertcat(*x) -> "JaxLike":
         """
diff --git a/src/adam/model/parametric_factories/parametric_joint.py b/src/adam/model/parametric_factories/parametric_joint.py
index 88583a48..1124740d 100644
--- a/src/adam/model/parametric_factories/parametric_joint.py
+++ b/src/adam/model/parametric_factories/parametric_joint.py
@@ -34,7 +34,15 @@ def __init__(
         self.offset = joint_offset
         self.origin = self.modify(self.parent_parametric.link_offset)
 
-    def modify(self, parent_joint_offset):
+    def modify(self, parent_joint_offset: npt.ArrayLike):
+        """
+        Args:
+            parent_joint_offset (npt.ArrayLike): offset of the parent joint
+
+        Returns:
+            npt.ArrayLike: the origin of the joint, parametric with respect to the parent link dimensions
+        """
+
         length = self.parent_parametric.get_principal_lenght_parametric()
         # Ack for avoiding depending on casadi
         vo = self.parent_parametric.origin[2]
diff --git a/src/adam/model/parametric_factories/parametric_link.py b/src/adam/model/parametric_factories/parametric_link.py
index a6cbc1f3..ace0a065 100644
--- a/src/adam/model/parametric_factories/parametric_link.py
+++ b/src/adam/model/parametric_factories/parametric_link.py
@@ -10,13 +10,17 @@
 
 
 class I_parametric:
+    """Inertia values parametric"""
+
     def __init__(self) -> None:
         self.ixx = 0.0
         self.ixy = 0.0
         self.ixz = 0.0
+        self.iyx = 0.0
         self.iyy = 0.0
         self.iyz = 0.0
         self.izz = 0.0
+        self.ixz = 0.0
 
 
 class Geometry(Enum):
@@ -52,20 +56,18 @@ def __init__(
         self.visuals = link.visual
         self.geometry_type, self.visual_data = self.get_geometry(self.visuals)
         self.link = link
-        # self.parent = self.link.parent
         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()
-
         self.inertial = Inertial(self.mass)
         self.inertial.mass = self.mass
         self.inertial.inertia = self.I
         self.inertial.origin = self.origin
 
     def get_principal_lenght(self):
+        """Method computing the principal link length, i.e. the dimension in which the kinematic chain grows"""
         xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
         if self.geometry_type == Geometry.CYLINDER:
             if xyz_rpy[3] < 0.0 or xyz_rpy[4] > 0.0:
@@ -85,7 +87,7 @@ def get_principal_lenght(self):
         return v_l
 
     def get_principal_lenght_parametric(self):
-
+        """Method computing the principal link length parametric, i.e. the dimension in which the kinematic chain grows"""
         xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
         if self.geometry_type == Geometry.CYLINDER:
             if xyz_rpy[3] < 0.0 or xyz_rpy[4] > 0.0:
@@ -105,6 +107,10 @@ def get_principal_lenght_parametric(self):
         return v_l
 
     def compute_offset(self):
+        """
+        Returns:
+            npt.ArrayLike: link offset
+        """
         xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
         v_l = self.get_principal_lenght()
         v_o = xyz_rpy[2]
@@ -115,6 +121,10 @@ def compute_offset(self):
         return link_offset
 
     def compute_joint_offset(self, joint_i, parent_offset):
+        """
+        Returns:
+            npt.ArrayLike: the child joint offset
+        """
         # Taking the principal direction i.e. the length
         xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
         v_l = self.get_principal_lenght()
@@ -130,8 +140,10 @@ def compute_joint_offset(self, joint_i, parent_offset):
 
     @staticmethod
     def get_geometry(visual_obj):
-
-        """Returns the geometry type and thez corresponding geometry object for a given visual"""
+        """
+        Returns:
+            (Geometry, urdf geometry): the geometry of the link and the related urdf object
+        """
         if type(visual_obj.geometry) is urdf_parser_py.urdf.Box:
             return (Geometry.BOX, visual_obj.geometry)
         if type(visual_obj.geometry) is urdf_parser_py.urdf.Cylinder:
@@ -139,9 +151,11 @@ def get_geometry(visual_obj):
         if type(visual_obj.geometry) is urdf_parser_py.urdf.Sphere:
             return (Geometry.SPHERE, visual_obj.geometry)
 
-    """Function that starting from a multiplier and link visual characteristics computes the link volume"""
-
     def compute_volume(self):
+        """
+        Returns:
+            (npt.ArrayLike, npt.ArrayLike): the volume and the dimension parametric
+        """
         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:
@@ -164,12 +178,19 @@ def compute_volume(self):
     """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."""
+        """
+        Returns:
+            (npt.ArrayLike): the link mass
+        """
         mass = 0.0
         mass = self.volume * self.density
         return mass
 
     def modify_origin(self):
+        """
+        Returns:
+            (npt.ArrayLike): the link origin parametrized
+        """
         origin = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
         xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
         v_o = xyz_rpy[2]
@@ -194,10 +215,13 @@ def modify_origin(self):
         return origin
 
     def compute_inertia_parametric(self):
+        """
+        Returns:
+            Inertia Parametric: 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
+        """
         I = I_parametric()
         xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
-        """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
diff --git a/src/adam/model/parametric_factories/parametric_model.py b/src/adam/model/parametric_factories/parametric_model.py
index 5cb19203..69b2a210 100644
--- a/src/adam/model/parametric_factories/parametric_model.py
+++ b/src/adam/model/parametric_factories/parametric_model.py
@@ -9,7 +9,7 @@
 
 
 class URDFParametricModelFactory(ModelFactory):
-    """This factory generates robot elements from urdf_parser_py
+    """This factory generates robot elements from urdf_parser_py parametrized w.r.t. link lengths and densities
 
     Args:
         ModelFactory: the Model factory
@@ -37,14 +37,14 @@ def __init__(
     def get_joints(self) -> List[Joint]:
         """
         Returns:
-            List[StdJoint]: build the list of the joints
+            List[Joint]: 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
+            List[Link]: build the list of the links
         """
         return [
             self.build_link(l) for l in self.urdf_desc.links if l.inertial is not None
@@ -53,7 +53,7 @@ def get_links(self) -> List[Link]:
     def get_frames(self) -> List[StdLink]:
         """
         Returns:
-            List[StdLink]: build the list of the links
+            List[Link]: build the list of the links
         """
         return [self.build_link(l) for l in self.urdf_desc.links if l.inertial is None]
 
@@ -63,7 +63,7 @@ def build_joint(self, joint: urdf_parser_py.urdf.Joint) -> Joint:
             joint (Joint): the urdf_parser_py joint
 
         Returns:
-            StdJoint: our joint representation
+            StdJoint/ParametricJoint: our joint representation
         """
         if joint.parent in self.link_parametric_list:
             index_link = self.link_parametric_list.index(joint.parent)
@@ -84,7 +84,7 @@ def build_link(self, link: urdf_parser_py.urdf.Link) -> Link:
             link (Link): the urdf_parser_py link
 
         Returns:
-            Link: our link representation
+            StdLink/ParametricLink: our link representation
         """
         if link.name in self.link_parametric_list:
             index_link = self.link_parametric_list.index(link.name)
@@ -97,7 +97,13 @@ def build_link(self, link: urdf_parser_py.urdf.Link) -> 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"""
+        """
+        Args:
+            link_name (Link): the link name
+
+        Returns:
+            Link: the urdf parser link object associated to the link name
+        """
         link_list = [
             corresponding_link
             for corresponding_link in self.urdf_desc.links
diff --git a/src/adam/numpy/numpy_like.py b/src/adam/numpy/numpy_like.py
index 4c77ddb0..ddfac73a 100644
--- a/src/adam/numpy/numpy_like.py
+++ b/src/adam/numpy/numpy_like.py
@@ -151,21 +151,6 @@ def sin(x: npt.ArrayLike) -> "NumpyLike":
         """
         return NumpyLike(np.sin(x))
 
-    @staticmethod
-    def mtimes(x: npt.ArrayLike, y: npt.ArrayLike) -> "NumpyLike":
-        """
-        Args:
-            x (npt.ArrayLike): angle value
-
-        Returns:
-            CasadiLike: the sin value of x
-        """
-        if isinstance(x, NumpyLike) and isinstance(y, NumpyLike):
-            return NumpyLike((x.array @ y.array))
-        else:
-            return NumpyLike(x @ y)
-        # return CasadiLike(cs.mtimes(x, y))
-
     @staticmethod
     def cos(x: npt.ArrayLike) -> "NumpyLike":
         """
diff --git a/src/adam/pytorch/torch_like.py b/src/adam/pytorch/torch_like.py
index 71991bf7..f24d91c5 100644
--- a/src/adam/pytorch/torch_like.py
+++ b/src/adam/pytorch/torch_like.py
@@ -170,11 +170,6 @@ def cos(x: ntp.ArrayLike) -> "TorchLike":
         x = torch.tensor(x)
         return TorchLike(torch.cos(x))
 
-    @staticmethod
-    def mtimes(x: ntp.ArrayLike, y: ntp.ArrayLike) -> "TorchLike":
-
-        return x @ y
-
     @staticmethod
     def outer(x: ntp.ArrayLike, y: ntp.ArrayLike) -> "TorchLike":
         """

From 8f60f7cd0de3a513c180f288eaeac979c95cdcb5 Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Tue, 7 Nov 2023 10:25:38 +0100
Subject: [PATCH 11/19] Added documentation for parametric kyndyn classes,
 refactor of the variable names

---
 src/adam/casadi/computations_parametric.py    |  43 +++---
 src/adam/jax/computations_parametric.py       | 137 +++++++++++-------
 .../parametric_factories/parametric_link.py   |   8 +-
 .../parametric_factories/parametric_model.py  |  28 ++--
 src/adam/numpy/computations_parametric.py     | 127 +++++++++-------
 src/adam/pytorch/computations_parametric.py   | 127 +++++++++-------
 6 files changed, 275 insertions(+), 195 deletions(-)

diff --git a/src/adam/casadi/computations_parametric.py b/src/adam/casadi/computations_parametric.py
index a999dbbf..0a4aad2c 100644
--- a/src/adam/casadi/computations_parametric.py
+++ b/src/adam/casadi/computations_parametric.py
@@ -12,14 +12,14 @@
 
 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.
+    in mixed representation, for Floating Base systems - as humanoid robots. This is parametric w.r.t the link length and denisties
     """
 
     def __init__(
         self,
         urdfstring: str,
         joints_name_list: list,
-        link_name_list: list,
+        links_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")),
@@ -28,18 +28,19 @@ def __init__(
         Args:
             urdfstring (str): path of the urdf
             joints_name_list (list): list of the actuated joints
+            links_name_list (list): list of the parametrized links
             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)
+        n_param_link = len(links_name_list)
+        self.densities = cs.SX.sym("densities", n_param_link)
         self.length_multiplier = cs.SX.sym("length_multiplier", n_param_link)
         self.factory = URDFParametricModelFactory(
             path=urdfstring,
             math=math,
-            link_parametric_list=link_name_list,
-            lenght_multiplier=self.length_multiplier,
-            density=self.density,
+            links_name_list=links_name_list,
+            length_multiplier=self.length_multiplier,
+            densities=self.densities,
         )
         model = Model.build(factory=self.factory, joints_name_list=joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=math)
@@ -57,7 +58,10 @@ def mass_matrix_fun(self) -> cs.Function:
         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
+            "M",
+            [T_b, s, self.length_multiplier, self.densities],
+            [M.array],
+            self.f_opts,
         )
 
     def centroidal_momentum_matrix_fun(self) -> cs.Function:
@@ -71,7 +75,7 @@ def centroidal_momentum_matrix_fun(self) -> cs.Function:
         [_, Jcm] = self.rbdalgos.crba(T_b, s)
         return cs.Function(
             "Jcm",
-            [T_b, s, self.length_multiplier, self.density],
+            [T_b, s, self.length_multiplier, self.densities],
             [Jcm.array],
             self.f_opts,
         )
@@ -90,7 +94,7 @@ def forward_kinematics_fun(self, frame: str) -> cs.Function:
         T_fk = self.rbdalgos.forward_kinematics(frame, T_b, s)
         return cs.Function(
             "T_fk",
-            [T_b, s, self.length_multiplier, self.density],
+            [T_b, s, self.length_multiplier, self.densities],
             [T_fk.array],
             self.f_opts,
         )
@@ -109,7 +113,7 @@ def jacobian_fun(self, frame: str) -> cs.Function:
         J_tot = self.rbdalgos.jacobian(frame, T_b, s)
         return cs.Function(
             "J_tot",
-            [T_b, s, self.length_multiplier, self.density],
+            [T_b, s, self.length_multiplier, self.densities],
             [J_tot.array],
             self.f_opts,
         )
@@ -126,7 +130,7 @@ def relative_jacobian_fun(self, frame: str) -> cs.Function:
         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
+            "J", [s, self.length_multiplier, self.densities], [J.array], self.f_opts
         )
 
     def CoM_position_fun(self) -> cs.Function:
@@ -140,7 +144,7 @@ def CoM_position_fun(self) -> cs.Function:
         com_pos = self.rbdalgos.CoM_position(T_b, s)
         return cs.Function(
             "CoM_pos",
-            [T_b, s, self.length_multiplier, self.density],
+            [T_b, s, self.length_multiplier, self.densities],
             [com_pos.array],
             self.f_opts,
         )
@@ -159,7 +163,7 @@ def bias_force_fun(self) -> cs.Function:
         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],
+            [T_b, s, v_b, s_dot, self.length_multiplier, self.densities],
             [h.array],
             self.f_opts,
         )
@@ -179,7 +183,7 @@ def coriolis_term_fun(self) -> cs.Function:
         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],
+            [T_b, q, v_b, q_dot, self.length_multiplier, self.densities],
             [C.array],
             self.f_opts,
         )
@@ -196,7 +200,10 @@ def gravity_term_fun(self) -> cs.Function:
         # 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
+            "G",
+            [T_b, q, self.length_multiplier, self.densities],
+            [G.array],
+            self.f_opts,
         )
 
     def forward_kinematics(self, frame, T_b, s) -> cs.Function:
@@ -210,7 +217,7 @@ def forward_kinematics(self, frame, T_b, s) -> cs.Function:
         """
 
         return self.rbdalgos.forward_kinematics(
-            frame, T_b, s, self.length_multiplier, self.density
+            frame, T_b, s, self.length_multiplier, self.densities
         )
 
     def get_total_mass(self) -> float:
@@ -221,6 +228,6 @@ def get_total_mass(self) -> float:
         """
         m = self.rbdalgos.get_total_mass()
         return cs.Function(
-            "m", [self.density, self.length_multiplier], [m], self.f_opts
+            "m", [self.densities, self.length_multiplier], [m], self.f_opts
         )
         return self.rbdalgos.get_total_mass()
diff --git a/src/adam/jax/computations_parametric.py b/src/adam/jax/computations_parametric.py
index c1be7b16..d4ccb216 100644
--- a/src/adam/jax/computations_parametric.py
+++ b/src/adam/jax/computations_parametric.py
@@ -13,14 +13,14 @@
 
 class KinDynComputationsParametric:
     """This is a small class that retrieves robot quantities using Jax
-    in mixed representation, for Floating Base systems - as humanoid robots.
+    in mixed representation, for Floating Base systems - as humanoid robots. This is parametric w.r.t the link length and denisties
     """
 
     def __init__(
         self,
         urdfstring: str,
         joints_name_list: list,
-        link_name_list: list,
+        links_name_list: list,
         root_link: str = "root_link",
         gravity: np.array = jnp.array([0, 0, -9.80665, 0, 0, 0]),
     ) -> None:
@@ -28,10 +28,11 @@ def __init__(
         Args:
             urdfstring (str): path of the urdf
             joints_name_list (list): list of the actuated joints
+            links_name_list (list): list of parametric links
             root_link (str, optional): the first link. Defaults to 'root_link'.
         """
         self.math = SpatialMath()
-        self.link_name_list = link_name_list
+        self.links_name_list = links_name_list
         self.g = gravity
         self.urdfstring = urdfstring
         self.joints_name_list = joints_name_list
@@ -40,14 +41,16 @@ def mass_matrix(
         self,
         base_transform: jnp.array,
         joint_positions: jnp.array,
-        lenght_multiplier: jnp.array,
-        density: jnp.array,
+        length_multiplier: jnp.array,
+        densities: jnp.array,
     ):
         """Returns the Mass Matrix functions computed the CRBA
 
         Args:
             base_transform (jnp.array): The homogenous transform from base to world frame
             joint_positions (jnp.array): The joints position
+            length_multiplier (jnp.array): The length multiplier of the parametrized links
+            densities (jnp.array): The densities of the parametrized links
 
         Returns:
             M (jax): Mass Matrix
@@ -56,9 +59,9 @@ def mass_matrix(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -70,14 +73,16 @@ def centroidal_momentum_matrix(
         self,
         base_transform: jnp.array,
         joint_positions: jnp.array,
-        lenght_multiplier: jnp.array,
-        density: jnp.array,
+        length_multiplier: jnp.array,
+        densities: jnp.array,
     ):
         """Returns the Centroidal Momentum Matrix functions computed the CRBA
 
         Args:
             base_transform (jnp.array): The homogenous transform from base to world frame
             joint_positions (jnp.array): The joints position
+            length_multiplier (jnp.array): The length multiplier of the parametrized links
+            densities (jnp.array): The densities of the parametrized links
 
         Returns:
             Jcc (jnp.array): Centroidal Momentum matrix
@@ -86,9 +91,9 @@ def centroidal_momentum_matrix(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -100,14 +105,16 @@ def relative_jacobian(
         self,
         frame: str,
         joint_positions: jnp.array,
-        lenght_multiplier: jnp.array,
-        density: jnp.array,
+        length_multiplier: jnp.array,
+        densities: jnp.array,
     ):
         """Returns the Jacobian between the root link and a specified frame frames
 
         Args:
             frame (str): The tip of the chain
             joint_positions (jnp.array): The joints position
+            length_multiplier (jnp.array): The length multiplier of the parametrized links
+            densities (jnp.array): The densities of the parametrized links
 
         Returns:
             J (jnp.array): The Jacobian between the root and the frame
@@ -116,9 +123,9 @@ def relative_jacobian(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -130,8 +137,8 @@ def forward_kinematics(
         frame: str,
         base_transform: jnp.array,
         joint_positions: jnp.array,
-        lenght_multiplier: jnp.array,
-        density: jnp.array,
+        length_multiplier: jnp.array,
+        densities: jnp.array,
     ):
         """Computes the forward kinematics relative to the specified frame
 
@@ -139,6 +146,8 @@ def forward_kinematics(
             frame (str): The frame to which the fk will be computed
             base_transform (jnp.array): The homogenous transform from base to world frame
             joint_positions (jnp.array): The joints position
+            length_multiplier (jnp.array): The length multiplier of the parametrized links
+            densities (jnp.array): The densities of the parametrized links
 
         Returns:
             T_fk (jnp.array): The fk represented as Homogenous transformation matrix
@@ -147,9 +156,9 @@ def forward_kinematics(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -159,7 +168,7 @@ def forward_kinematics(
         ).array
 
     def forward_kinematics_fun(
-        self, frame, lenght_multiplier: jnp.array, density: jnp.array
+        self, frame, length_multiplier: jnp.array, densities: jnp.array
     ):
         return lambda T, joint_positions: self.forward_kinematics(
             frame, T, joint_positions
@@ -170,15 +179,18 @@ def jacobian(
         frame: str,
         base_transform,
         joint_positions,
-        lenght_multiplier: jnp.array,
-        density: jnp.array,
+        length_multiplier: jnp.array,
+        densities: jnp.array,
     ):
         """Returns the Jacobian relative to the specified frame
 
-        Args:
+        Args
+
+            frame (str): The frame to which the jacobian will be computed
             base_transform (jnp.array): The homogenous transform from base to world frame
             s (jnp.array): The joints position
-            frame (str): The frame to which the jacobian will be computed
+            length_multiplier (jnp.array): The length multiplier of the parametrized links
+            densities (jnp.array): The densities of the parametrized links
 
         Returns:
             J_tot (jnp.array): The Jacobian relative to the frame
@@ -187,9 +199,9 @@ def jacobian(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -202,8 +214,8 @@ def bias_force(
         joint_positions: jnp.array,
         base_velocity: jnp.array,
         s_dot: jnp.array,
-        lenght_multiplier: jnp.array,
-        density: jnp.array,
+        length_multiplier: jnp.array,
+        densities: jnp.array,
     ) -> jnp.array:
         """Returns the bias force of the floating-base dynamics ejoint_positionsuation,
         using a reduced RNEA (no acceleration and external forces)
@@ -213,6 +225,9 @@ def bias_force(
             joint_positions (jnp.array): The joints position
             base_velocity (jnp.array): The base velocity in mixed representation
             s_dot (jnp.array): The joints velocity
+            length_multiplier (jnp.array): The length multiplier of the parametrized links
+            densities (jnp.array): The densities of the parametrized links
+
 
         Returns:
             h (jnp.array): the bias force
@@ -221,9 +236,9 @@ def bias_force(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -238,8 +253,8 @@ def coriolis_term(
         joint_positions: jnp.array,
         base_velocity: jnp.array,
         s_dot: jnp.array,
-        lenght_multiplier: jnp.array,
-        density: jnp.array,
+        length_multiplier: jnp.array,
+        densities: jnp.array,
     ) -> jnp.array:
         """Returns the coriolis term of the floating-base dynamics ejoint_positionsuation,
         using a reduced RNEA (no acceleration and external forces)
@@ -249,6 +264,8 @@ def coriolis_term(
             joint_positions (jnp.array): The joints position
             base_velocity (jnp.array): The base velocity in mixed representation
             s_dot (jnp.array): The joints velocity
+            length_multiplier (jnp.array): The length multiplier of the parametrized links
+            densities (jnp.array): The densities of the parametrized links
 
         Returns:
             C (jnp.array): the Coriolis term
@@ -257,9 +274,9 @@ def coriolis_term(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -276,8 +293,8 @@ def gravity_term(
         self,
         base_transform: jnp.array,
         joint_positions: jnp.array,
-        lenght_multiplier: jnp.array,
-        density: jnp.array,
+        length_multiplier: jnp.array,
+        densities: jnp.array,
     ) -> jnp.array:
         """Returns the gravity term of the floating-base dynamics ejoint_positionsuation,
         using a reduced RNEA (no acceleration and external forces)
@@ -285,6 +302,8 @@ def gravity_term(
         Args:
             base_transform (jnp.array): The homogenous transform from base to world frame
             joint_positions (jnp.array): The joints position
+            length_multiplier (jnp.array): The length multiplier of the parametrized links
+            densities (jnp.array): The densities of the parametrized links
 
         Returns:
             G (jnp.array): the gravity term
@@ -293,9 +312,9 @@ def gravity_term(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -312,14 +331,16 @@ def CoM_position(
         self,
         base_transform: jnp.array,
         joint_positions: jnp.array,
-        lenght_multiplier: jnp.array,
-        density: jnp.array,
+        length_multiplier: jnp.array,
+        densities: jnp.array,
     ) -> jnp.array:
         """Returns the CoM positon
 
         Args:
             base_transform (jnp.array): The homogenous transform from base to world frame
             joint_positions (jnp.array): The joints position
+            length_multiplier (jnp.array): The length multiplier of the parametrized links
+            densities (jnp.array): The densities of the parametrized links
 
         Returns:
             com (jnp.array): The CoM position
@@ -328,9 +349,9 @@ def CoM_position(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -339,18 +360,24 @@ def CoM_position(
             base_transform, joint_positions
         ).array.squeeze()
 
-    def get_total_mass(self, lenght_multiplier: jnp.array, density: jnp.array) -> float:
+    def get_total_mass(
+        self, length_multiplier: jnp.array, densities: jnp.array
+    ) -> float:
         """Returns the total mass of the robot
 
+        Args:
+            length_multiplier (jnp.array): The length multiplier of the parametrized links
+            densities (jnp.array): The densities of the parametrized links
+
         Returns:
             mass: The total mass
         """
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
diff --git a/src/adam/model/parametric_factories/parametric_link.py b/src/adam/model/parametric_factories/parametric_link.py
index ace0a065..aa56b818 100644
--- a/src/adam/model/parametric_factories/parametric_link.py
+++ b/src/adam/model/parametric_factories/parametric_link.py
@@ -47,12 +47,12 @@ def __init__(
         link: urdf_parser_py.urdf.Link,
         math: SpatialMath,
         length_multiplier,
-        density,
+        densities,
     ):
         self.math = math
         self.name = link.name
         self.length_multiplier = length_multiplier
-        self.density = density
+        self.densities = densities
         self.visuals = link.visual
         self.geometry_type, self.visual_data = self.get_geometry(self.visuals)
         self.link = link
@@ -175,7 +175,7 @@ def compute_volume(self):
             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"""
+    """Function that computes the mass starting from the densities, the length multiplier and the link"""
 
     def compute_mass(self):
         """
@@ -183,7 +183,7 @@ def compute_mass(self):
             (npt.ArrayLike): the link mass
         """
         mass = 0.0
-        mass = self.volume * self.density
+        mass = self.volume * self.densities
         return mass
 
     def modify_origin(self):
diff --git a/src/adam/model/parametric_factories/parametric_model.py b/src/adam/model/parametric_factories/parametric_model.py
index 69b2a210..bd45dd58 100644
--- a/src/adam/model/parametric_factories/parametric_model.py
+++ b/src/adam/model/parametric_factories/parametric_model.py
@@ -19,20 +19,20 @@ def __init__(
         self,
         path: str,
         math: SpatialMath,
-        link_parametric_list: List,
-        lenght_multiplier,
-        density,
+        links_name_list: List,
+        length_multiplier,
+        densities,
     ):
         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 = link_parametric_list
+        self.links_name_list = links_name_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
+        self.length_multiplier = length_multiplier
+        self.densities = densities
 
     def get_joints(self) -> List[Joint]:
         """
@@ -65,14 +65,14 @@ def build_joint(self, joint: urdf_parser_py.urdf.Joint) -> Joint:
         Returns:
             StdJoint/ParametricJoint: our joint representation
         """
-        if joint.parent in self.link_parametric_list:
-            index_link = self.link_parametric_list.index(joint.parent)
+        if joint.parent in self.links_name_list:
+            index_link = self.links_name_list.index(joint.parent)
             link_parent = self.get_element_by_name(joint.parent)
             parent_link_parametric = ParametricLink(
                 link_parent,
                 self.math,
-                self.lenght_multiplier[index_link],
-                self.density[index_link],
+                self.length_multiplier[index_link],
+                self.densities[index_link],
             )
             return ParmetricJoint(joint, self.math, parent_link_parametric)
 
@@ -86,13 +86,13 @@ def build_link(self, link: urdf_parser_py.urdf.Link) -> Link:
         Returns:
             StdLink/ParametricLink: our link representation
         """
-        if link.name in self.link_parametric_list:
-            index_link = self.link_parametric_list.index(link.name)
+        if link.name in self.links_name_list:
+            index_link = self.links_name_list.index(link.name)
             return ParametricLink(
                 link,
                 self.math,
-                self.lenght_multiplier[index_link],
-                self.density[index_link],
+                self.length_multiplier[index_link],
+                self.densities[index_link],
             )
         return StdLink(link, self.math)
 
diff --git a/src/adam/numpy/computations_parametric.py b/src/adam/numpy/computations_parametric.py
index d53de5fe..1280b33a 100644
--- a/src/adam/numpy/computations_parametric.py
+++ b/src/adam/numpy/computations_parametric.py
@@ -11,14 +11,14 @@
 
 class KinDynComputationsParametric:
     """This is a small class that retrieves robot quantities using NumPy
-    in mixed representation, for Floating Base systems - as humanoid robots.
+    in mixed representation, for Floating Base systems - as humanoid robots. This is parametric w.r.t the link length and denisties
     """
 
     def __init__(
         self,
         urdfstring: str,
         joints_name_list: list,
-        link_name_list: list,
+        links_name_list: list,
         root_link: str = "root_link",
         gravity: np.array = np.array([0, 0, -9.80665, 0, 0, 0]),
     ) -> None:
@@ -26,9 +26,10 @@ def __init__(
         Args:
             urdfstring (str): path of the urdf
             joints_name_list (list): list of the actuated joints
+            links_name_list (list): list of parametric links
             root_link (str, optional): the first link. Defaults to 'root_link'.
         """
-        self.link_name_list = link_name_list
+        self.links_name_list = links_name_list
         self.math = SpatialMath()
         self.g = gravity
         self.urdfstring = urdfstring
@@ -38,14 +39,16 @@ def mass_matrix(
         self,
         base_transform: np.ndarray,
         joint_positions: np.ndarray,
-        lenght_multiplier: np.ndarray,
-        density: np.ndarray,
+        length_multiplier: np.ndarray,
+        densities: np.ndarray,
     ) -> np.ndarray:
         """Returns the Mass Matrix functions computed the CRBA
 
         Args:
             base_transform (np.ndarray): The homogenous transform from base to world frame
             joint_positions (np.ndarray): The joints position
+            length_multiplier (np.ndarray): The length multiplier of the parametrized links
+            densities (np.ndarray): The densities of the parametrized links
 
         Returns:
             M (np.ndarray): Mass Matrix
@@ -54,9 +57,9 @@ def mass_matrix(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -68,14 +71,16 @@ def centroidal_momentum_matrix(
         self,
         base_transform: np.ndarray,
         s: np.ndarray,
-        lenght_multiplier: np.ndarray,
-        density: np.ndarray,
+        length_multiplier: np.ndarray,
+        densities: np.ndarray,
     ) -> np.ndarray:
         """Returns the Centroidal Momentum Matrix functions computed the CRBA
 
         Args:
             base_transform (np.ndarray): The homogenous transform from base to world frame
             joint_positions (np.ndarray): The joint positions
+            length_multiplier (np.ndarray): The length multiplier of the parametrized links
+            densities (np.ndarray): The densities of the parametrized links
 
         Returns:
             Jcc (np.ndarray): Centroidal Momentum matrix
@@ -84,9 +89,9 @@ def centroidal_momentum_matrix(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -99,8 +104,8 @@ def forward_kinematics(
         frame: str,
         base_transform: np.ndarray,
         joint_positions: np.ndarray,
-        lenght_multiplier: np.ndarray,
-        density: np.ndarray,
+        length_multiplier: np.ndarray,
+        densities: np.ndarray,
     ) -> np.ndarray:
         """Computes the forward kinematics relative to the specified frame
 
@@ -108,6 +113,8 @@ def forward_kinematics(
             frame (str): The frame to which the fk will be computed
             base_transform (np.ndarray): The homogenous transform from base to world frame
             joint_positions (np.ndarray): The joints position
+            length_multiplier (np.ndarray): The length multiplier of the parametrized links
+            densities (np.ndarray): The densities of the parametrized links
 
         Returns:
             T_fk (np.ndarray): The fk represented as Homogenous transformation matrix
@@ -116,9 +123,9 @@ def forward_kinematics(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -132,8 +139,8 @@ def jacobian(
         frame: str,
         base_transform: np.ndarray,
         joint_positions: np.ndarray,
-        lenght_multiplier: np.ndarray,
-        density: np.ndarray,
+        length_multiplier: np.ndarray,
+        densities: np.ndarray,
     ) -> np.ndarray:
         """Returns the Jacobian relative to the specified frame
 
@@ -141,6 +148,8 @@ def jacobian(
             frame (str): The frame to which the jacobian will be computed
             base_transform (np.ndarray): The homogenous transform from base to world frame
             joint_positions (np.ndarray): The joints position
+            length_multiplier (np.ndarray): The length multiplier of the parametrized links
+            densities (np.ndarray): The densities of the parametrized links
 
         Returns:
             J_tot (np.ndarray): The Jacobian relative to the frame
@@ -149,9 +158,9 @@ def jacobian(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -164,14 +173,16 @@ def relative_jacobian(
         self,
         frame: str,
         joint_positions: np.ndarray,
-        lenght_multiplier: np.ndarray,
-        density: np.ndarray,
+        length_multiplier: np.ndarray,
+        densities: np.ndarray,
     ) -> np.ndarray:
         """Returns the Jacobian between the root link and a specified frame frames
 
         Args:
             frame (str): The tip of the chain
             joint_positions (np.ndarray): The joints position
+            length_multiplier (np.ndarray): The length multiplier of the parametrized links
+            densities (np.ndarray): The densities of the parametrized links
 
         Returns:
             J (np.ndarray): The Jacobian between the root and the frame
@@ -180,9 +191,9 @@ def relative_jacobian(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -193,14 +204,16 @@ def CoM_position(
         self,
         base_transform: np.ndarray,
         joint_positions: np.ndarray,
-        lenght_multiplier: np.ndarray,
-        density: np.ndarray,
+        length_multiplier: np.ndarray,
+        densities: np.ndarray,
     ) -> np.ndarray:
         """Returns the CoM positon
 
         Args:
             base_transform (np.ndarray): The homogenous transform from base to world frame
             joint_positions (np.ndarray): The joints position
+            length_multiplier (np.ndarray): The length multiplier of the parametrized links
+            densities (np.ndarray): The densities of the parametrized links
 
         Returns:
             CoM (np.ndarray): The CoM position
@@ -209,9 +222,9 @@ def CoM_position(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -226,8 +239,8 @@ def bias_force(
         joint_positions: np.ndarray,
         base_velocity: np.ndarray,
         joint_velocities: np.ndarray,
-        lenght_multiplier: np.ndarray,
-        density: np.ndarray,
+        length_multiplier: np.ndarray,
+        densities: np.ndarray,
     ) -> np.ndarray:
         """Returns the bias force of the floating-base dynamics equation,
         using a reduced RNEA (no acceleration and external forces)
@@ -237,6 +250,8 @@ def bias_force(
             joint_positions (np.ndarray): The joints position
             base_velocity (np.ndarray): The base velocity in mixed representation
             joint_velocities (np.ndarray): The joint velocities
+            length_multiplier (np.ndarray): The length multiplier of the parametrized links
+            densities (np.ndarray): The densities of the parametrized links
 
         Returns:
             h (np.ndarray): the bias force
@@ -245,9 +260,9 @@ def bias_force(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -266,8 +281,8 @@ def coriolis_term(
         joint_positions: np.ndarray,
         base_velocity: np.ndarray,
         joint_velocities: np.ndarray,
-        lenght_multiplier: np.ndarray,
-        density: np.ndarray,
+        length_multiplier: np.ndarray,
+        densities: np.ndarray,
     ) -> np.ndarray:
         """Returns the coriolis term of the floating-base dynamics equation,
         using a reduced RNEA (no acceleration and external forces)
@@ -277,6 +292,8 @@ def coriolis_term(
             joint_positions (np.ndarray): The joints position
             base_velocity (np.ndarray): The base velocity in mixed representation
             joint_velocities (np.ndarray): The joint velocities
+            length_multiplier (np.ndarray): The length multiplier of the parametrized links
+            densities (np.ndarray): The densities of the parametrized links
 
         Returns:
             C (np.ndarray): the Coriolis term
@@ -285,9 +302,9 @@ def coriolis_term(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -305,8 +322,8 @@ def gravity_term(
         self,
         base_transform: np.ndarray,
         joint_positions: np.ndarray,
-        lenght_multiplier: np.ndarray,
-        density: np.ndarray,
+        length_multiplier: np.ndarray,
+        densities: np.ndarray,
     ) -> np.ndarray:
         """Returns the gravity term of the floating-base dynamics equation,
         using a reduced RNEA (no acceleration and external forces)
@@ -314,6 +331,8 @@ def gravity_term(
         Args:
             base_transform (np.ndarray): The homogenous transform from base to world frame
             joint_positions (np.ndarray): The joints position
+            length_multiplier (np.ndarray): The length multiplier of the parametrized links
+            densities (np.ndarray): The densities of the parametrized links
 
         Returns:
             G (np.ndarray): the gravity term
@@ -322,9 +341,9 @@ def gravity_term(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -338,19 +357,23 @@ def gravity_term(
         ).array.squeeze()
 
     def get_total_mass(
-        self, lenght_multiplier: np.ndarray, density: np.ndarray
+        self, length_multiplier: np.ndarray, densities: np.ndarray
     ) -> float:
         """Returns the total mass of the robot
 
+        Args:
+            length_multiplier (np.ndarray): The length multiplier of the parametrized links
+            densities (np.ndarray): The densities of the parametrized links
+
         Returns:
             mass: The total mass
         """
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
diff --git a/src/adam/pytorch/computations_parametric.py b/src/adam/pytorch/computations_parametric.py
index c3007e1d..50b1aa7f 100644
--- a/src/adam/pytorch/computations_parametric.py
+++ b/src/adam/pytorch/computations_parametric.py
@@ -12,14 +12,14 @@
 
 class KinDynComputationsParametric:
     """This is a small class that retrieves robot quantities using Pytorch
-    in mixed representation, for Floating Base systems - as humanoid robots.
+    in mixed representation, for Floating Base systems - as humanoid robots. This is parametric w.r.t the link length and denisties
     """
 
     def __init__(
         self,
         urdfstring: str,
         joints_name_list: list,
-        link_name_list: list,
+        links_name_list: list,
         root_link: str = "root_link",
         gravity: np.array = torch.FloatTensor([0, 0, -9.80665, 0, 0, 0]),
     ) -> None:
@@ -27,11 +27,12 @@ def __init__(
         Args:
             urdfstring (str): path of the urdf
             joints_name_list (list): list of the actuated joints
+            links_name_list (list): list of parametric links
             root_link (str, optional): the first link. Defaults to 'root_link'.
         """
         self.math = SpatialMath()
         self.g = gravity
-        self.link_name_list = link_name_list
+        self.links_name_list = links_name_list
         self.joints_name_list = joints_name_list
         self.urdfstring = urdfstring
 
@@ -39,14 +40,16 @@ def mass_matrix(
         self,
         base_transform: torch.Tensor,
         s: torch.Tensor,
-        lenght_multiplier: torch.Tensor,
-        density: torch.Tensor,
+        length_multiplier: torch.Tensor,
+        densities: torch.Tensor,
     ) -> torch.Tensor:
         """Returns the Mass Matrix functions computed the CRBA
 
         Args:
             base_transform (torch.tensor): The homogenous transform from base to world frame
             s (torch.tensor): The joints position
+            length_multiplier (torch.tensor): The length multiplier of the parametrized links
+            densities (torch.tensor): The densities of the parametrized links
 
         Returns:
             M (torch.tensor): Mass Matrix
@@ -55,9 +58,9 @@ def mass_matrix(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -69,14 +72,16 @@ def centroidal_momentum_matrix(
         self,
         base_transform: torch.Tensor,
         s: torch.Tensor,
-        lenght_multiplier: torch.Tensor,
-        density: torch.Tensor,
+        length_multiplier: torch.Tensor,
+        densities: torch.Tensor,
     ) -> torch.Tensor:
         """Returns the Centroidal Momentum Matrix functions computed the CRBA
 
         Args:
             base_transform (torch.tensor): The homogenous transform from base to world frame
             s (torch.tensor): The joints position
+            length_multiplier (torch.tensor): The length multiplier of the parametrized links
+            densities (torch.tensor): The densities of the parametrized links
 
         Returns:
             Jcc (torch.tensor): Centroidal Momentum matrix
@@ -85,9 +90,9 @@ def centroidal_momentum_matrix(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -100,8 +105,8 @@ def forward_kinematics(
         frame,
         base_transform: torch.Tensor,
         s: torch.Tensor,
-        lenght_multiplier: torch.Tensor,
-        density: torch.Tensor,
+        length_multiplier: torch.Tensor,
+        densities: torch.Tensor,
     ) -> torch.Tensor:
         """Computes the forward kinematics relative to the specified frame
 
@@ -109,6 +114,8 @@ def forward_kinematics(
             frame (str): The frame to which the fk will be computed
             base_transform (torch.tensor): The homogenous transform from base to world frame
             s (torch.tensor): The joints position
+            length_multiplier (torch.tensor): The length multiplier of the parametrized links
+            densities (torch.tensor): The densities of the parametrized links
 
         Returns:
             T_fk (torch.tensor): The fk represented as Homogenous transformation matrix
@@ -117,9 +124,9 @@ def forward_kinematics(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -135,8 +142,8 @@ def jacobian(
         frame: str,
         base_transform: torch.Tensor,
         joint_positions: torch.Tensor,
-        lenght_multiplier: torch.Tensor,
-        density: torch.Tensor,
+        length_multiplier: torch.Tensor,
+        densities: torch.Tensor,
     ) -> torch.Tensor:
         """Returns the Jacobian relative to the specified frame
 
@@ -144,6 +151,8 @@ def jacobian(
             frame (str): The frame to which the jacobian will be computed
             base_transform (torch.tensor): The homogenous transform from base to world frame
             joint_positions (torch.tensor): The joints position
+            length_multiplier (torch.tensor): The length multiplier of the parametrized links
+            densities (torch.tensor): The densities of the parametrized links
 
         Returns:
             J_tot (torch.tensor): The Jacobian relative to the frame
@@ -152,9 +161,9 @@ def jacobian(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -165,14 +174,16 @@ def relative_jacobian(
         self,
         frame,
         joint_positions: torch.Tensor,
-        lenght_multiplier: torch.Tensor,
-        density: torch.Tensor,
+        length_multiplier: torch.Tensor,
+        densities: torch.Tensor,
     ) -> torch.Tensor:
         """Returns the Jacobian between the root link and a specified frame frames
 
         Args:
             frame (str): The tip of the chain
             joint_positions (torch.tensor): The joints position
+            length_multiplier (torch.tensor): The length multiplier of the parametrized links
+            densities (torch.tensor): The densities of the parametrized links
 
         Returns:
             J (torch.tensor): The Jacobian between the root and the frame
@@ -181,9 +192,9 @@ def relative_jacobian(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -194,14 +205,16 @@ def CoM_position(
         self,
         base_transform: torch.Tensor,
         joint_positions: torch.Tensor,
-        lenght_multiplier: torch.Tensor,
-        density: torch.Tensor,
+        length_multiplier: torch.Tensor,
+        densities: torch.Tensor,
     ) -> torch.Tensor:
         """Returns the CoM positon
 
         Args:
             base_transform (torch.tensor): The homogenous transform from base to world frame
             joint_positions (torch.tensor): The joints position
+            length_multiplier (torch.tensor): The length multiplier of the parametrized links
+            densities (torch.tensor): The densities of the parametrized links
 
         Returns:
             com (torch.tensor): The CoM position
@@ -210,9 +223,9 @@ def CoM_position(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -227,8 +240,8 @@ def bias_force(
         s: torch.Tensor,
         base_velocity: torch.Tensor,
         joint_velocities: torch.Tensor,
-        lenght_multiplier: torch.Tensor,
-        density: torch.Tensor,
+        length_multiplier: torch.Tensor,
+        densities: torch.Tensor,
     ) -> torch.Tensor:
         """Returns the bias force of the floating-base dynamics ejoint_positionsuation,
         using a reduced RNEA (no acceleration and external forces)
@@ -238,6 +251,8 @@ def bias_force(
             s (torch.tensor): The joints position
             base_velocity (torch.tensor): The base velocity in mixed representation
             joint_velocities (torch.tensor): The joints velocity
+            length_multiplier (torch.tensor): The length multiplier of the parametrized links
+            densities (torch.tensor): The densities of the parametrized links
 
         Returns:
             h (torch.tensor): the bias force
@@ -246,9 +261,9 @@ def bias_force(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -267,8 +282,8 @@ def coriolis_term(
         joint_positions: torch.Tensor,
         base_velocity: torch.Tensor,
         joint_velocities: torch.Tensor,
-        lenght_multiplier: torch.Tensor,
-        density: torch.Tensor,
+        length_multiplier: torch.Tensor,
+        densities: torch.Tensor,
     ) -> torch.Tensor:
         """Returns the coriolis term of the floating-base dynamics ejoint_positionsuation,
         using a reduced RNEA (no acceleration and external forces)
@@ -278,6 +293,8 @@ def coriolis_term(
             joint_positions (torch.tensor): The joints position
             base_velocity (torch.tensor): The base velocity in mixed representation
             joint_velocities (torch.tensor): The joints velocity
+            length_multiplier (torch.tensor): The length multiplier of the parametrized links
+            densities (torch.tensor): The densities of the parametrized links
 
         Returns:
             C (torch.tensor): the Coriolis term
@@ -286,9 +303,9 @@ def coriolis_term(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -306,8 +323,8 @@ def gravity_term(
         self,
         base_transform: torch.Tensor,
         base_positions: torch.Tensor,
-        lenght_multiplier: torch.Tensor,
-        density: torch.Tensor,
+        length_multiplier: torch.Tensor,
+        densities: torch.Tensor,
     ) -> torch.Tensor:
         """Returns the gravity term of the floating-base dynamics ejoint_positionsuation,
         using a reduced RNEA (no acceleration and external forces)
@@ -315,6 +332,8 @@ def gravity_term(
         Args:
             base_transform (torch.tensor): The homogenous transform from base to world frame
             base_positions (torch.tensor): The joints position
+            length_multiplier (torch.tensor): The length multiplier of the parametrized links
+            densities (torch.tensor): The densities of the parametrized links
 
         Returns:
             G (torch.tensor): the gravity term
@@ -322,9 +341,9 @@ def gravity_term(
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
@@ -338,19 +357,23 @@ def gravity_term(
         ).array.squeeze()
 
     def get_total_mass(
-        self, lenght_multiplier: torch.Tensor, density: torch.Tensor
+        self, length_multiplier: torch.Tensor, densities: torch.Tensor
     ) -> float:
         """Returns the total mass of the robot
 
+        Args:
+            length_multiplier (torch.tensor): The length multiplier of the parametrized links
+            densities (torch.tensor): The densities of the parametrized links
+
         Returns:
             mass: The total mass
         """
         factory = URDFParametricModelFactory(
             path=self.urdfstring,
             math=self.math,
-            link_parametric_list=self.link_name_list,
-            lenght_multiplier=lenght_multiplier,
-            density=density,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)

From 164de58da86fef2529a02fe80cbc231e3bdea059 Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Tue, 7 Nov 2023 11:33:29 +0100
Subject: [PATCH 12/19] rebase on top of main, implement jdotnu for parametric,
 update tests, implement change in representation in parametrix

---
 src/adam/casadi/computations_parametric.py    | 45 +++++++++-
 src/adam/jax/computations_parametric.py       | 62 +++++++++++++
 src/adam/numpy/computations_parametric.py     | 62 +++++++++++++
 src/adam/pytorch/computations_parametric.py   | 61 +++++++++++++
 .../test_CasADi_computations_parametric.py    | 86 +++++++++----------
 .../test_Jax_computations_parametric.py       | 64 ++++++++------
 .../test_NumPy_computations_parametric.py     | 66 ++++++++------
 .../test_pytorch_computations_parametric.py   | 18 ++++
 8 files changed, 359 insertions(+), 105 deletions(-)
 rename tests/{ => parametric}/test_CasADi_computations_parametric.py (74%)
 rename tests/{ => parametric}/test_Jax_computations_parametric.py (74%)
 rename tests/{ => parametric}/test_NumPy_computations_parametric.py (71%)
 rename tests/{ => parametric}/test_pytorch_computations_parametric.py (90%)

diff --git a/src/adam/casadi/computations_parametric.py b/src/adam/casadi/computations_parametric.py
index 0a4aad2c..f44e6a88 100644
--- a/src/adam/casadi/computations_parametric.py
+++ b/src/adam/casadi/computations_parametric.py
@@ -7,6 +7,7 @@
 
 from adam.casadi.casadi_like import SpatialMath
 from adam.core import RBDAlgorithms
+from adam.core.constants import Representations
 from adam.model import Model, URDFModelFactory, URDFParametricModelFactory
 
 
@@ -35,19 +36,29 @@ def __init__(
         n_param_link = len(links_name_list)
         self.densities = cs.SX.sym("densities", n_param_link)
         self.length_multiplier = cs.SX.sym("length_multiplier", n_param_link)
-        self.factory = URDFParametricModelFactory(
+        factory = URDFParametricModelFactory(
             path=urdfstring,
             math=math,
             links_name_list=links_name_list,
             length_multiplier=self.length_multiplier,
             densities=self.densities,
         )
-        model = Model.build(factory=self.factory, joints_name_list=joints_name_list)
+        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 set_frame_velocity_representation(
+        self, representation: Representations
+    ) -> None:
+        """Sets the representation of the velocity of the frames
+
+        Args:
+            representation (Representations): The representation of the velocity
+        """
+        self.rbdalgos.set_frame_velocity_representation(representation)
+
     def mass_matrix_fun(self) -> cs.Function:
         """Returns the Mass Matrix functions computed the CRBA
 
@@ -133,6 +144,36 @@ def relative_jacobian_fun(self, frame: str) -> cs.Function:
             "J", [s, self.length_multiplier, self.densities], [J.array], self.f_opts
         )
 
+    def jacobian_dot_fun(self, frame: str) -> cs.Function:
+        """Returns the Jacobian derivative relative to the specified frame
+
+        Args:
+            frame (str): The frame to which the jacobian will be computed
+
+        Returns:
+            J_dot (casADi function): The Jacobian derivative relative to the frame
+        """
+        base_transform = cs.SX.sym("H", 4, 4)
+        joint_positions = cs.SX.sym("s", self.NDoF)
+        base_velocity = cs.SX.sym("v_b", 6)
+        joint_velocities = cs.SX.sym("s_dot", self.NDoF)
+        J_dot = self.rbdalgos.jacobian_dot(
+            frame, base_transform, joint_positions, base_velocity, joint_velocities
+        )
+        return cs.Function(
+            "J_dot",
+            [
+                base_transform,
+                joint_positions,
+                base_velocity,
+                joint_velocities,
+                self.length_multiplier,
+                self.densities,
+            ],
+            [J_dot.array],
+            self.f_opts,
+        )
+
     def CoM_position_fun(self) -> cs.Function:
         """Returns the CoM positon
 
diff --git a/src/adam/jax/computations_parametric.py b/src/adam/jax/computations_parametric.py
index d4ccb216..043ad4a4 100644
--- a/src/adam/jax/computations_parametric.py
+++ b/src/adam/jax/computations_parametric.py
@@ -7,6 +7,7 @@
 from jax import grad, jit, vmap
 
 from adam.core.rbd_algorithms import RBDAlgorithms
+from adam.core.constants import Representations
 from adam.jax.jax_like import SpatialMath
 from adam.model import Model, URDFParametricModelFactory
 
@@ -36,6 +37,17 @@ def __init__(
         self.g = gravity
         self.urdfstring = urdfstring
         self.joints_name_list = joints_name_list
+        self.representation = Representations.MIXED_REPRESENTATION  # Default
+
+    def set_frame_velocity_representation(
+        self, representation: Representations
+    ) -> None:
+        """Sets the representation of the velocity of the frames
+
+        Args:
+            representation (Representations): The representation of the velocity
+        """
+        self.representation = representation
 
     def mass_matrix(
         self,
@@ -65,6 +77,7 @@ def mass_matrix(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         [M, _] = self.rbdalgos.crba(base_transform, joint_positions)
         return M.array
@@ -97,6 +110,7 @@ def centroidal_momentum_matrix(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         [_, Jcm] = self.rbdalgos.crba(base_transform, joint_positions)
         return Jcm.array
@@ -129,9 +143,50 @@ def relative_jacobian(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.relative_jacobian(frame, joint_positions).array
 
+    def jacobian_dot(
+        self,
+        frame: str,
+        base_transform: jnp.array,
+        joint_positions: jnp.array,
+        base_velocity: jnp.array,
+        joint_velocities: jnp.array,
+        length_multiplier: jnp.array,
+        densities: jnp.array,
+    ) -> jnp.array:
+        """Returns the Jacobian derivative relative to the specified frame
+
+        Args:
+            frame (str): The frame to which the jacobian will be computed
+            base_transform (jnp.array): The homogenous transform from base to world frame
+            joint_positions (jnp.array): The joints position
+            base_velocity (jnp.array): The base velocity in mixed representation
+            joint_velocities (jnp.array): The joint velocities
+            length_multiplier (jnp.array): The length multiplier of the parametrized links
+            densities (jnp.array): The densities of the parametrized links
+
+        Returns:
+            Jdot (jnp.array): The Jacobian derivative relative to the frame
+        """
+
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
+        self.NDoF = self.rbdalgos.NDoF
+        return self.rbdalgos.jacobian_dot(
+            frame, base_transform, joint_positions, base_velocity, joint_velocities
+        ).array
+
     def forward_kinematics(
         self,
         frame: str,
@@ -162,6 +217,7 @@ def forward_kinematics(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.forward_kinematics(
             frame, base_transform, joint_positions
@@ -205,6 +261,7 @@ def jacobian(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.jacobian(frame, base_transform, joint_positions).array
 
@@ -242,6 +299,7 @@ def bias_force(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.rnea(
             base_transform, joint_positions, base_velocity, s_dot, self.g
@@ -280,6 +338,7 @@ def coriolis_term(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.rnea(
             base_transform,
@@ -318,6 +377,7 @@ def gravity_term(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.rnea(
             base_transform,
@@ -355,6 +415,7 @@ def CoM_position(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.CoM_position(
             base_transform, joint_positions
@@ -381,5 +442,6 @@ def get_total_mass(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.get_total_mass()
diff --git a/src/adam/numpy/computations_parametric.py b/src/adam/numpy/computations_parametric.py
index 1280b33a..6d3ed9d5 100644
--- a/src/adam/numpy/computations_parametric.py
+++ b/src/adam/numpy/computations_parametric.py
@@ -5,6 +5,7 @@
 import numpy as np
 
 from adam.core.rbd_algorithms import RBDAlgorithms
+from adam.core.constants import Representations
 from adam.model import Model, URDFParametricModelFactory
 from adam.numpy.numpy_like import SpatialMath
 
@@ -34,6 +35,17 @@ def __init__(
         self.g = gravity
         self.urdfstring = urdfstring
         self.joints_name_list = joints_name_list
+        self.representation = Representations.MIXED_REPRESENTATION  # Default
+
+    def set_frame_velocity_representation(
+        self, representation: Representations
+    ) -> None:
+        """Sets the representation of the velocity of the frames
+
+        Args:
+            representation (Representations): The representation of the velocity
+        """
+        self.representation = representation
 
     def mass_matrix(
         self,
@@ -63,6 +75,7 @@ def mass_matrix(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = model.NDoF
         [M, _] = self.rbdalgos.crba(base_transform, joint_positions)
         return M.array
@@ -95,6 +108,7 @@ def centroidal_momentum_matrix(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = model.NDoF
         [_, Jcm] = self.rbdalgos.crba(base_transform, s)
         return Jcm.array
@@ -129,6 +143,7 @@ def forward_kinematics(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = model.NDoF
         return self.rbdalgos.forward_kinematics(
             frame, base_transform, joint_positions
@@ -164,6 +179,7 @@ def jacobian(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = model.NDoF
         return self.rbdalgos.jacobian(
             frame, base_transform, joint_positions
@@ -197,9 +213,50 @@ def relative_jacobian(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = model.NDoF
         return self.rbdalgos.relative_jacobian(frame, joint_positions).array
 
+    def jacobian_dot(
+        self,
+        frame: str,
+        base_transform: np.ndarray,
+        joint_positions: np.ndarray,
+        base_velocity: np.ndarray,
+        joint_velocities: np.ndarray,
+        length_multiplier: np.ndarray,
+        densities: np.ndarray,
+    ) -> np.ndarray:
+
+        """Returns the Jacobian derivative relative to the specified frame
+
+        Args:
+            frame (str): The frame to which the jacobian will be computed
+            base_transform (np.ndarray): The homogenous transform from base to world frame
+            joint_positions (np.ndarray): The joints position
+            base_velocity (np.ndarray): The base velocity in mixed representation
+            joint_velocities (np.ndarray): The joint velocities
+            length_multiplier (np.ndarray): The length multiplier of the parametrized links
+            densities (np.ndarray): The densities of the parametrized links
+
+        Returns:
+            Jdot (np.ndarray): The Jacobian derivative relative to the frame
+        """
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
+        self.NDoF = model.NDoF
+        return self.rbdalgos.jacobian_dot(
+            frame, base_transform, joint_positions, base_velocity, joint_velocities
+        ).array.squeeze()
+
     def CoM_position(
         self,
         base_transform: np.ndarray,
@@ -228,6 +285,7 @@ def CoM_position(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = model.NDoF
         return self.rbdalgos.CoM_position(
             base_transform, joint_positions
@@ -266,6 +324,7 @@ def bias_force(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = model.NDoF
         return self.rbdalgos.rnea(
             base_transform,
@@ -308,6 +367,7 @@ def coriolis_term(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = model.NDoF
         # set in the bias force computation the gravity term to zero
         return self.rbdalgos.rnea(
@@ -347,6 +407,7 @@ def gravity_term(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = model.NDoF
         return self.rbdalgos.rnea(
             base_transform,
@@ -377,5 +438,6 @@ def get_total_mass(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = model.NDoF
         return self.rbdalgos.get_total_mass()
diff --git a/src/adam/pytorch/computations_parametric.py b/src/adam/pytorch/computations_parametric.py
index 50b1aa7f..4584476a 100644
--- a/src/adam/pytorch/computations_parametric.py
+++ b/src/adam/pytorch/computations_parametric.py
@@ -6,6 +6,7 @@
 import torch
 
 from adam.core.rbd_algorithms import RBDAlgorithms
+from adam.core.constants import Representations
 from adam.model import Model, URDFModelFactory, URDFParametricModelFactory
 from adam.pytorch.torch_like import SpatialMath
 
@@ -35,6 +36,17 @@ def __init__(
         self.links_name_list = links_name_list
         self.joints_name_list = joints_name_list
         self.urdfstring = urdfstring
+        self.representation = Representations.MIXED_REPRESENTATION  # Default
+
+    def set_frame_velocity_representation(
+        self, representation: Representations
+    ) -> None:
+        """Sets the representation of the velocity of the frames
+
+        Args:
+            representation (Representations): The representation of the velocity
+        """
+        self.representation = representation
 
     def mass_matrix(
         self,
@@ -64,6 +76,7 @@ def mass_matrix(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         [M, _] = self.rbdalgos.crba(base_transform, s)
         return M.array
@@ -96,6 +109,7 @@ def centroidal_momentum_matrix(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         [_, Jcm] = self.rbdalgos.crba(base_transform, s)
         return Jcm.array
@@ -130,6 +144,7 @@ def forward_kinematics(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return (
             self.rbdalgos.forward_kinematics(
@@ -167,6 +182,7 @@ def jacobian(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.jacobian(frame, base_transform, joint_positions).array
 
@@ -198,9 +214,49 @@ def relative_jacobian(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.relative_jacobian(frame, joint_positions).array
 
+    def jacobian_dot(
+        self,
+        frame: str,
+        base_transform: torch.Tensor,
+        joint_positions: torch.Tensor,
+        base_velocity: torch.Tensor,
+        joint_velocities: torch.Tensor,
+        length_multiplier: torch.Tensor,
+        densities: torch.Tensor,
+    ) -> torch.Tensor:
+        """Returns the Jacobian derivative relative to the specified frame
+
+        Args:
+            frame (str): The frame to which the jacobian will be computed
+            base_transform (torch.Tensor): The homogenous transform from base to world frame
+            joint_positions (torch.Tensor): The joints position
+            base_velocity (torch.Tensor): The base velocity in mixed representation
+            joint_velocities (torch.Tensor): The joint velocities
+            length_multiplier (torch.tensor): The length multiplier of the parametrized links
+            densities (torch.tensor): The densities of the parametrized links
+
+        Returns:
+            Jdot (torch.Tensor): The Jacobian derivative relative to the frame
+        """
+        factory = URDFParametricModelFactory(
+            path=self.urdfstring,
+            math=self.math,
+            links_name_list=self.links_name_list,
+            length_multiplier=length_multiplier,
+            densities=densities,
+        )
+        model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
+        self.NDoF = self.rbdalgos.NDoF
+        return self.rbdalgos.jacobian_dot(
+            frame, base_transform, joint_positions, base_velocity, joint_velocities
+        ).array
+
     def CoM_position(
         self,
         base_transform: torch.Tensor,
@@ -229,6 +285,7 @@ def CoM_position(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.CoM_position(
             base_transform, joint_positions
@@ -267,6 +324,7 @@ def bias_force(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.rnea(
             base_transform,
@@ -309,6 +367,7 @@ def coriolis_term(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         # set in the bias force computation the gravity term to zero
         return self.rbdalgos.rnea(
@@ -347,6 +406,7 @@ def gravity_term(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.rnea(
             base_transform,
@@ -377,5 +437,6 @@ def get_total_mass(
         )
         model = Model.build(factory=factory, joints_name_list=self.joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=self.math)
+        self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.get_total_mass()
diff --git a/tests/test_CasADi_computations_parametric.py b/tests/parametric/test_CasADi_computations_parametric.py
similarity index 74%
rename from tests/test_CasADi_computations_parametric.py
rename to tests/parametric/test_CasADi_computations_parametric.py
index 42d42e54..f6adddc3 100644
--- a/tests/test_CasADi_computations_parametric.py
+++ b/tests/parametric/test_CasADi_computations_parametric.py
@@ -15,6 +15,7 @@
 from adam.geometry import utils
 import tempfile
 from git import Repo
+from adam import Representations
 
 # Getting stickbot urdf file
 temp_dir = tempfile.TemporaryDirectory()
@@ -49,30 +50,6 @@
 ]
 
 
-def ComputeOriginalDensity(kinDyn, link_name):
-    link_original = kinDyn.factory.get_element_by_name(link_name)
-    mass = link_original.inertial.mass
-    volume = 0
-    visual_obj = link_original.visuals[0]
-    if type(visual_obj.geometry) is urdf_parser_py.urdf.Box:
-        width = link_original.visuals[0].geometry.size[0]
-        depth = link_original.visuals[0].geometry.size[2]
-        height = link_original.visuals[0].geometry.size[1]
-        volume = width * depth * height
-    if type(visual_obj.geometry) is urdf_parser_py.urdf.Cylinder:
-        length = link_original.visuals[0].geometry.length
-        radius = link_original.visuals[0].geometry.radius
-        volume = math.pi * radius ** 2 * length
-    if type(visual_obj.geometry) is urdf_parser_py.urdf.Sphere:
-        radius = link_original.visuals[0].geometry.radius
-        volume = 4 * (math.pi * radius ** 3) / 3
-    return mass / volume
-
-
-def SX2DM(x):
-    return cs.DM(x)
-
-
 logging.basicConfig(level=logging.DEBUG)
 logging.debug("Showing the robot tree.")
 
@@ -84,9 +61,8 @@ def SX2DM(x):
 comp_w_hardware = KinDynComputationsParametric(
     model_path, joints_name_list, link_name_list, root_link
 )
-original_density = []
-for item in link_name_list:
-    original_density += [ComputeOriginalDensity(comp_w_hardware, item)]
+
+original_density = [628.0724496264945]
 original_length = np.ones(len(link_name_list))
 
 
@@ -108,8 +84,8 @@ def SX2DM(x):
 def test_mass_matrix():
     M = comp.mass_matrix_fun()
     M_with_hardware = comp_w_hardware.mass_matrix_fun()
-    mass_test = SX2DM(M(H_b, s_))
-    mass_test_hardware = SX2DM(
+    mass_test = cs.DM(M(H_b, s_))
+    mass_test_hardware = cs.DM(
         M_with_hardware(H_b, s_, original_length, original_density)
     )
     assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5)
@@ -118,8 +94,8 @@ def test_mass_matrix():
 def test_CMM():
     Jcm = comp.centroidal_momentum_matrix_fun()
     Jcm_with_hardware = comp_w_hardware.centroidal_momentum_matrix_fun()
-    Jcm_test = SX2DM(Jcm(H_b, s_))
-    Jcm_test_hardware = SX2DM(
+    Jcm_test = cs.DM(Jcm(H_b, s_))
+    Jcm_test_hardware = cs.DM(
         Jcm_with_hardware(H_b, s_, original_length, original_density)
     )
     assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5)
@@ -128,8 +104,8 @@ def test_CMM():
 def test_CoM_pos():
     com_f = comp.CoM_position_fun()
     com_with_hardware_f = comp_w_hardware.CoM_position_fun()
-    CoM_cs = SX2DM(com_f(H_b, s_))
-    CoM_hardware = SX2DM(
+    CoM_cs = cs.DM(com_f(H_b, s_))
+    CoM_hardware = cs.DM(
         com_with_hardware_f(H_b, s_, original_length, original_density)
     )
     assert CoM_cs - CoM_hardware == pytest.approx(0.0, abs=1e-5)
@@ -138,15 +114,15 @@ def test_CoM_pos():
 def test_total_mass():
     mass = comp.get_total_mass()
     mass_hardware_fun = comp_w_hardware.get_total_mass()
-    mass_hardware = SX2DM(mass_hardware_fun(original_length, original_density))
+    mass_hardware = cs.DM(mass_hardware_fun(original_length, original_density))
     assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5)
 
 
 def test_jacobian():
     J_tot = comp.jacobian_fun("l_sole")
     J_tot_with_hardware = comp_w_hardware.jacobian_fun("l_sole")
-    J_test = SX2DM(J_tot(H_b, s_))
-    J_test_hardware = SX2DM(
+    J_test = cs.DM(J_tot(H_b, s_))
+    J_test_hardware = cs.DM(
         J_tot_with_hardware(H_b, s_, original_length, original_density)
     )
     assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
@@ -154,19 +130,35 @@ def test_jacobian():
 
 def test_jacobian_non_actuated():
     J_tot = comp.jacobian_fun("head")
-    J_test = SX2DM(J_tot(H_b, s_))
+    J_test = cs.DM(J_tot(H_b, s_))
     J_tot_with_hardware = comp_w_hardware.jacobian_fun("head")
-    J_tot_with_hardware_test = SX2DM(
+    J_tot_with_hardware_test = cs.DM(
         J_tot_with_hardware(H_b, s_, original_length, original_density)
     )
     assert J_test - J_tot_with_hardware_test == pytest.approx(0.0, abs=1e-5)
 
 
+def test_jacobian_dot():
+    J_dot = comp.jacobian_dot_fun("l_sole")
+    J_dot_hardware = comp_w_hardware.jacobian_dot_fun("l_sole")
+    J_dot_nu_test = cs.DM(
+        J_dot(H_b, joints_val, base_vel, joints_dot_val)
+        @ np.concatenate((base_vel, joints_dot_val))
+    )
+    J_dot_nu_test2 = cs.DM(
+        J_dot_hardware(
+            H_b, joints_val, base_vel, joints_dot_val, original_length, original_density
+        )
+        @ np.concatenate((base_vel, joints_dot_val))
+    )
+    assert J_dot_nu_test - J_dot_nu_test2 == pytest.approx(0.0, abs=1e-5)
+
+
 def test_fk():
     T = comp.forward_kinematics_fun("l_sole")
-    H_test = SX2DM(T(H_b, s_))
+    H_test = cs.DM(T(H_b, s_))
     T_with_hardware = comp_w_hardware.forward_kinematics_fun("l_sole")
-    H_with_hardware_test = SX2DM(
+    H_with_hardware_test = cs.DM(
         T_with_hardware(H_b, s_, original_length, original_density)
     )
     assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
@@ -175,9 +167,9 @@ def test_fk():
 
 def test_fk_non_actuated():
     T = comp.forward_kinematics_fun("head")
-    H_test = SX2DM(T(H_b, s_))
+    H_test = cs.DM(T(H_b, s_))
     T_with_hardware = comp_w_hardware.forward_kinematics_fun("head")
-    H_with_hardware_test = SX2DM(
+    H_with_hardware_test = cs.DM(
         T_with_hardware(H_b, s_, original_length, original_density)
     )
     assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
@@ -186,10 +178,10 @@ def test_fk_non_actuated():
 
 def test_bias_force():
     h = comp.bias_force_fun()
-    h_test = SX2DM(h(H_b, s_, vb_, s_dot_))
+    h_test = cs.DM(h(H_b, s_, vb_, s_dot_))
 
     h_with_hardware = comp_w_hardware.bias_force_fun()
-    h_with_hardware_test = SX2DM(
+    h_with_hardware_test = cs.DM(
         h_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density)
     )
     assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4)
@@ -197,9 +189,9 @@ def test_bias_force():
 
 def test_coriolis_term():
     C = comp.coriolis_term_fun()
-    C_test = SX2DM(C(H_b, s_, vb_, s_dot_))
+    C_test = cs.DM(C(H_b, s_, vb_, s_dot_))
     C_with_hardware = comp_w_hardware.coriolis_term_fun()
-    C_with_hardware_test = SX2DM(
+    C_with_hardware_test = cs.DM(
         C_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density)
     )
     assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4)
@@ -207,7 +199,7 @@ def test_coriolis_term():
 
 def test_gravity_term():
     G = comp.gravity_term_fun()
-    G_test = SX2DM(G(H_b, s_))
+    G_test = cs.DM(G(H_b, s_))
     G_with_hardware = comp_w_hardware.gravity_term_fun()
     G_with_hardware_test = G_with_hardware(H_b, s_, original_length, original_density)
     assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4)
diff --git a/tests/test_Jax_computations_parametric.py b/tests/parametric/test_Jax_computations_parametric.py
similarity index 74%
rename from tests/test_Jax_computations_parametric.py
rename to tests/parametric/test_Jax_computations_parametric.py
index 2d245055..7f78c11a 100644
--- a/tests/test_Jax_computations_parametric.py
+++ b/tests/parametric/test_Jax_computations_parametric.py
@@ -16,6 +16,7 @@
 from adam.geometry import utils
 import tempfile
 from git import Repo
+from adam import Representations
 
 np.random.seed(42)
 config.update("jax_enable_x64", True)
@@ -68,6 +69,7 @@ def SX2DM(x):
 comp_w_hardware = KinDynComputationsParametric(
     model_path, joints_name_list, link_name_list, root_link
 )
+
 original_density = [628.0724496264945]
 original_length = np.ones(len(link_name_list))
 
@@ -88,27 +90,27 @@ def SX2DM(x):
 
 def test_mass_matrix():
     mass_test = comp.mass_matrix(H_b, s_)
-    mass_test_hardware = np.array(
-        comp_w_hardware.mass_matrix(H_b, s_, original_length, original_density)
+    mass_test_hardware = comp_w_hardware.mass_matrix(
+        H_b, s_, original_length, original_density
     )
     assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5)
 
 
 def test_CMM():
     Jcm_test = comp.centroidal_momentum_matrix(H_b, s_)
-    Jcm_test_hardware = np.array(
-        comp_w_hardware.centroidal_momentum_matrix(
-            H_b, s_, original_length, original_density
-        )
+    Jcm_test_hardware = comp_w_hardware.centroidal_momentum_matrix(
+        H_b, s_, original_length, original_density
     )
+
     assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5)
 
 
 def test_CoM_pos():
     CoM_test = comp.CoM_position(H_b, s_)
-    CoM_hardware = np.array(
-        comp_w_hardware.CoM_position(H_b, s_, original_length, original_density)
+    CoM_hardware = comp_w_hardware.CoM_position(
+        H_b, s_, original_length, original_density
     )
+
     assert CoM_test - CoM_hardware == pytest.approx(0.0, abs=1e-5)
 
 
@@ -120,26 +122,38 @@ def test_total_mass():
 
 def test_jacobian():
     J_test = comp.jacobian("l_sole", H_b, s_)
-    J_test_hardware = np.array(
-        comp_w_hardware.jacobian("l_sole", H_b, s_, original_length, original_density)
+    J_test_hardware = comp_w_hardware.jacobian(
+        "l_sole", H_b, s_, original_length, original_density
     )
     assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
 
 
 def test_jacobian_non_actuated():
     J_test = comp.jacobian("head", H_b, s_)
-    J_test_hardware = np.array(
-        comp_w_hardware.jacobian("head", H_b, s_, original_length, original_density)
+    J_test_hardware = comp_w_hardware.jacobian(
+        "head", H_b, s_, original_length, original_density
     )
     assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
 
 
+def test_jacobian_dot():
+    J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val)
+    J_dot_hardware = comp_w_hardware.jacobian_dot(
+        "l_sole",
+        H_b,
+        joints_val,
+        base_vel,
+        joints_dot_val,
+        original_length,
+        original_density,
+    )
+    assert J_dot - J_dot_hardware == pytest.approx(0.0, abs=1e-5)
+
+
 def test_fk():
     H_test = comp.forward_kinematics("l_sole", H_b, s_)
-    H_with_hardware_test = np.array(
-        comp_w_hardware.forward_kinematics(
-            "l_sole", H_b, s_, original_length, original_density
-        )
+    H_with_hardware_test = comp_w_hardware.forward_kinematics(
+        "l_sole", H_b, s_, original_length, original_density
     )
     assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
     assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
@@ -147,10 +161,8 @@ def test_fk():
 
 def test_fk_non_actuated():
     H_test = comp.forward_kinematics("head", H_b, s_)
-    H_with_hardware_test = np.array(
-        comp_w_hardware.forward_kinematics(
-            "head", H_b, s_, original_length, original_density
-        )
+    H_with_hardware_test = comp_w_hardware.forward_kinematics(
+        "head", H_b, s_, original_length, original_density
     )
     assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
     assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
@@ -158,20 +170,16 @@ def test_fk_non_actuated():
 
 def test_bias_force():
     h_test = comp.bias_force(H_b, s_, vb_, s_dot_)
-    h_with_hardware_test = np.array(
-        comp_w_hardware.bias_force(
-            H_b, s_, vb_, s_dot_, original_length, original_density
-        )
+    h_with_hardware_test = comp_w_hardware.bias_force(
+        H_b, s_, vb_, s_dot_, original_length, original_density
     )
     assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4)
 
 
 def test_coriolis_term():
     C_test = comp.coriolis_term(H_b, s_, vb_, s_dot_)
-    C_with_hardware_test = np.array(
-        comp_w_hardware.coriolis_term(
-            H_b, s_, vb_, s_dot_, original_length, original_density
-        )
+    C_with_hardware_test = comp_w_hardware.coriolis_term(
+        H_b, s_, vb_, s_dot_, original_length, original_density
     )
     assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4)
 
diff --git a/tests/test_NumPy_computations_parametric.py b/tests/parametric/test_NumPy_computations_parametric.py
similarity index 71%
rename from tests/test_NumPy_computations_parametric.py
rename to tests/parametric/test_NumPy_computations_parametric.py
index 8bfddf38..12edcace 100644
--- a/tests/test_NumPy_computations_parametric.py
+++ b/tests/parametric/test_NumPy_computations_parametric.py
@@ -14,6 +14,7 @@
 from adam.geometry import utils
 import tempfile
 from git import Repo
+from adam import Representations
 
 np.random.seed(42)
 
@@ -65,6 +66,9 @@ def SX2DM(x):
 comp_w_hardware = KinDynComputationsParametric(
     model_path, joints_name_list, link_name_list, root_link
 )
+# comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION)
+# comp_w_hardware.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION)
+
 original_density = [628.0724496264945]
 original_length = np.ones(len(link_name_list))
 
@@ -85,26 +89,26 @@ def SX2DM(x):
 
 def test_mass_matrix():
     mass_test = comp.mass_matrix(H_b, s_)
-    mass_test_hardware = np.array(
-        comp_w_hardware.mass_matrix(H_b, s_, original_length, original_density)
+    mass_test_hardware = comp_w_hardware.mass_matrix(
+        H_b, s_, original_length, original_density
     )
+
     assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5)
 
 
 def test_CMM():
     Jcm_test = comp.centroidal_momentum_matrix(H_b, s_)
-    Jcm_test_hardware = np.array(
-        comp_w_hardware.centroidal_momentum_matrix(
-            H_b, s_, original_length, original_density
-        )
+    Jcm_test_hardware = comp_w_hardware.centroidal_momentum_matrix(
+        H_b, s_, original_length, original_density
     )
+
     assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5)
 
 
 def test_CoM_pos():
     CoM_test = comp.CoM_position(H_b, s_)
-    CoM_hardware = np.array(
-        comp_w_hardware.CoM_position(H_b, s_, original_length, original_density)
+    CoM_hardware = comp_w_hardware.CoM_position(
+        H_b, s_, original_length, original_density
     )
     assert CoM_test - CoM_hardware == pytest.approx(0.0, abs=1e-5)
 
@@ -117,26 +121,38 @@ def test_total_mass():
 
 def test_jacobian():
     J_test = comp.jacobian("l_sole", H_b, s_)
-    J_test_hardware = np.array(
-        comp_w_hardware.jacobian("l_sole", H_b, s_, original_length, original_density)
+    J_test_hardware = comp_w_hardware.jacobian(
+        "l_sole", H_b, s_, original_length, original_density
     )
     assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
 
 
 def test_jacobian_non_actuated():
     J_test = comp.jacobian("head", H_b, s_)
-    J_test_hardware = np.array(
-        comp_w_hardware.jacobian("head", H_b, s_, original_length, original_density)
+    J_test_hardware = comp_w_hardware.jacobian(
+        "head", H_b, s_, original_length, original_density
     )
     assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
 
 
+def test_jacobian_dot():
+    J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val)
+    J_dot_w_hardware = comp_w_hardware.jacobian_dot(
+        "l_sole",
+        H_b,
+        joints_val,
+        base_vel,
+        joints_dot_val,
+        original_length,
+        original_density,
+    )
+    assert J_dot - J_dot_w_hardware == pytest.approx(0.0, abs=1e-5)
+
+
 def test_fk():
     H_test = comp.forward_kinematics("l_sole", H_b, s_)
-    H_with_hardware_test = np.array(
-        comp_w_hardware.forward_kinematics(
-            "l_sole", H_b, s_, original_length, original_density
-        )
+    H_with_hardware_test = comp_w_hardware.forward_kinematics(
+        "l_sole", H_b, s_, original_length, original_density
     )
     assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
     assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
@@ -144,10 +160,8 @@ def test_fk():
 
 def test_fk_non_actuated():
     H_test = comp.forward_kinematics("head", H_b, s_)
-    H_with_hardware_test = np.array(
-        comp_w_hardware.forward_kinematics(
-            "head", H_b, s_, original_length, original_density
-        )
+    H_with_hardware_test = comp_w_hardware.forward_kinematics(
+        "head", H_b, s_, original_length, original_density
     )
     assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5)
     assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5)
@@ -155,20 +169,16 @@ def test_fk_non_actuated():
 
 def test_bias_force():
     h_test = comp.bias_force(H_b, s_, vb_, s_dot_)
-    h_with_hardware_test = np.array(
-        comp_w_hardware.bias_force(
-            H_b, s_, vb_, s_dot_, original_length, original_density
-        )
+    h_with_hardware_test = comp_w_hardware.bias_force(
+        H_b, s_, vb_, s_dot_, original_length, original_density
     )
     assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4)
 
 
 def test_coriolis_term():
     C_test = comp.coriolis_term(H_b, s_, vb_, s_dot_)
-    C_with_hardware_test = np.array(
-        comp_w_hardware.coriolis_term(
-            H_b, s_, vb_, s_dot_, original_length, original_density
-        )
+    C_with_hardware_test = comp_w_hardware.coriolis_term(
+        H_b, s_, vb_, s_dot_, original_length, original_density
     )
     assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4)
 
diff --git a/tests/test_pytorch_computations_parametric.py b/tests/parametric/test_pytorch_computations_parametric.py
similarity index 90%
rename from tests/test_pytorch_computations_parametric.py
rename to tests/parametric/test_pytorch_computations_parametric.py
index e085f336..7c25061d 100644
--- a/tests/test_pytorch_computations_parametric.py
+++ b/tests/parametric/test_pytorch_computations_parametric.py
@@ -15,6 +15,7 @@
 from adam.geometry import utils
 import tempfile
 from git import Repo
+from adam import Representations
 
 np.random.seed(42)
 torch.set_default_dtype(torch.float64)
@@ -67,6 +68,9 @@ def SX2DM(x):
 comp_w_hardware = KinDynComputationsParametric(
     model_path, joints_name_list, link_name_list, root_link
 )
+# comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION)
+# comp_w_hardware.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION)
+
 original_density = [628.0724496264945]
 original_length = np.ones(len(link_name_list))
 
@@ -133,6 +137,20 @@ def test_jacobian_non_actuated():
     assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5)
 
 
+def test_jacobian_dot():
+    J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val)
+    J_dot_hardware = comp_w_hardware.jacobian_dot(
+        "l_sole",
+        H_b,
+        joints_val,
+        base_vel,
+        joints_dot_val,
+        original_length,
+        original_density,
+    )
+    assert J_dot - J_dot_hardware == pytest.approx(0.0, abs=1e-5)
+
+
 def test_fk():
     H_test = np.array(comp.forward_kinematics("l_sole", H_b, s_))
     H_with_hardware_test = np.array(

From 8bf4fdab5d2e417abe0caa57e00dbbc61da2208b Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Tue, 7 Nov 2023 11:39:13 +0100
Subject: [PATCH 13/19] update CI

---
 ci_env.yml | 1 +
 setup.cfg  | 1 +
 2 files changed, 2 insertions(+)

diff --git a/ci_env.yml b/ci_env.yml
index 114c442f..9df3d792 100644
--- a/ci_env.yml
+++ b/ci_env.yml
@@ -16,3 +16,4 @@ dependencies:
   - pytest-repeat
   - icub-models
   - idyntree
+  - gitpython 
diff --git a/setup.cfg b/setup.cfg
index 95729eae..b457f256 100644
--- a/setup.cfg
+++ b/setup.cfg
@@ -53,6 +53,7 @@ test =
         idyntree
         icub-models
         black
+        gitpython
 all =
         jax
         jaxlib

From 123462df5535e4888503a9e8b76eab01af933747 Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Tue, 7 Nov 2023 11:49:12 +0100
Subject: [PATCH 14/19] fix urdf parser py version

---
 setup.cfg | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/setup.cfg b/setup.cfg
index b457f256..cf6f90fe 100644
--- a/setup.cfg
+++ b/setup.cfg
@@ -31,7 +31,7 @@ install_requires =
         scipy
         casadi
         prettytable
-        urdf_parser_py
+        urdf_parser_py==0.0.4
 
 [options.packages.find]
 where = src

From 0903bd114e51ce320274dead30fb7b592ac5f93f Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Tue, 7 Nov 2023 12:30:47 +0100
Subject: [PATCH 15/19] remove fix urdf parser py version

---
 setup.cfg | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/setup.cfg b/setup.cfg
index cf6f90fe..b457f256 100644
--- a/setup.cfg
+++ b/setup.cfg
@@ -31,7 +31,7 @@ install_requires =
         scipy
         casadi
         prettytable
-        urdf_parser_py==0.0.4
+        urdf_parser_py
 
 [options.packages.find]
 where = src

From 5bec8244823cbb87e864b2d5e5692b7227401d8e Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Tue, 7 Nov 2023 17:05:48 +0100
Subject: [PATCH 16/19] remove encoding from stickbot for tests with hardware
 parameters

---
 tests/parametric/test_CasADi_computations_parametric.py  | 9 +++++++++
 tests/parametric/test_Jax_computations_parametric.py     | 9 +++++++++
 tests/parametric/test_NumPy_computations_parametric.py   | 9 +++++++++
 tests/parametric/test_pytorch_computations_parametric.py | 9 +++++++++
 4 files changed, 36 insertions(+)

diff --git a/tests/parametric/test_CasADi_computations_parametric.py b/tests/parametric/test_CasADi_computations_parametric.py
index f6adddc3..6bd05eea 100644
--- a/tests/parametric/test_CasADi_computations_parametric.py
+++ b/tests/parametric/test_CasADi_computations_parametric.py
@@ -23,6 +23,15 @@
 Repo.clone_from(git_url, temp_dir.name)
 model_path = temp_dir.name + "/models/stickBot/model.urdf"
 
+## Ack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49
+robot_file_read = open(model_path, "r")
+robot_urdf_string = robot_file_read.read()
+robot_urdf_string = robot_urdf_string.replace("<?xml", "")
+robot_urdf_string = robot_urdf_string.replace("version='1.0'", "")
+robot_urdf_string = robot_urdf_string.replace("encoding='UTF-8'?>", "")
+robot_file_write = open(model_path, "w")
+robot_file_write.write(robot_urdf_string)
+
 joints_name_list = [
     "torso_pitch",
     "torso_roll",
diff --git a/tests/parametric/test_Jax_computations_parametric.py b/tests/parametric/test_Jax_computations_parametric.py
index 7f78c11a..545b4853 100644
--- a/tests/parametric/test_Jax_computations_parametric.py
+++ b/tests/parametric/test_Jax_computations_parametric.py
@@ -27,6 +27,15 @@
 Repo.clone_from(git_url, temp_dir.name)
 model_path = temp_dir.name + "/models/stickBot/model.urdf"
 
+## Ack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49
+robot_file_read = open(model_path, "r")
+robot_urdf_string = robot_file_read.read()
+robot_urdf_string = robot_urdf_string.replace("<?xml", "")
+robot_urdf_string = robot_urdf_string.replace("version='1.0'", "")
+robot_urdf_string = robot_urdf_string.replace("encoding='UTF-8'?>", "")
+robot_file_write = open(model_path, "w")
+robot_file_write.write(robot_urdf_string)
+
 joints_name_list = [
     "torso_pitch",
     "torso_roll",
diff --git a/tests/parametric/test_NumPy_computations_parametric.py b/tests/parametric/test_NumPy_computations_parametric.py
index 12edcace..ccc3b30b 100644
--- a/tests/parametric/test_NumPy_computations_parametric.py
+++ b/tests/parametric/test_NumPy_computations_parametric.py
@@ -24,6 +24,15 @@
 Repo.clone_from(git_url, temp_dir.name)
 model_path = temp_dir.name + "/models/stickBot/model.urdf"
 
+## Ack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49
+robot_file_read = open(model_path, "r")
+robot_urdf_string = robot_file_read.read()
+robot_urdf_string = robot_urdf_string.replace("<?xml", "")
+robot_urdf_string = robot_urdf_string.replace("version='1.0'", "")
+robot_urdf_string = robot_urdf_string.replace("encoding='UTF-8'?>", "")
+robot_file_write = open(model_path, "w")
+robot_file_write.write(robot_urdf_string)
+
 joints_name_list = [
     "torso_pitch",
     "torso_roll",
diff --git a/tests/parametric/test_pytorch_computations_parametric.py b/tests/parametric/test_pytorch_computations_parametric.py
index 7c25061d..8e48bfc4 100644
--- a/tests/parametric/test_pytorch_computations_parametric.py
+++ b/tests/parametric/test_pytorch_computations_parametric.py
@@ -26,6 +26,15 @@
 Repo.clone_from(git_url, temp_dir.name)
 model_path = temp_dir.name + "/models/stickBot/model.urdf"
 
+## Ack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49
+robot_file_read = open(model_path, "r")
+robot_urdf_string = robot_file_read.read()
+robot_urdf_string = robot_urdf_string.replace("<?xml", "")
+robot_urdf_string = robot_urdf_string.replace("version='1.0'", "")
+robot_urdf_string = robot_urdf_string.replace("encoding='UTF-8'?>", "")
+robot_file_write = open(model_path, "w")
+robot_file_write.write(robot_urdf_string)
+
 joints_name_list = [
     "torso_pitch",
     "torso_roll",

From 5bdcf9c3c575dd6d15f6265b5c967a7e8bf38e3a Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Thu, 9 Nov 2023 15:05:09 +0100
Subject: [PATCH 17/19] refactor of parmetric implementation

---
 src/adam/casadi/__init__.py                                | 1 -
 src/adam/model/__init__.py                                 | 3 ---
 src/adam/model/parametric_factories/__init__.py            | 0
 src/adam/parametric/__init__.py                            | 3 +++
 src/adam/parametric/casadi/__init__.py                     | 4 ++++
 .../{ => parametric}/casadi/computations_parametric.py     | 3 ++-
 src/adam/parametric/jax/__init__.py                        | 5 +++++
 src/adam/{ => parametric}/jax/computations_parametric.py   | 3 ++-
 src/adam/parametric/model/__init__.py                      | 7 +++++++
 src/adam/parametric/model/parametric_factories/__init__.py | 3 +++
 .../model/parametric_factories/parametric_joint.py         | 2 +-
 .../model/parametric_factories/parametric_link.py          | 0
 .../model/parametric_factories/parametric_model.py         | 2 +-
 src/adam/parametric/numpy/__init__.py                      | 5 +++++
 src/adam/{ => parametric}/numpy/computations_parametric.py | 3 ++-
 src/adam/parametric/pytorch/__init__.py                    | 5 +++++
 .../{ => parametric}/pytorch/computations_parametric.py    | 3 ++-
 tests/parametric/test_CasADi_computations_parametric.py    | 3 +--
 tests/parametric/test_Jax_computations_parametric.py       | 2 +-
 tests/parametric/test_NumPy_computations_parametric.py     | 2 +-
 tests/parametric/test_pytorch_computations_parametric.py   | 2 +-
 21 files changed, 46 insertions(+), 15 deletions(-)
 delete mode 100644 src/adam/model/parametric_factories/__init__.py
 create mode 100644 src/adam/parametric/__init__.py
 create mode 100644 src/adam/parametric/casadi/__init__.py
 rename src/adam/{ => parametric}/casadi/computations_parametric.py (99%)
 create mode 100644 src/adam/parametric/jax/__init__.py
 rename src/adam/{ => parametric}/jax/computations_parametric.py (99%)
 create mode 100644 src/adam/parametric/model/__init__.py
 create mode 100644 src/adam/parametric/model/parametric_factories/__init__.py
 rename src/adam/{ => parametric}/model/parametric_factories/parametric_joint.py (98%)
 rename src/adam/{ => parametric}/model/parametric_factories/parametric_link.py (100%)
 rename src/adam/{ => parametric}/model/parametric_factories/parametric_model.py (98%)
 create mode 100644 src/adam/parametric/numpy/__init__.py
 rename src/adam/{ => parametric}/numpy/computations_parametric.py (99%)
 create mode 100644 src/adam/parametric/pytorch/__init__.py
 rename src/adam/{ => parametric}/pytorch/computations_parametric.py (99%)

diff --git a/src/adam/casadi/__init__.py b/src/adam/casadi/__init__.py
index b6dc7fb6..1c3b0795 100644
--- a/src/adam/casadi/__init__.py
+++ b/src/adam/casadi/__init__.py
@@ -4,4 +4,3 @@
 
 from .casadi_like import CasadiLike
 from .computations import KinDynComputations
-from .computations_parametric import KinDynComputationsParametric
diff --git a/src/adam/model/__init__.py b/src/adam/model/__init__.py
index 2fb7d510..0dd4bf9e 100644
--- a/src/adam/model/__init__.py
+++ b/src/adam/model/__init__.py
@@ -3,7 +3,4 @@
 from .std_factories.std_joint import StdJoint
 from .std_factories.std_link import StdLink
 from .std_factories.std_model import URDFModelFactory
-from .parametric_factories.parametric_joint import ParmetricJoint
-from .parametric_factories.parametric_link import ParametricLink
-from .parametric_factories.parametric_model import URDFParametricModelFactory
 from .tree import Node, Tree
diff --git a/src/adam/model/parametric_factories/__init__.py b/src/adam/model/parametric_factories/__init__.py
deleted file mode 100644
index e69de29b..00000000
diff --git a/src/adam/parametric/__init__.py b/src/adam/parametric/__init__.py
new file mode 100644
index 00000000..6f130a0d
--- /dev/null
+++ b/src/adam/parametric/__init__.py
@@ -0,0 +1,3 @@
+# 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.
diff --git a/src/adam/parametric/casadi/__init__.py b/src/adam/parametric/casadi/__init__.py
new file mode 100644
index 00000000..46a0a0ac
--- /dev/null
+++ b/src/adam/parametric/casadi/__init__.py
@@ -0,0 +1,4 @@
+# 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.
+from .computations_parametric import KinDynComputationsParametric
diff --git a/src/adam/casadi/computations_parametric.py b/src/adam/parametric/casadi/computations_parametric.py
similarity index 99%
rename from src/adam/casadi/computations_parametric.py
rename to src/adam/parametric/casadi/computations_parametric.py
index f44e6a88..301403c1 100644
--- a/src/adam/casadi/computations_parametric.py
+++ b/src/adam/parametric/casadi/computations_parametric.py
@@ -8,7 +8,8 @@
 from adam.casadi.casadi_like import SpatialMath
 from adam.core import RBDAlgorithms
 from adam.core.constants import Representations
-from adam.model import Model, URDFModelFactory, URDFParametricModelFactory
+from adam.model import Model
+from adam.parametric.model import URDFParametricModelFactory
 
 
 class KinDynComputationsParametric:
diff --git a/src/adam/parametric/jax/__init__.py b/src/adam/parametric/jax/__init__.py
new file mode 100644
index 00000000..04b2fc09
--- /dev/null
+++ b/src/adam/parametric/jax/__init__.py
@@ -0,0 +1,5 @@
+# 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.
+
+from .computations_parametric import KinDynComputationsParametric
diff --git a/src/adam/jax/computations_parametric.py b/src/adam/parametric/jax/computations_parametric.py
similarity index 99%
rename from src/adam/jax/computations_parametric.py
rename to src/adam/parametric/jax/computations_parametric.py
index 043ad4a4..f078cd7d 100644
--- a/src/adam/jax/computations_parametric.py
+++ b/src/adam/parametric/jax/computations_parametric.py
@@ -9,7 +9,8 @@
 from adam.core.rbd_algorithms import RBDAlgorithms
 from adam.core.constants import Representations
 from adam.jax.jax_like import SpatialMath
-from adam.model import Model, URDFParametricModelFactory
+from adam.model import Model
+from adam.parametric.model import URDFParametricModelFactory
 
 
 class KinDynComputationsParametric:
diff --git a/src/adam/parametric/model/__init__.py b/src/adam/parametric/model/__init__.py
new file mode 100644
index 00000000..729aec3e
--- /dev/null
+++ b/src/adam/parametric/model/__init__.py
@@ -0,0 +1,7 @@
+# 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.
+
+from .parametric_factories.parametric_joint import ParmetricJoint
+from .parametric_factories.parametric_link import ParametricLink
+from .parametric_factories.parametric_model import URDFParametricModelFactory
diff --git a/src/adam/parametric/model/parametric_factories/__init__.py b/src/adam/parametric/model/parametric_factories/__init__.py
new file mode 100644
index 00000000..6f130a0d
--- /dev/null
+++ b/src/adam/parametric/model/parametric_factories/__init__.py
@@ -0,0 +1,3 @@
+# 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.
diff --git a/src/adam/model/parametric_factories/parametric_joint.py b/src/adam/parametric/model/parametric_factories/parametric_joint.py
similarity index 98%
rename from src/adam/model/parametric_factories/parametric_joint.py
rename to src/adam/parametric/model/parametric_factories/parametric_joint.py
index 1124740d..302b755b 100644
--- a/src/adam/model/parametric_factories/parametric_joint.py
+++ b/src/adam/parametric/model/parametric_factories/parametric_joint.py
@@ -5,7 +5,7 @@
 
 from adam.core.spatial_math import SpatialMath
 from adam.model import Joint
-from adam.model.parametric_factories.parametric_link import ParametricLink
+from adam.parametric.model.parametric_factories.parametric_link import ParametricLink
 
 
 class ParmetricJoint(Joint):
diff --git a/src/adam/model/parametric_factories/parametric_link.py b/src/adam/parametric/model/parametric_factories/parametric_link.py
similarity index 100%
rename from src/adam/model/parametric_factories/parametric_link.py
rename to src/adam/parametric/model/parametric_factories/parametric_link.py
diff --git a/src/adam/model/parametric_factories/parametric_model.py b/src/adam/parametric/model/parametric_factories/parametric_model.py
similarity index 98%
rename from src/adam/model/parametric_factories/parametric_model.py
rename to src/adam/parametric/model/parametric_factories/parametric_model.py
index bd45dd58..1a460944 100644
--- a/src/adam/model/parametric_factories/parametric_model.py
+++ b/src/adam/parametric/model/parametric_factories/parametric_model.py
@@ -5,7 +5,7 @@
 
 from adam.core.spatial_math import SpatialMath
 from adam.model import ModelFactory, StdJoint, StdLink, Link, Joint
-from adam.model import ParmetricJoint, ParametricLink
+from adam.parametric.model import ParmetricJoint, ParametricLink
 
 
 class URDFParametricModelFactory(ModelFactory):
diff --git a/src/adam/parametric/numpy/__init__.py b/src/adam/parametric/numpy/__init__.py
new file mode 100644
index 00000000..04b2fc09
--- /dev/null
+++ b/src/adam/parametric/numpy/__init__.py
@@ -0,0 +1,5 @@
+# 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.
+
+from .computations_parametric import KinDynComputationsParametric
diff --git a/src/adam/numpy/computations_parametric.py b/src/adam/parametric/numpy/computations_parametric.py
similarity index 99%
rename from src/adam/numpy/computations_parametric.py
rename to src/adam/parametric/numpy/computations_parametric.py
index 6d3ed9d5..48a5f9ea 100644
--- a/src/adam/numpy/computations_parametric.py
+++ b/src/adam/parametric/numpy/computations_parametric.py
@@ -6,7 +6,8 @@
 
 from adam.core.rbd_algorithms import RBDAlgorithms
 from adam.core.constants import Representations
-from adam.model import Model, URDFParametricModelFactory
+from adam.model import Model
+from adam.parametric.model import URDFParametricModelFactory
 from adam.numpy.numpy_like import SpatialMath
 
 
diff --git a/src/adam/parametric/pytorch/__init__.py b/src/adam/parametric/pytorch/__init__.py
new file mode 100644
index 00000000..04b2fc09
--- /dev/null
+++ b/src/adam/parametric/pytorch/__init__.py
@@ -0,0 +1,5 @@
+# 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.
+
+from .computations_parametric import KinDynComputationsParametric
diff --git a/src/adam/pytorch/computations_parametric.py b/src/adam/parametric/pytorch/computations_parametric.py
similarity index 99%
rename from src/adam/pytorch/computations_parametric.py
rename to src/adam/parametric/pytorch/computations_parametric.py
index 4584476a..8e53eea1 100644
--- a/src/adam/pytorch/computations_parametric.py
+++ b/src/adam/parametric/pytorch/computations_parametric.py
@@ -7,7 +7,8 @@
 
 from adam.core.rbd_algorithms import RBDAlgorithms
 from adam.core.constants import Representations
-from adam.model import Model, URDFModelFactory, URDFParametricModelFactory
+from adam.model import Model, URDFModelFactory
+from adam.parametric.model import URDFParametricModelFactory
 from adam.pytorch.torch_like import SpatialMath
 
 
diff --git a/tests/parametric/test_CasADi_computations_parametric.py b/tests/parametric/test_CasADi_computations_parametric.py
index 6bd05eea..7feb5eb8 100644
--- a/tests/parametric/test_CasADi_computations_parametric.py
+++ b/tests/parametric/test_CasADi_computations_parametric.py
@@ -4,12 +4,11 @@
 
 import logging
 from os import link
-import urdf_parser_py.urdf
 import casadi as cs
 import numpy as np
 import pytest
 import math
-from adam.casadi.computations_parametric import KinDynComputationsParametric
+from adam.parametric.casadi import KinDynComputationsParametric
 from adam.casadi import KinDynComputations
 
 from adam.geometry import utils
diff --git a/tests/parametric/test_Jax_computations_parametric.py b/tests/parametric/test_Jax_computations_parametric.py
index 545b4853..9c139016 100644
--- a/tests/parametric/test_Jax_computations_parametric.py
+++ b/tests/parametric/test_Jax_computations_parametric.py
@@ -10,7 +10,7 @@
 import numpy as np
 import pytest
 import math
-from adam.jax.computations_parametric import KinDynComputationsParametric
+from adam.parametric.jax import KinDynComputationsParametric
 from adam.jax import KinDynComputations
 
 from adam.geometry import utils
diff --git a/tests/parametric/test_NumPy_computations_parametric.py b/tests/parametric/test_NumPy_computations_parametric.py
index ccc3b30b..c6be049e 100644
--- a/tests/parametric/test_NumPy_computations_parametric.py
+++ b/tests/parametric/test_NumPy_computations_parametric.py
@@ -8,7 +8,7 @@
 import pytest
 import math
 import numpy as np
-from adam.numpy.computations_parametric import KinDynComputationsParametric
+from adam.parametric.numpy import KinDynComputationsParametric
 from adam.numpy import KinDynComputations
 
 from adam.geometry import utils
diff --git a/tests/parametric/test_pytorch_computations_parametric.py b/tests/parametric/test_pytorch_computations_parametric.py
index 8e48bfc4..055aff94 100644
--- a/tests/parametric/test_pytorch_computations_parametric.py
+++ b/tests/parametric/test_pytorch_computations_parametric.py
@@ -9,7 +9,7 @@
 import math
 import torch
 import numpy as np
-from adam.pytorch.computations_parametric import KinDynComputationsParametric
+from adam.parametric.pytorch import KinDynComputationsParametric
 from adam.pytorch import KinDynComputations
 
 from adam.geometry import utils

From bb5a96353cf961a3c818720f0850322e60ffd491 Mon Sep 17 00:00:00 2001
From: Carlotta Sartore <56030908+CarlottaSartore@users.noreply.github.com>
Date: Tue, 5 Dec 2023 18:02:55 +0100
Subject: [PATCH 18/19] Update
 src/adam/parametric/jax/computations_parametric.py

Co-authored-by: Giuseppe L'Erario <gl.giuseppelerario@gmail.com>
---
 src/adam/parametric/jax/computations_parametric.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/adam/parametric/jax/computations_parametric.py b/src/adam/parametric/jax/computations_parametric.py
index f078cd7d..fb0823c9 100644
--- a/src/adam/parametric/jax/computations_parametric.py
+++ b/src/adam/parametric/jax/computations_parametric.py
@@ -66,7 +66,7 @@ def mass_matrix(
             densities (jnp.array): The densities of the parametrized links
 
         Returns:
-            M (jax): Mass Matrix
+            M (jnp.array): Mass Matrix
         """
 
         factory = URDFParametricModelFactory(

From ab08cf9142f7ceffd92e8aece2de3da6f94dbff6 Mon Sep 17 00:00:00 2001
From: Carlotta <carlotta.sartore@iit.it>
Date: Tue, 5 Dec 2023 18:10:56 +0100
Subject: [PATCH 19/19] fix typos and copy paste leftovers

---
 src/adam/casadi/casadi_like.py                              | 5 +----
 src/adam/core/spatial_math.py                               | 2 +-
 src/adam/parametric/casadi/computations_parametric.py       | 6 +++---
 .../model/parametric_factories/parametric_link.py           | 6 +++---
 src/adam/parametric/numpy/computations_parametric.py        | 1 -
 tests/parametric/test_CasADi_computations_parametric.py     | 4 +++-
 tests/parametric/test_Jax_computations_parametric.py        | 4 +++-
 tests/parametric/test_NumPy_computations_parametric.py      | 4 +++-
 tests/parametric/test_pytorch_computations_parametric.py    | 4 +++-
 9 files changed, 20 insertions(+), 16 deletions(-)

diff --git a/src/adam/casadi/casadi_like.py b/src/adam/casadi/casadi_like.py
index 5bfeaa41..9ee0ce02 100644
--- a/src/adam/casadi/casadi_like.py
+++ b/src/adam/casadi/casadi_like.py
@@ -202,10 +202,7 @@ def horzcat(*x) -> "CasadiLike":
         Returns:
             CasadiLike:  horizontal concatenation of elements
         """
-        # here the logic is a bit convoluted: x is a tuple containing CasadiLike
-        # cs.vertcat accepts *args. A list of cs types is created extracting the value
-        # from the CasadiLike stored in the tuple x.
-        # Then the list is unpacked with the * operator.
+
         y = [xi.array if isinstance(xi, CasadiLike) else xi for xi in x]
         return CasadiLike(cs.horzcat(*y))
 
diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py
index 01515d74..3e1a0f8c 100644
--- a/src/adam/core/spatial_math.py
+++ b/src/adam/core/spatial_math.py
@@ -400,7 +400,7 @@ def spatial_inertia(
         IO[:3, :3] = self.factory.eye(3) * mass
         return IO
 
-    def spatial_inertial_with_parameter(self, I, mass, c, rpy):
+    def spatial_inertial_with_parameters(self, I, mass, c, rpy):
         """
         Args:
             I (npt.ArrayLike): inertia values parametric
diff --git a/src/adam/parametric/casadi/computations_parametric.py b/src/adam/parametric/casadi/computations_parametric.py
index 301403c1..7e1e8aaa 100644
--- a/src/adam/parametric/casadi/computations_parametric.py
+++ b/src/adam/parametric/casadi/computations_parametric.py
@@ -34,9 +34,9 @@ def __init__(
             root_link (str, optional): the first link. Defaults to 'root_link'.
         """
         math = SpatialMath()
-        n_param_link = len(links_name_list)
-        self.densities = cs.SX.sym("densities", n_param_link)
-        self.length_multiplier = cs.SX.sym("length_multiplier", n_param_link)
+        n_param_links = len(links_name_list)
+        self.densities = cs.SX.sym("densities", n_param_links)
+        self.length_multiplier = cs.SX.sym("length_multiplier", n_param_links)
         factory = URDFParametricModelFactory(
             path=urdfstring,
             math=math,
diff --git a/src/adam/parametric/model/parametric_factories/parametric_link.py b/src/adam/parametric/model/parametric_factories/parametric_link.py
index aa56b818..389e417b 100644
--- a/src/adam/parametric/model/parametric_factories/parametric_link.py
+++ b/src/adam/parametric/model/parametric_factories/parametric_link.py
@@ -172,7 +172,7 @@ def compute_volume(self):
         elif self.geometry_type == Geometry.SPHERE:
             visual_data_new = 0.0
             visual_data_new = self.visual_data.radius * self.length_multiplier
-            volume = 4 * math.pi * visual_data_new ** 3 / 3
+            volume = 4 * math.pi * visual_data_new**3 / 3
         return volume, visual_data_new
 
     """Function that computes the mass starting from the densities, the length multiplier and the link"""
@@ -256,7 +256,7 @@ def compute_inertia_parametric(self):
                 I.izz = I.iyy
             return I
         elif self.geometry_type == Geometry.SPHERE:
-            I.ixx = 2 * self.mass * self.visual_data_new ** 2 / 5
+            I.ixx = 2 * self.mass * self.visual_data_new**2 / 5
             I.iyy = I.ixx
             I.izz = I.ixx
         return I
@@ -276,7 +276,7 @@ def spatial_inertia(self) -> npt.ArrayLike:
         o[1] = self.origin[1]
         o[2] = self.origin[2]
         rpy = self.origin[3:]
-        return self.math.spatial_inertial_with_parameter(I, mass, o, rpy)
+        return self.math.spatial_inertial_with_parameters(I, mass, o, rpy)
 
     def homogeneous(self) -> npt.ArrayLike:
         """
diff --git a/src/adam/parametric/numpy/computations_parametric.py b/src/adam/parametric/numpy/computations_parametric.py
index 48a5f9ea..e4338251 100644
--- a/src/adam/parametric/numpy/computations_parametric.py
+++ b/src/adam/parametric/numpy/computations_parametric.py
@@ -228,7 +228,6 @@ def jacobian_dot(
         length_multiplier: np.ndarray,
         densities: np.ndarray,
     ) -> np.ndarray:
-
         """Returns the Jacobian derivative relative to the specified frame
 
         Args:
diff --git a/tests/parametric/test_CasADi_computations_parametric.py b/tests/parametric/test_CasADi_computations_parametric.py
index 7feb5eb8..6969bd44 100644
--- a/tests/parametric/test_CasADi_computations_parametric.py
+++ b/tests/parametric/test_CasADi_computations_parametric.py
@@ -70,7 +70,9 @@
     model_path, joints_name_list, link_name_list, root_link
 )
 
-original_density = [628.0724496264945]
+original_density = [
+    628.0724496264945
+]  # This is the original density value associated to the chest link, computed as mass/volume
 original_length = np.ones(len(link_name_list))
 
 
diff --git a/tests/parametric/test_Jax_computations_parametric.py b/tests/parametric/test_Jax_computations_parametric.py
index 9c139016..1de2ac69 100644
--- a/tests/parametric/test_Jax_computations_parametric.py
+++ b/tests/parametric/test_Jax_computations_parametric.py
@@ -79,7 +79,9 @@ def SX2DM(x):
     model_path, joints_name_list, link_name_list, root_link
 )
 
-original_density = [628.0724496264945]
+original_density = [
+    628.0724496264945
+]  # This is the original density value associated to the chest link, computed as mass/volume
 original_length = np.ones(len(link_name_list))
 
 n_dofs = len(joints_name_list)
diff --git a/tests/parametric/test_NumPy_computations_parametric.py b/tests/parametric/test_NumPy_computations_parametric.py
index c6be049e..db54c120 100644
--- a/tests/parametric/test_NumPy_computations_parametric.py
+++ b/tests/parametric/test_NumPy_computations_parametric.py
@@ -78,7 +78,9 @@ def SX2DM(x):
 # comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION)
 # comp_w_hardware.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION)
 
-original_density = [628.0724496264945]
+original_density = [
+    628.0724496264945
+]  # This is the original density value associated to the chest link, computed as mass/volume
 original_length = np.ones(len(link_name_list))
 
 n_dofs = len(joints_name_list)
diff --git a/tests/parametric/test_pytorch_computations_parametric.py b/tests/parametric/test_pytorch_computations_parametric.py
index 055aff94..f40a89ee 100644
--- a/tests/parametric/test_pytorch_computations_parametric.py
+++ b/tests/parametric/test_pytorch_computations_parametric.py
@@ -80,7 +80,9 @@ def SX2DM(x):
 # comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION)
 # comp_w_hardware.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION)
 
-original_density = [628.0724496264945]
+original_density = [
+    628.0724496264945
+]  # This is the original density value associated to the chest link, computed as mass/volume
 original_length = np.ones(len(link_name_list))
 
 n_dofs = len(joints_name_list)