diff --git a/agimus_controller/agimus_controller/factory/robot_model.py b/agimus_controller/agimus_controller/factory/robot_model.py
index c8be7d6e..ec133a51 100644
--- a/agimus_controller/agimus_controller/factory/robot_model.py
+++ b/agimus_controller/agimus_controller/factory/robot_model.py
@@ -1,96 +1,207 @@
 from copy import deepcopy
-from dataclasses import dataclass
-import numpy as np
+from dataclasses import dataclass, field
 from pathlib import Path
+
+import coal
+import numpy as np
+import numpy.typing as npt
 import pinocchio as pin
-from typing import Union
 
 
 @dataclass
 class RobotModelParameters:
-    q0_name = str()
-    free_flyer = False
-    locked_joint_names = []
-    urdf = Path() | str
-    srdf = Path() | str
-    collision_as_capsule = False
-    self_collision = False
-
-
-class RobotModelFactory:
+    q0: npt.NDArray[np.float64] = np.array(
+        [], dtype=np.float64
+    )  # Initial configuration of the robot
+    free_flyer: bool = False  # True if the robot has a free flyer
+    locked_joint_names: list[str] = field(default_factory=list)
+    urdf_path: Path = Path()  # Path to the URDF file
+    srdf_path: Path | None = None  # Path to the SRDF file
+    urdf_meshes_dir: Path | None = (
+        Path()  # Path to the directory containing the meshes and the URDF file.
+    )
+    collision_as_capsule: bool = (
+        False  # True if the collision model should be reduced to capsules.
+    )
+    # By default, the collision model when convexified is a sum of spheres and cylinders, often representing capsules. Here, all the couples sphere cylinder sphere are replaced by coal capsules.
+    self_collision: bool = False  # If True, the collision model takes into account collisions pairs written in the srdf file.
+    armature: npt.NDArray[np.float64] = field(
+        default_factory=lambda: np.array([], dtype=np.float64)
+    )  # Default empty NumPy array
+    collision_color: npt.NDArray[np.float64] = (
+        np.array([249.0, 136.0, 126.0, 125.0]) / 255.0
+    )  # Red color for the collision model
+
+    def __post_init__(self):
+        # Check q0 is not empty
+        if len(self.q0) == 0:
+            raise ValueError("q0 cannot be empty.")
+
+        # Handle armature:
+        if self.armature.size == 0:
+            # Use a default armature filled with 0s, based on the size of q0
+            self.armature = np.zeros(len(self.q0), dtype=np.float64)
+
+        # Ensure armature has the same shape as q0
+        if (
+            self.armature.shape != self.q0.shape and not self.free_flyer
+        ):  #! TODO: Do the same for free flyer
+            raise ValueError(
+                f"Armature must have the same shape as q0. Got {self.armature.shape} and {self.q0.shape}."
+            )
+
+        # Ensure paths are valid strings
+        if not self.urdf_path.is_file():
+            raise ValueError("urdf_path must be a valid file path.")
+
+        if self.srdf_path is not None and not self.srdf_path.is_file():
+            raise ValueError("srdf_path must be a valid file path.")
+
+
+class RobotModels:
     """Parse the robot model, reduce it and filter the collision model."""
 
-    """Complete model of the robot."""
-    _complete_model = pin.Model()
-    """ Complete model of the robot with collision meshes. """
-    _complete_collision_model = pin.GeometryModel()
-    """ Complete model of the robot with visualization meshes. """
-    _complete_visual_model = pin.GeometryModel()
-    """ Reduced model of the robot. """
-    _rmodel = pin.Model()
-    """ Reduced model of the robot with visualization meshes. """
-    _rcmodel = pin.GeometryModel()
-    """ Reduced model of the robot with collision meshes. """
-    _rvmodel = pin.GeometryModel()
-    """ Default configuration q0. """
-    _q0 = np.array([])
-    """ Parameters of the model. """
-    _params = RobotModelParameters()
-    """ Path to the collisions environment. """
-    _env = Path()
-
-    def load_model(self, param: RobotModelParameters, env: Union[Path, None]) -> None:
+    def __init__(self, param: RobotModelParameters):
+        """Parse the robot model, reduce it and filter the collision model.
+
+        Args:
+            param (RobotModelParameters): Parameters to load the robot models.
+        """
         self._params = param
