diff --git a/python/gym_ignition/rbd/__init__.py b/python/gym_ignition/rbd/__init__.py
new file mode 100644
index 000000000..c2ec49f14
--- /dev/null
+++ b/python/gym_ignition/rbd/__init__.py
@@ -0,0 +1,6 @@
+# Copyright (C) 2020 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 . import idyntree
+from . import conversions
diff --git a/python/gym_ignition/rbd/conversions.py b/python/gym_ignition/rbd/conversions.py
new file mode 100644
index 000000000..eb4658d38
--- /dev/null
+++ b/python/gym_ignition/rbd/conversions.py
@@ -0,0 +1,87 @@
+import abc
+import numpy as np
+from typing import Tuple
+from scipy.spatial.transform import Rotation
+
+
+class Transform(abc.ABC):
+
+    @staticmethod
+    def from_position_and_quaternion(position: np.ndarray,
+                                     quaternion: np.ndarray) -> np.ndarray:
+
+        if quaternion.size != 4:
+            raise ValueError("Quaternion array must have 4 elements")
+
+        rotation = Quaternion.to_rotation(quaternion)
+        transform = Transform.from_position_and_rotation(position=position,
+                                                         rotation=rotation)
+
+        return transform
+
+    @staticmethod
+    def from_position_and_rotation(position: np.ndarray,
+                                   rotation: np.ndarray) -> np.ndarray:
+
+        if position.size != 3:
+            raise ValueError("Position array must have 3 elements")
+
+        if rotation.shape != (3, 3):
+            raise ValueError("Rotation must be a square 3x3 matrix")
+
+        transform = np.eye(4)
+        transform[0:3, 3] = position
+        transform[0:3, 0:3] = rotation
+
+        return transform
+
+    @staticmethod
+    def get_position_and_rotation(transform: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
+
+        if transform.shape != (4, 4):
+            raise ValueError("Transform must be a 4x4 matrix")
+
+        position = transform[0:3, 3]
+        rotation = transform[0:3, 0:3]
+
+        return position, rotation
+
+
+class Quaternion(abc.ABC):
+
+    @staticmethod
+    def to_wxyz(xyzw: np.ndarray) -> np.ndarray:
+
+        if xyzw.shape != (4,):
+            raise ValueError(xyzw)
+
+        return xyzw[[3, 0, 1, 2]]
+
+    @staticmethod
+    def to_xyzw(wxyz: np.ndarray) -> np.ndarray:
+
+        if wxyz.shape != (4,):
+            raise ValueError(wxyz)
+
+        return wxyz[[1, 2, 3, 0]]
+
+    @staticmethod
+    def to_rotation(quaternion: np.ndarray) -> np.ndarray:
+
+        if quaternion.shape != (4,):
+            raise ValueError(quaternion)
+
+        xyzw = Quaternion.to_xyzw(quaternion)
+
+        return Rotation.from_quat(xyzw).as_matrix()
+
+    @staticmethod
+    def from_matrix(matrix: np.ndarray) -> np.ndarray:
+
+        if matrix.shape != (3, 3):
+            raise ValueError(matrix)
+
+        quaternion_xyzw = Rotation.from_matrix(matrix).as_quat()
+        quaternion_wxyz = Quaternion.to_wxyz(quaternion_xyzw)
+
+        return quaternion_wxyz
diff --git a/python/gym_ignition/rbd/idyntree/__init__.py b/python/gym_ignition/rbd/idyntree/__init__.py
new file mode 100644
index 000000000..97c8e6094
--- /dev/null
+++ b/python/gym_ignition/rbd/idyntree/__init__.py
@@ -0,0 +1,7 @@
+# Copyright (C) 2020 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 . import numpy
+from . import helpers
+from . import kindyncomputations
diff --git a/python/gym_ignition/rbd/idyntree/helpers.py b/python/gym_ignition/rbd/idyntree/helpers.py
new file mode 100644
index 000000000..625807560
--- /dev/null
+++ b/python/gym_ignition/rbd/idyntree/helpers.py
@@ -0,0 +1,86 @@
+# Copyright (C) 2020 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 os
+import abc
+from typing import List
+from enum import Enum, auto
+import idyntree.bindings as idt
+from gym_ignition.utils import resource_finder
+
+
+class FrameVelocityRepresentation(Enum):
+
+    MIXED_REPRESENTATION = auto()
+    BODY_FIXED_REPRESENTATION = auto()
+    INERTIAL_FIXED_REPRESENTATION = auto()
+
+    def to_idyntree(self):
+
+        if self.value == FrameVelocityRepresentation.MIXED_REPRESENTATION.value:
+            return idt.MIXED_REPRESENTATION
+        elif self.value == FrameVelocityRepresentation.BODY_FIXED_REPRESENTATION.value:
+            return idt.BODY_FIXED_REPRESENTATION
+        elif self.value == FrameVelocityRepresentation.INERTIAL_FIXED_REPRESENTATION.value:
+            return idt.INERTIAL_FIXED_REPRESENTATION
+        else:
+            raise ValueError(self.value)
+
+
+class iDynTreeHelpers(abc.ABC):
+
+    @staticmethod
+    def get_model_loader(model_file: str, considered_joints: List[str] = None):
+
+        # Find the urdf file
+        urdf_file = resource_finder.find_resource(file_name=model_file)
+
+        # Get the file extension
+        folder, model_file = os.path.split(urdf_file)
+        model_name, extension = os.path.splitext(model_file)
+
+        if extension == ".sdf":
+            raise RuntimeError("SDF models are not currently supported by iDynTree")
+
+        # Create the model loader
+        mdl_loader = idt.ModelLoader()
+
+        # Load the urdf model
+        if considered_joints is None:
+            ok_load = mdl_loader.loadModelFromFile(urdf_file)
+        else:
+            ok_load = mdl_loader.loadReducedModelFromFile(urdf_file, considered_joints)
+
+        if not ok_load:
+            raise RuntimeError(f"Failed to load model from file '{urdf_file}'")
+
+        return mdl_loader
+
+    @staticmethod
+    def get_kindyncomputations(
+            model_file: str,
+            considered_joints: List[str] = None,
+            velocity_representation: FrameVelocityRepresentation = None):
+
+        # Get the model loader
+        model_loader = iDynTreeHelpers.get_model_loader(model_file, considered_joints)
+
+        # Create KinDynComputations and insert the model
+        kindyn = idt.KinDynComputations()
+        ok_load = kindyn.loadRobotModel(model_loader.model())
+
+        if not ok_load:
+            raise RuntimeError("Failed to load model")
+
+        if velocity_representation is None:
+            velocity_representation = FrameVelocityRepresentation.MIXED_REPRESENTATION
+
+        # Configure the velocity representation
+        velocity_representation_idyntree = velocity_representation.to_idyntree()
+        ok_repr = kindyn.setFrameVelocityRepresentation(velocity_representation_idyntree)
+
+        if not ok_repr:
+            raise RuntimeError("Failed to set the velocity representation")
+
+        return kindyn
diff --git a/python/gym_ignition/rbd/idyntree/kindyncomputations.py b/python/gym_ignition/rbd/idyntree/kindyncomputations.py
new file mode 100644
index 000000000..018e6a92a
--- /dev/null
+++ b/python/gym_ignition/rbd/idyntree/kindyncomputations.py
@@ -0,0 +1,383 @@
+# Copyright (C) 2020 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 typing import List, Tuple
+import idyntree.bindings as idt
+from scenario import core as scenario_core
+from gym_ignition.rbd import conversions
+from gym_ignition.rbd.idyntree import numpy
+from .helpers import FrameVelocityRepresentation, iDynTreeHelpers
+
+
+class KinDynComputations:
+
+    def __init__(self,
+                 model_file: str,
+                 considered_joints: List[str] = None,
+                 velocity_representation: FrameVelocityRepresentation = None) -> None:
+
+        self.kindyn = iDynTreeHelpers.get_kindyncomputations(model_file,
+                                                             considered_joints,
+                                                             velocity_representation)
+        self.dofs = self.kindyn.getNrOfDegreesOfFreedom()
+
+        if considered_joints is None:
+
+            model: idt.Model = self.kindyn.model()
+
+            all_joints = [model.getJointName(i) for i in range(model.getNrOfJoints())]
+            self._considered_joints = all_joints
+        else:
+            self._considered_joints = considered_joints
+
+    def joint_serialization(self) -> List[str]:
+        return self._considered_joints
+
+    def set_robot_state(self,
+                        s: np.ndarray,
+                        ds: np.ndarray,
+                        world_H_base: np.ndarray = np.eye(4),
+                        base_velocity: np.ndarray = np.zeros(6),
+                        world_gravity: np.ndarray = np.array([0, 0, -9.806])) -> None:
+
+        if s.size != self.dofs:
+            raise ValueError(s)
+
+        if ds.size != self.dofs:
+            raise ValueError(ds)
+
+        if world_gravity.size != 3:
+            raise ValueError(world_gravity)
+
+        if world_H_base.shape != (4, 4):
+            raise ValueError(world_H_base)
+
+        if base_velocity.size != 6:
+            raise ValueError(base_velocity)
+
+        s_idyntree = numpy.FromNumPy.to_idyntree_dyn_vector(array=s)
+        ds_idyntree = numpy.FromNumPy.to_idyntree_dyn_vector(array=ds)
+
+        world_gravity_idyntree = \
+            numpy.FromNumPy.to_idyntree_fixed_vector(array=world_gravity)
+
+        world_H_base_idyntree = numpy.FromNumPy.to_idyntree_transform(
+            position=world_H_base[0:3, 3], rotation=world_H_base[0:3, 0:3])
+
+        base_velocity_idyntree = numpy.FromNumPy.to_idyntree_twist(
+            linear_velocity=base_velocity[0:3], angular_velocity=base_velocity[3:6])
+
+        ok_state = self.kindyn.setRobotState(world_H_base_idyntree,
+                                             s_idyntree,
+                                             base_velocity_idyntree,
+                                             ds_idyntree,
+                                             world_gravity_idyntree)
+
+        if not ok_state:
+            raise RuntimeError("Failed to set the robot state")
+
+    def set_robot_state_from_model(self,
+                                   model: scenario_core.Model,
+                                   world_gravity: np.ndarray = np.array([0, 0, -9.806])) \
+            -> None:
+
+        s = np.array(model.joint_positions(self.joint_serialization()))
+        ds = np.array(model.joint_velocities(self.joint_serialization()))
+
+        world_o_base = np.array(model.base_position())
+        world_quat_base = np.array(model.base_orientation())
+
+        # Velocity representations
+        body = FrameVelocityRepresentation.BODY_FIXED_REPRESENTATION.to_idyntree()
+        mixed = FrameVelocityRepresentation.MIXED_REPRESENTATION.to_idyntree()
+
+        if self.kindyn.getFrameVelocityRepresentation() is mixed:
+
+            base_linear_velocity = np.array(model.base_world_linear_velocity())
+            base_angular_velocity = np.array(model.base_world_angular_velocity())
+
+        elif self.kindyn.getFrameVelocityRepresentation() is body:
+
+            base_linear_velocity = np.array(model.base_body_linear_velocity())
+            base_angular_velocity = np.array(model.base_body_angular_velocity())
+
+        else:
+            raise RuntimeError("INERTIAL_FIXED_REPRESENTATION not yet supported")
+
+        # Pack the data structures
+        world_H_base = conversions.Transform.from_position_and_quaternion(
+            position=world_o_base, quaternion=world_quat_base)
+        base_velocity_6d = np.concatenate((base_linear_velocity, base_angular_velocity))
+
+        self.set_robot_state(s=s,
+                             ds=ds,
+                             world_H_base=world_H_base,
+                             base_velocity=base_velocity_6d,
+                             world_gravity=world_gravity)
+
+    def get_floating_base(self) -> str:
+
+        return self.kindyn.getFloatingBase()
+
+    def get_joint_positions(self) -> np.ndarray:
+
+        vector = idt.VectorDynSize()
+
+        ok = self.kindyn.getJointPos(vector)
+
+        if not ok:
+            raise RuntimeError("Failed to extract joint positions")
+
+        return vector.toNumPy()
+
+    def get_joint_velocities(self) -> np.ndarray:
+
+        vector = idt.VectorDynSize()
+
+        ok = self.kindyn.getJointVel(vector)
+
+        if not ok:
+            raise RuntimeError("Failed to extract joint velocities")
+
+        return vector.toNumPy()
+
+    def get_model_velocity(self) -> np.ndarray:
+
+        nu = idt.VectorDynSize()
+
+        ok_nu = self.kindyn.getModelVel(nu)
+
+        if not ok_nu:
+            raise RuntimeError("Failed to get the model velocity")
+
+        return nu.toNumPy()
+
+    def get_world_transform(self, frame_name: str) -> np.ndarray:
+
+        if self.kindyn.getFrameIndex(frame_name) < 0:
+            raise ValueError(f"Frame '{frame_name}' does not exist")
+
+        H = self.kindyn.getWorldTransform(frame_name)
+
+        return numpy.ToNumPy.from_idyntree_transform(transform=H)
+
+    def get_relative_transform(self,
+                               ref_frame_name: str,
+                               frame_name: str) -> np.ndarray:
+
+        if self.kindyn.getFrameIndex(ref_frame_name) < 0:
+            raise ValueError(f"Frame '{ref_frame_name}' does not exist")
+
+        if self.kindyn.getFrameIndex(frame_name) < 0:
+            raise ValueError(f"Frame '{frame_name}' does not exist")
+
+        ref_H_other: idt.Transform = self.kindyn.getRelativeTransform(ref_frame_name,
+                                                                      frame_name)
+
+        return ref_H_other.asHomogeneousTransform().toNumPy()
+
+    def get_relative_transform_explicit(self,
+                                        ref_frame_origin: str,
+                                        ref_frame_orientation: str,
+                                        frame_origin: str,
+                                        frame_orientation: str):
+
+        for frame in {ref_frame_origin,
+                      ref_frame_orientation,
+                      frame_origin,
+                      frame_orientation}:
+
+            if frame != "world" and self.kindyn.getFrameIndex(frame) < 0:
+                raise ValueError(f"Frame '{frame}' does not exist")
+
+        # Compute the transform AB_H_CD
+        frameA = ref_frame_origin
+        frameB = ref_frame_orientation
+        frameC = frame_origin
+        frameD = frame_orientation
+
+        if frame_orientation == "world":
+            frameD = frameC
+
+        if ref_frame_orientation == "world":
+            frameB = frameA
+
+        frameA_index = self.kindyn.getFrameIndex(frameName=frameA)
+        frameB_index = self.kindyn.getFrameIndex(frameName=frameB)
+        frameC_index = self.kindyn.getFrameIndex(frameName=frameC)
+        frameD_index = self.kindyn.getFrameIndex(frameName=frameD)
+
+        ref_H_other: idt.Transform = self.kindyn.getRelativeTransformExplicit(
+            frameA_index,
+            frameB_index,
+            frameC_index,
+            frameD_index)
+
+        AB_H_CD = ref_H_other
+        # print("->", AB_H_CD.asHomogeneousTransform().toNumPy())
+
+        if frame_orientation == "world":
+            AB_H_C = AB_H_CD
+            C_H_CW: idt.Transform = self.kindyn.getWorldTransform(frameC).inverse()
+            C_H_CW.setPosition(idt.Position(0, 0, 0))
+            AB_H_CW = AB_H_C * C_H_CW
+            AB_H_CD = AB_H_CW
+            # print(C_H_CW.asHomogeneousTransform().toNumPy())
+            # print("->", AB_H_CW.asHomogeneousTransform().toNumPy())
+            # print("->", AB_H_CD.asHomogeneousTransform().toNumPy())
+
+        if ref_frame_orientation == "world":
+            A_H_CD = AB_H_CD
+            AW_H_A: idt.Transform = self.kindyn.getWorldTransform(frameA)
+            AW_H_A.setPosition(idt.Position(0, 0, 0))
+            AW_H_CD = AW_H_A * A_H_CD
+            AB_H_CD = AW_H_CD
+            # print("->", AW_H_A.asHomogeneousTransform().toNumPy())
+            # print("->", AB_H_CD.asHomogeneousTransform().toNumPy())
+
+        # return ref_H_other.asHomogeneousTransform().toNumPy()
+        # return ref_H_other.asAdjointTransformWrench().toNumPy()
+        # print("->", AB_H_CD.asHomogeneousTransform().toNumPy().tolist())
+        # print("~>\n", AB_H_CD.asHomogeneousTransform().toNumPy())
+        # return AB_H_CD.asAdjointTransformWrench().toNumPy()
+        return AB_H_CD.asHomogeneousTransform().toNumPy()
+
+    def get_mass_matrix(self) -> np.ndarray:
+
+        M = idt.MatrixDynSize()
+
+        ok_M = self.kindyn.getFreeFloatingMassMatrix(M)
+
+        if not ok_M:
+            raise RuntimeError("Failed to get the free floating mass matrix")
+
+        return M.toNumPy()
+
+    def get_bias_forces(self) -> np.ndarray:
+
+        h = idt.FreeFloatingGeneralizedTorques(self.kindyn.model())
+        ok_h = self.kindyn.generalizedBiasForces(h)
+
+        if not ok_h:
+            raise RuntimeError("Failed to get the generalized bias forces")
+
+        base_wrench: idt.Wrench = h.baseWrench()
+        joint_torques: idt.JointDOFsDoubleArray = h.jointTorques()
+
+        return np.concatenate([base_wrench.toNumPy().flatten(),
+                               joint_torques.toNumPy().flatten()])
+
+    def get_momentum(self) -> Tuple[np.ndarray, np.ndarray]:
+
+        spatial_momentum = self.kindyn.getLinearAngularMomentum()
+        momentum_6d = spatial_momentum.asVector().toNumPy()
+
+        linear, angular = np.split(momentum_6d, 2)
+        return linear, angular
+
+    def get_centroidal_momentum(self) -> Tuple[np.ndarray, np.ndarray]:
+
+        spatial_momentum = self.kindyn.getCentroidalTotalMomentum()
+        momentum_6d = spatial_momentum.asVector().toNumPy()
+
+        linear, angular = np.split(momentum_6d, 2)
+        return linear, angular
+
+    def get_com_position(self) -> np.ndarray:
+
+        W_p_com = self.kindyn.getCenterOfMassPosition()
+        return W_p_com.toNumPy()
+
+    def get_com_velocity(self) -> np.ndarray:
+
+        # Velocity representations
+        body = FrameVelocityRepresentation.BODY_FIXED_REPRESENTATION.to_idyntree()
+        mixed = FrameVelocityRepresentation.MIXED_REPRESENTATION.to_idyntree()
+
+        if self.kindyn.getFrameVelocityRepresentation() is mixed:
+
+            # The method always returns the MIXED velocity of the CoM, regardless of
+            # how KinDynComputations was configured.
+            v_com = self.kindyn.getCenterOfMassVelocity()
+            return v_com.toNumPy()
+
+        elif self.kindyn.getFrameVelocityRepresentation() is body:
+
+            # Get the transform of the base frame
+            W_H_B = self.kindyn.getWorldBaseTransform()
+            _, W_R_B = numpy.ToNumPy.from_idyntree_transform(transform=W_H_B, split=True)
+
+            # Get the rotation between world and base frame
+            B_R_W = np.linalg.inv(W_R_B)
+
+            # Convert linear velocity from MIXED to BODY representation
+            BW_v_com = self.kindyn.getCenterOfMassVelocity().toNumPy()
+            B_v_com = B_R_W @ BW_v_com
+
+            return B_v_com
+
+        else:
+            raise RuntimeError("INERTIAL_FIXED_REPRESENTATION not yet supported")
+
+    def get_frame_jacobian(self, frame_name: str) -> np.ndarray:
+
+        # frame_index = self.kindyn.getFrameIndex(frame_name)
+
+        if self.kindyn.getFrameIndex(frame_name) < 0:
+        # if frame_index < 0:
+            raise ValueError(f"Frame '{frame_name}' does not exist")
+
+        J = idt.MatrixDynSize(6, self.dofs + 6)  # TODO: open PR?
+
+        # Error:
+        # python /home/dferigo/.config/JetBrains/PyCharmCE2020.1/scratches/scratch_8.py: /usr/include/eigen3/Eigen/src/Core/Block.h:146: Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel>::Block(XprType&, Eigen::Index, Eigen::Index, Eigen::Index, Eigen::Index) [with XprType = Eigen::Map<Eigen::Matrix<double, -1, -1, 1> >; int BlockRows = -1; int BlockCols = -1; bool InnerPanel = false; Eigen::Index = long int]: Assertion `startRow >= 0 && blockRows >= 0 && startRow <= xpr.rows() - blockRows && startCol >= 0 && blockCols >= 0 && startCol <= xpr.cols() - blockCols' failed.
+
+        ok_J = self.kindyn.getFrameFreeFloatingJacobian(frame_name, J)
+        # ok_J = self.kindyn.getFrameFreeFloatingJacobian(1, J)
+        # print(ok_J)
+        # raise
+
+        if not ok_J:
+            raise RuntimeError("Failed to get the frame free floating jacobian")
+
+        # print(J.toNumPy().shape)
+        # raise
+        return J.toNumPy()
+
+    def get_linear_angular_momentum_jacobian(self) -> np.ndarray:
+
+        J_mom = idt.MatrixDynSize()
+
+        ok_J_mom = self.kindyn.getLinearAngularMomentumJacobian(J_mom)
+
+        if not ok_J_mom:
+            raise RuntimeError("Failed to get the momentum jacobian")
+
+        return J_mom.toNumPy()
+
+    def get_centroidal_total_momentum_jacobian(self) -> np.ndarray:
+
+        J_cmm = idt.MatrixDynSize()
+
+        ok_J_cmm = self.kindyn.getCentroidalTotalMomentumJacobian(J_cmm)
+
+        if not ok_J_cmm:
+            raise RuntimeError("Failed to get the centroidal total momentum jacobian")
+
+        return J_cmm.toNumPy()
+
+    def get_frame_bias_acc(self, frame_name: str) -> np.ndarray:
+
+        if self.kindyn.getFrameIndex(frame_name) < 0:
+            raise ValueError(f"Frame '{frame_name}' does not exist")
+
+        dJ_nu = self.kindyn.getFrameBiasAcc(frame_name)
+
+        return dJ_nu.toNumPy()
+
+    def get_com_bias_acc(self) -> np.ndarray:
+
+        dJ_nu = self.kindyn.getCenterOfMassBiasAcc()
+        return dJ_nu.toNumPy()
diff --git a/python/gym_ignition/rbd/idyntree/numpy.py b/python/gym_ignition/rbd/idyntree/numpy.py
new file mode 100644
index 000000000..5b80d93e8
--- /dev/null
+++ b/python/gym_ignition/rbd/idyntree/numpy.py
@@ -0,0 +1,135 @@
+import abc
+import numpy as np
+from gym_ignition import rbd
+from typing import Tuple, Union
+import idyntree.bindings as idt
+
+
+class FromNumPy(abc.ABC):
+
+    @staticmethod
+    def to_idyntree_dyn_vector(array: np.ndarray) -> idt.VectorDynSize:
+
+        return idt.VectorDynSize_FromPython(array)
+
+    @staticmethod
+    def to_idyntree_fixed_vector(array: np.ndarray):
+
+        size = array.size
+        supported_sizes = [3, 4, 6]
+
+        if size not in supported_sizes:
+            raise ValueError(array)
+
+        if size == 3:
+            idyntree_vector = idt.Vector3()
+        elif size == 4:
+            idyntree_vector = idt.Vector4()
+        elif size == 6:
+            idyntree_vector = idt.Vector6()
+        else:
+            raise RuntimeError
+
+        return idyntree_vector.FromPython(array)
+
+    @staticmethod
+    def to_idyntree_position(position: np.ndarray) -> idt.Position:
+
+        if position.size != 3:
+            raise ValueError("The position array must have 3 elements")
+
+        return idt.Position(position[0], position[1], position[2])
+
+    @staticmethod
+    def to_idyntree_rotation(quaternion: np.ndarray) -> idt.Rotation:
+
+        if quaternion.size != 4:
+            raise ValueError("The quaternion array must have 4 elements")
+
+        quat = idt.Vector4_FromPython(quaternion)
+
+        R = idt.Rotation()
+        R.fromQuaternion(quat)
+
+        return R
+
+    @staticmethod
+    def to_idyntree_transform(position: np.ndarray,
+                              quaternion: np.ndarray = None,
+                              rotation: np.ndarray = None) -> idt.Transform:
+
+        if quaternion is None and rotation is None:
+            raise ValueError("You must pass either a quaternion or a rotation")
+
+        if quaternion is not None and rotation is not None:
+            raise ValueError("You must pass either a quaternion or a rotation")
+
+        if rotation is not None:
+            quaternion = rbd.conversions.Quaternion.from_matrix(matrix=rotation)
+
+        p = FromNumPy.to_idyntree_position(position=position)
+        R = FromNumPy.to_idyntree_rotation(quaternion=quaternion)
+
+        H = idt.Transform()
+        H.setPosition(p)
+        H.setRotation(R)
+
+        return H
+
+    @staticmethod
+    def to_idyntree_twist(linear_velocity: np.ndarray,
+                          angular_velocity: np.ndarray) -> idt.Twist:
+
+        if linear_velocity.size != 3:
+            raise ValueError("The linear velocity must have 3 elements")
+
+        if angular_velocity.size != 3:
+            raise ValueError("The angular velocity must have 3 elements")
+
+        twist_numpy = np.concatenate((linear_velocity, angular_velocity))
+
+        twist = idt.Twist_FromPython(twist_numpy)
+
+        return twist
+
+
+class ToNumPy(abc.ABC):
+
+    @staticmethod
+    def from_idyntree_vector(vector) -> np.ndarray:
+
+        input_types = (
+            idt.Vector3,
+            idt.Vector4,
+            idt.Vector6,
+            idt.VectorDynSize,
+            idt.Matrix3x3,
+            idt.Matrix4x4,
+            idt.Matrix6x6,
+            idt.MatrixDynSize,
+        )
+
+        if not isinstance(vector, input_types):
+            raise ValueError(vector)
+
+        return np.array(vector.toNumPy())
+
+    @staticmethod
+    def from_idyntree_transform(transform: idt.Transform, split: bool = False) \
+            -> Union[Tuple[np.ndarray, np.ndarray], np.ndarray]:
+
+        if not isinstance(transform, idt.Transform):
+            raise ValueError(transform)
+
+        rotation = transform.getRotation()
+        position = transform.getPosition()
+
+        if split:
+            return position.toNumPy(), rotation.toNumPy()
+
+        else:
+            H = np.zeros(shape=[4, 4])
+            H[0:3, 3] = position.toNumPy()
+            H[0:3, 0:3] = rotation.toNumPy()
+
+            return H