-        self._env = env
-        self._load_pinocchio_models(param.urdf, param.free_flyer)
-        self._load_default_configuration(param.srdf, param.q0_name)
-        self._load_reduced_model(param.locked_joint_names, param.q0_name)
-        self._update_collision_model(
-            env, param.collision_as_capsule, param.self_collision, param.srdf
+        self._full_robot_model = None
+        self._robot_model = None
+        self._collision_model = None
+        self._visual_model = None
+        self._q0 = deepcopy(self._params.q0)
+        self.load_models()  # Populate models
+
+    @property
+    def full_robot_model(self) -> pin.Model:
+        """Full robot model."""
+        if self._full_robot_model is None:
+            raise AttributeError("Full robot model has not been initialized yet.")
+        return self._full_robot_model
+
+    @property
+    def robot_model(self) -> pin.Model:
+        """Robot model, reduced if specified in the parameters."""
+        if self._robot_model is None:
+            raise AttributeError("Robot model has not been computed yet.")
+        return self._robot_model
+
+    @property
+    def visual_model(self) -> pin.GeometryModel:
+        """Visual model of the robot."""
+        if self._visual_model is None:
+            raise AttributeError("Visual model has not been computed yet.")
+        return self._visual_model
+
+    @property
+    def collision_model(self) -> pin.GeometryModel:
+        """Collision model of the robot."""
+        if self._collision_model is None:
+            raise AttributeError("Visual model has not been computed yet.")
+        return self._collision_model
+
+    @property
+    def q0(self) -> np.array:
+        """Initial configuration of the robot."""
+        return self._q0
+
+    def load_models(self) -> None:
+        """Load and prepare robot models based on parameters."""
+        self._load_full_pinocchio_models()
+        if self._params.locked_joint_names:
+            self._apply_locked_joints()
+        if self._params.collision_as_capsule:
+            self._update_collision_model_to_capsules()
+        if self._params.self_collision:
+            self._update_collision_model_to_self_collision()
+
+    def _load_full_pinocchio_models(self) -> None:
+        """Load the full robot model, the visual model and the collision model."""
+        try:
+            (
+                self._full_robot_model,
+                self._collision_model,
+                self._visual_model,
+            ) = pin.buildModelsFromUrdf(
+                str(self._params.urdf_path),
+                str(self._params.urdf_meshes_dir),
+                pin.JointModelFreeFlyer() if self._params.free_flyer else None,
+            )
+        except Exception as e:
+            raise ValueError(
+                f"Failed to load URDF models from {self._params.urdf_path}: {e}"
+            )
+
+    def _apply_locked_joints(self) -> None:
+        """Apply locked joints."""
+        joints_to_lock = []
+        for jn in self._params.locked_joint_names:
+            if self._full_robot_model.existJointName(jn):
+                joints_to_lock.append(self._full_robot_model.getJointId(jn))
+            else:
+                raise ValueError(f"Joint {jn} not found in the robot model.")
+
+        self._robot_model = pin.buildReducedModel(
+            self._full_robot_model, joints_to_lock, self._q0
         )
 
-    def _load_pinocchio_models(self, urdf: Path, free_flyer: bool) -> None:
-        pass
-
-    def _load_default_configuration(self, srdf_path: Path, q0_name: str) -> None:
-        pass
-
-    def _load_reduced_model(self, locked_joint_names, q0_name) -> None:
-        pass
-
-    def _update_collision_model(
-        self,
-        env: Union[Path, None],
-        collision_as_capsule: bool,
-        self_collision: bool,
-        srdf: Path,
-    ) -> None:
-        pass
-
-    def create_complete_robot_model(self) -> pin.Model:
-        return self._complete_model.copy()
-
-    def create_complete_collision_model(self) -> pin.GeometryModel:
-        return self._complete_collision_model.copy()
-
-    def create_complete_visual_model(self) -> pin.GeometryModel:
-        return self._complete_visual_model.copy()
-
-    def create_reduced_robot_model(self) -> pin.Model:
-        return self._rmodel.copy()
-
-    def create_reduced_collision_model(self) -> pin.GeometryModel:
-        return self._rcmodel.copy()
-
-    def create_reduced_visual_model(self) -> pin.GeometryModel:
-        return self._rvmodel.copy()
-
-    def create_default_configuration(self) -> np.array:
-        return self._q0.copy()
-
-    def create_model_parameters(self) -> RobotModelParameters:
-        return deepcopy(self._params)
+    def _update_collision_model_to_capsules(self) -> None:
+        """Update the collision model to capsules."""
+        cmodel = self._collision_model.copy()
+        list_names_capsules = []
+        # Iterate through geometry objects in the collision model
+        for geom_object in cmodel.geometryObjects:
+            geometry = geom_object.geometry
+            # Remove superfluous suffix from the name
+            base_name = "_".join(geom_object.name.split("_")[:-1])
+            # Convert cylinders to capsules
+            if isinstance(geometry, coal.Cylinder):
+                name = self._generate_capsule_name(base_name, list_names_capsules)
+                list_names_capsules.append(name)
+                capsule = pin.GeometryObject(
+                    name=name,
+                    parent_frame=geom_object.parentFrame,
+                    parent_joint=geom_object.parentJoint,
+                    collision_geometry=coal.Capsule(
+                        geometry.radius, geometry.halfLength
+                    ),
+                    placement=geom_object.placement,
+                )
+                capsule.meshColor = self._params.collision_color
+                self._collision_model.addGeometryObject(capsule)
+                self._collision_model.removeGeometryObject(geom_object.name)
+
+            # Remove spheres associated with links
+            elif isinstance(geometry, coal.Sphere) and "link" in geom_object.name:
+                self._collision_model.removeGeometryObject(geom_object.name)
+
+    def _update_collision_model_to_self_collision(self) -> None:
+        """Update the collision model to self collision."""
+        self._collision_model.addAllCollisionPairs()
+        pin.removeCollisionPairs(
+            self._robot_model, self._collision_model, str(self._params.srdf_path)
+        )
 
-    def print_model(self):
-        print("full model =\n", self._complete_model)
-        print("reduced model =\n", self._rmodel)
+    def _generate_capsule_name(self, base_name: str, existing_names: list[str]) -> str:
+        """Generates a unique capsule name for a geometry object.
+        Args:
+            base_name (str): The base name of the geometry object.
+            existing_names (list): List of names already assigned to capsules.
+        Returns:
+            str: Unique capsule name.
+        """
+        i = 0
+        while f"{base_name}_capsule_{i}" in existing_names:
+            i += 1
+        return f"{base_name}_capsule_{i}"
+
+    @property
+    def armature(self) -> npt.NDArray[np.float64]:
+        """Armature of the robot.
+        Returns:
+            npt.NDArray[np.float64]: Armature of the robot.
+        """
+        return self._params.armature
diff --git a/agimus_controller/tests/test_robot_models.py b/agimus_controller/tests/test_robot_models.py
new file mode 100644
index 00000000..c8ab1813
--- /dev/null
+++ b/agimus_controller/tests/test_robot_models.py
@@ -0,0 +1,170 @@
+from copy import deepcopy
+from os.path import dirname
+import unittest
+from pathlib import Path
+import example_robot_data as robex
+import numpy as np
+import pinocchio as pin
+
+from agimus_controller.factory.robot_model import RobotModelParameters, RobotModels
+
+
+class TestRobotModelParameters(unittest.TestCase):
+    def test_valid_initialization(self):
+        """Test that the dataclass initializes correctly with valid input."""
+
+        robot = robex.load("panda")
+        urdf_path = Path(robot.urdf)
+        srdf_path = Path(robot.urdf.replace("urdf", "srdf"))
+        urdf_meshes_dir = urdf_path.parent.parent.parent.parent.parent
+        free_flyer = False
+        q0 = np.zeros(robot.model.nq)
+        locked_joint_names = ["panda_joint1", "panda_joint2"]
+        params = RobotModelParameters(
+            q0=q0,
+            free_flyer=free_flyer,
+            locked_joint_names=locked_joint_names,
+            urdf_path=urdf_path,
+            srdf_path=srdf_path,
+            urdf_meshes_dir=urdf_meshes_dir,
+            collision_as_capsule=True,
+            self_collision=True,
+            armature=np.linspace(0.1, 0.9, 9),
+        )
+
+        self.assertTrue(np.array_equal(params.q0, q0))
+        self.assertEqual(params.free_flyer, free_flyer)
+        self.assertEqual(params.locked_joint_names, ["panda_joint1", "panda_joint2"])
+        self.assertEqual(params.urdf_path, urdf_path)
+        self.assertEqual(params.srdf_path, srdf_path)
+        self.assertEqual(params.urdf_meshes_dir, urdf_meshes_dir)
+        self.assertTrue(params.collision_as_capsule)
+        self.assertTrue(params.self_collision)
+        self.assertTrue(np.array_equal(params.armature, np.linspace(0.1, 0.9, 9)))
+
+    def test_empty_q0_raises_error(self):
+        """Test that an empty q0 raises a ValueError."""
+        with self.assertRaises(ValueError):
+            RobotModelParameters(q0=np.array([]))
+
+    def test_armature_default_value(self):
+        """Test that the armature is set to default if not provided."""
+        robot = robex.load("panda")
+        urdf_path = Path(robot.urdf)
+        srdf_path = Path(robot.urdf.replace("urdf", "srdf"))
+        urdf_meshes_dir = urdf_path.parent.parent.parent.parent.parent
+        locked_joint_names = ["panda_joint1", "panda_joint2"]
+        free_flyer = False
+        q0 = np.zeros(robot.model.nq)
+        params = RobotModelParameters(
+            q0=q0,
+            free_flyer=free_flyer,
+            locked_joint_names=locked_joint_names,
+            urdf_path=urdf_path,
+            srdf_path=srdf_path,
+            urdf_meshes_dir=urdf_meshes_dir,
+            collision_as_capsule=True,
+            self_collision=True,
+        )
+        self.assertTrue(np.array_equal(params.armature, np.zeros_like(q0)))
+
+    def test_armature_mismatched_shape_raises_error(self):
+        """Test that a mismatched armature raises a ValueError."""
+        q0 = np.array([0.0, 1.0, 2.0])
+        armature = np.array([0.1, 0.2])  # Wrong shape
+        with self.assertRaises(ValueError):
+            RobotModelParameters(q0=q0, armature=armature)
+
+    def test_invalid_urdf_path_raises_error(self):
+        """Test that an invalid URDF path raises a ValueError."""
+        q0 = np.array([0.0, 1.0, 2.0])
+        with self.assertRaises(ValueError):
+            RobotModelParameters(q0=q0, urdf_path=Path("invalid_path"))
+
+    def test_invalid_srdf_path_type_raises_error(self):
+        """Test that a non-string SRDF path raises a ValueError."""
+        q0 = np.array([0.0, 1.0, 2.0])
+        with self.assertRaises(ValueError):
+            RobotModelParameters(q0=q0, srdf_path=Path("invalid_path"))
+
+
+class TestRobotModels(unittest.TestCase):
+    @classmethod
+    def setUpClass(self):
+        """
+        This method sets up the shared environment for all test cases in the class.
+        """
+        # Load the example robot model using example robot data to get the URDF path.
+        robot = robex.load("panda")
+        urdf_path = Path(robot.urdf)
+        srdf_path = Path(robot.urdf.replace("urdf", "srdf"))
+        urdf_meshes_dir = urdf_path.parent.parent.parent.parent.parent
+        free_flyer = False
+        q0 = np.zeros(robot.model.nq)
+        locked_joint_names = ["panda_joint1", "panda_joint2"]
+        # Store shared initial parameters
+        self.shared_params = RobotModelParameters(
+            q0=q0,
+            free_flyer=free_flyer,
+            locked_joint_names=locked_joint_names,
+            urdf_path=urdf_path,
+            srdf_path=srdf_path,
+            urdf_meshes_dir=urdf_meshes_dir,
+            collision_as_capsule=True,
+            self_collision=True,
+            armature=np.linspace(0.1, 0.9, robot.model.nq),
+        )
+
+    def setUp(self):
+        """
+        This method ensures that a fresh RobotModelParameters and RobotModels instance
+        are created for each test case while still benefiting from shared setup computations.
+        """
+        self.params = deepcopy(self.shared_params)
+        self.robot_models = RobotModels(self.params)
+
+    def test_initial_configuration(self):
+        self.assertTrue(np.array_equal(self.robot_models.q0, self.params.q0))
+
+    def test_load_models_populates_models(self):
+        self.robot_models.load_models()
+        self.assertIsNotNone(self.robot_models.full_robot_model)
+        self.assertIsNotNone(self.robot_models.visual_model)
+        self.assertIsNotNone(self.robot_models.collision_model)
+
+    def test_reduced_robot_model(self):
+        self.robot_models.load_models()
+        self.assertTrue(
+            self.robot_models.robot_model.nq
+            == self.robot_models.full_robot_model.nq
+            - len(self.params.locked_joint_names)
+        )
+
+    def test_invalid_joint_name_raises_value_error(self):
+        # Modify a fresh instance of parameters for this test
+        self.params.locked_joint_names = ["InvalidJoint"]
+        with self.assertRaises(ValueError):
+            self.robot_models._apply_locked_joints()
+
+    def test_armature_property(self):
+        self.assertTrue(
+            np.array_equal(self.robot_models.armature, self.params.armature)
+        )
+
+    def test_collision_pairs(self):
+        """Checking that the collision model has collision pairs."""
+        self.assertTrue(
+            len(self.robot_models.collision_model.collisionPairs) == 44
+        )  # Number of collision pairs in the panda model
+
+    def test_rnea(self):
+        """Checking that the RNEA method works."""
+        q = np.zeros(self.robot_models.robot_model.nq)
+        v = np.zeros(self.robot_models.robot_model.nv)
+        a = np.zeros(self.robot_models.robot_model.nv)
+        robot_data = self.robot_models.robot_model.createData()
+        pin.rnea(self.robot_models.robot_model, robot_data, q, v, a)
+
+
+if __name__ == "__main__":
+    unittest.main()