diff --git a/docs/examples/05_backends_pybullet/files/02_forward_kinematics.py b/docs/examples/05_backends_pybullet/files/02_forward_kinematics.py
index 222735ecf..ed7d201ea 100644
--- a/docs/examples/05_backends_pybullet/files/02_forward_kinematics.py
+++ b/docs/examples/05_backends_pybullet/files/02_forward_kinematics.py
@@ -4,6 +4,8 @@
 from compas_fab.backends import PyBulletPlanner
 from compas_fab.robots import RobotCell
 from compas_fab.robots import RobotCellState
+from compas_fab.robots import RobotLibrary
+
 
 # #############################################
 # Headless (no-GUI) forwards kinematics example
@@ -12,8 +14,7 @@
 # 'direct' mode
 with PyBulletClient("direct") as client:
     # The robot in this example is loaded from a URDF file
-    urdf_filename = compas_fab.get("robot_library/ur5_robot/urdf/robot_description.urdf")
-    robot = client.load_robot(urdf_filename)
+    robot = RobotLibrary.ur5()
 
     # The planner object is needed to call the forward kinematics function
     planner = PyBulletPlanner(client)
diff --git a/docs/examples/05_backends_pybullet/files/02_inverse_kinematics.py b/docs/examples/05_backends_pybullet/files/02_inverse_kinematics.py
index d0aaf4cbe..2de450c8c 100644
--- a/docs/examples/05_backends_pybullet/files/02_inverse_kinematics.py
+++ b/docs/examples/05_backends_pybullet/files/02_inverse_kinematics.py
@@ -4,12 +4,14 @@
 from compas_fab.backends import PyBulletPlanner
 
 from compas_fab.robots import FrameTarget
+from compas_fab.robots import RobotCell
 from compas_fab.robots import RobotCellState
+from compas_fab.robots import RobotLibrary
 
 with PyBulletClient() as client:
-    urdf_filename = compas_fab.get("robot_library/ur5_robot/urdf/robot_description.urdf")
-    robot = client.load_robot(urdf_filename)
+    robot = RobotLibrary.ur5()
     planner = PyBulletPlanner(client)
+    planner.set_robot_cell(RobotCell(robot))
 
     frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
     target = FrameTarget(frame_WCF)
diff --git a/docs/examples/05_backends_pybullet/files/02_iter_inverse_kinematics.py b/docs/examples/05_backends_pybullet/files/02_iter_inverse_kinematics.py
index e696756d3..ba13663e6 100644
--- a/docs/examples/05_backends_pybullet/files/02_iter_inverse_kinematics.py
+++ b/docs/examples/05_backends_pybullet/files/02_iter_inverse_kinematics.py
@@ -4,12 +4,14 @@
 from compas_fab.backends import PyBulletPlanner
 
 from compas_fab.robots import FrameTarget
+from compas_fab.robots import RobotCell
 from compas_fab.robots import RobotCellState
+from compas_fab.robots import RobotLibrary
 
 with PyBulletClient(connection_type="direct") as client:
-    urdf_filename = compas_fab.get("robot_library/ur5_robot/urdf/robot_description.urdf")
-    robot = client.load_robot(urdf_filename)
+    robot = RobotLibrary.ur5()
     planner = PyBulletPlanner(client)
+    planner.set_robot_cell(RobotCell(robot))
 
     frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
     target = FrameTarget(frame_WCF)
diff --git a/src/compas_fab/backends/interfaces/client.py b/src/compas_fab/backends/interfaces/client.py
index cde566c4d..e5b599db2 100644
--- a/src/compas_fab/backends/interfaces/client.py
+++ b/src/compas_fab/backends/interfaces/client.py
@@ -25,9 +25,6 @@ class ClientInterface(object):
     ----------
     robot : :class:`compas_fab.robots.Robot`, read-only
         The robot instance associated with the client.
-        Typically this is set by the backend client after it is initialized, using
-        `client.load_robot()`. It cannot be changed after it is set.
-        Consult the chosen backend client for how to set this attribute.
     """
 
     def __init__(self):
diff --git a/src/compas_fab/backends/pybullet/backend_features/__init__.py b/src/compas_fab/backends/pybullet/backend_features/__init__.py
index dca7f2f05..95df12bd5 100644
--- a/src/compas_fab/backends/pybullet/backend_features/__init__.py
+++ b/src/compas_fab/backends/pybullet/backend_features/__init__.py
@@ -21,29 +21,15 @@
 from __future__ import absolute_import
 
 
-from compas_fab.backends.pybullet.backend_features.pybullet_add_attached_collision_mesh import (
-    PyBulletAddAttachedCollisionMesh,
-)
-from compas_fab.backends.pybullet.backend_features.pybullet_add_collision_mesh import PyBulletAddCollisionMesh
-from compas_fab.backends.pybullet.backend_features.pybullet_append_collision_mesh import PyBulletAppendCollisionMesh
 from compas_fab.backends.pybullet.backend_features.pybullet_forward_kinematics import PyBulletForwardKinematics
 from compas_fab.backends.pybullet.backend_features.pybullet_inverse_kinematics import PyBulletInverseKinematics
-from compas_fab.backends.pybullet.backend_features.pybullet_remove_attached_collision_mesh import (
-    PyBulletRemoveAttachedCollisionMesh,
-)
-from compas_fab.backends.pybullet.backend_features.pybullet_remove_collision_mesh import PyBulletRemoveCollisionMesh
 from compas_fab.backends.pybullet.backend_features.pybullet_set_robot_cell import PyBulletSetRobotCell
 from compas_fab.backends.pybullet.backend_features.pybullet_set_robot_cell_state import PyBulletSetRobotCellState
 
 
 __all__ = [
-    "PyBulletAddAttachedCollisionMesh",
-    "PyBulletAddCollisionMesh",
-    "PyBulletAppendCollisionMesh",
     "PyBulletForwardKinematics",
     "PyBulletInverseKinematics",
-    "PyBulletRemoveAttachedCollisionMesh",
-    "PyBulletRemoveCollisionMesh",
     "PyBulletSetRobotCell",
     "PyBulletSetRobotCellState",
 ]
diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_add_attached_collision_mesh.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_add_attached_collision_mesh.py
deleted file mode 100644
index ee14a2815..000000000
--- a/src/compas_fab/backends/pybullet/backend_features/pybullet_add_attached_collision_mesh.py
+++ /dev/null
@@ -1,95 +0,0 @@
-from __future__ import absolute_import
-from __future__ import division
-from __future__ import print_function
-
-import itertools
-import os
-
-from compas.geometry import Frame
-from compas_robots.model import Inertia
-from compas_robots.model import Inertial
-from compas_robots.model import Joint
-from compas_robots.model import Mass
-
-from compas_fab.backends.interfaces import AddAttachedCollisionMesh
-from compas_fab.utilities import LazyLoader
-
-pybullet = LazyLoader("pybullet", globals(), "pybullet")
-
-
-__all__ = [
-    "PyBulletAddAttachedCollisionMesh",
-]
-
-
-class PyBulletAddAttachedCollisionMesh(AddAttachedCollisionMesh):
-    """Callable to add a collision mesh and attach it to the robot."""
-
-    def add_attached_collision_mesh(self, attached_collision_mesh, options=None):
-        """Add a collision mesh and attach it to the robot.
-
-        Parameters
-        ----------
-        attached_collision_mesh : :class:`compas_fab.robots.AttachedCollisionMesh`
-            Object containing the collision mesh to be attached.
-        options : dict
-            Dictionary containing the following key-value pairs:
-
-            - ``"robot"``: (:class:`compas_fab.robots.Robot`) Robot instance
-              to which the object should be attached.
-            - ``"mass"``: (:obj:`float`) The mass of the attached collision
-              object.  Defaults to ``1``.
-            - ``"inertia"``: (:obj:`list`) The elements of the inertia matrix
-              of the attached collision object given as
-              ``[<ixx>, <ixy>, <ixz>, <iyy>, <iyz>, <izz>]``.  Defaults to
-              ``[1., 0., 0., 1., 0., 1.]``.
-            - ``"inertial_origin"``: (:class:`compas.geometry.Frame`) This is
-              the pose of the inertial reference frame, relative to the link
-              reference frame. Defaults to
-              :class:`compas.geometry.Frame.worldXY()`.
-            - ``"collision_origin"``: (:class:`compas.geometry.Frame`) This is
-              the pose of the collision reference frame, relative to the link
-              reference frame. Defaults to
-              :class:`compas.geometry.Frame.worldXY()`.
-            - ``"concavity"``: (:obj:`bool`) When ``False`` (the default),
-              the mesh will be loaded as its convex hull for collision checking purposes.
-              When ``True``, a non-static mesh will be decomposed into convex parts using v-HACD.
-
-        Returns
-        -------
-        ``None``
-        """
-        robot = options["robot"]
-        self.client.ensure_cached_robot_model_geometry(robot)
-
-        mass = options.get("mass", 1.0)
-        concavity = options.get("concavity", False)
-        inertia = options.get("inertia", [1.0, 0.0, 0.0, 1.0, 0.0, 1.0])
-        inertial_origin = options.get("inertial_origin", Frame.worldXY())
-        collision_origin = options.get("collision_origin", Frame.worldXY())
-
-        cached_robot_model = self.client.get_cached_robot_model(robot)
-
-        # add link
-        mesh = attached_collision_mesh.collision_mesh.mesh
-        name = attached_collision_mesh.collision_mesh.id
-        mesh_file_name = name + ".obj"
-        mesh_fp = os.path.join(self.client._cache_dir.name, mesh_file_name)
-        mesh.to_obj(mesh_fp)
-        mesh_fp = self.client._handle_concavity(mesh_fp, self.client._cache_dir.name, concavity, mass, name)
-        link = cached_robot_model.add_link(name, visual_meshes=[mesh], collision_meshes=[mesh])
-        mass_urdf = Mass(mass)
-        inertia_urdf = Inertia(*inertia)
-        inertial_origin_urdf = Frame(inertial_origin.point, inertial_origin.xaxis, inertial_origin.yaxis)
-        inertial_urdf = Inertial(inertial_origin_urdf, mass_urdf, inertia_urdf)
-        link.inertial = inertial_urdf
-        collision_origin_urdf = Frame(collision_origin.point, collision_origin.xaxis, collision_origin.yaxis)
-        for element in itertools.chain(link.visual, link.collision):
-            element.geometry.shape.filename = mesh_fp
-            element.origin = collision_origin_urdf
-
-        # add joint
-        parent_link = cached_robot_model.get_link_by_name(attached_collision_mesh.link_name)
-        cached_robot_model.add_joint(name + "_fixed_joint", Joint.FIXED, parent_link, link)
-
-        self.client.reload_from_cache(robot)
diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_add_collision_mesh.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_add_collision_mesh.py
deleted file mode 100644
index 9d4502b78..000000000
--- a/src/compas_fab/backends/pybullet/backend_features/pybullet_add_collision_mesh.py
+++ /dev/null
@@ -1,49 +0,0 @@
-from __future__ import absolute_import
-from __future__ import division
-from __future__ import print_function
-
-from compas_fab.backends.interfaces import AddCollisionMesh
-
-__all__ = [
-    "PyBulletAddCollisionMesh",
-]
-
-from compas_fab.backends.pybullet.const import STATIC_MASS
-
-
-class PyBulletAddCollisionMesh(AddCollisionMesh):
-    """Callable to add a collision mesh to the planning scene."""
-
-    def add_collision_mesh(self, collision_mesh, options=None):
-        """Add a collision mesh to the planning scene.
-
-        Parameters
-        ----------
-        collision_mesh : :class:`compas_fab.robots.CollisionMesh`
-            Object containing the collision mesh to be added.
-        options : dict
-            Dictionary containing the following key-value pairs:
-
-            - ``"mass"``: (:obj:`float`) The mass of the object, in kg.
-              If `0` is given, (the default), the object added is static.
-            - ``"concavity"``: (:obj:`bool`) When ``False`` (the default),
-              the mesh will be loaded as its convex hull for collision checking purposes.
-              When ``True``, a non-static mesh will be decomposed into convex parts using v-HACD.
-
-        Returns
-        -------
-        ``None``
-        """
-        options = options or {}
-        mesh = collision_mesh.mesh
-        name = collision_mesh.id
-        frame = collision_mesh.frame
-        mass = options.get("mass", STATIC_MASS)
-        concavity = options.get("concavity", False)
-
-        # mimic ROS' behavior: collision object with same name is replaced
-        if name in self.client.collision_objects:
-            self.client.remove_collision_mesh(name)
-
-        body_id = self.client.convert_mesh_to_body(mesh, frame, concavity, mass)
-        self.client.collision_objects[name] = [body_id]
diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_append_collision_mesh.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_append_collision_mesh.py
deleted file mode 100644
index 6d2531959..000000000
--- a/src/compas_fab/backends/pybullet/backend_features/pybullet_append_collision_mesh.py
+++ /dev/null
@@ -1,41 +0,0 @@
-from __future__ import absolute_import
-from __future__ import division
-from __future__ import print_function
-
-from compas_fab.backends.interfaces import AppendCollisionMesh
-
-__all__ = [
-    "PyBulletAppendCollisionMesh",
-]
-
-
-class PyBulletAppendCollisionMesh(AppendCollisionMesh):
-    """Callable to append a collision mesh to the planning scene."""
-
-    def append_collision_mesh(self, collision_mesh, options=None):
-        """Append a collision mesh to the planning scene.
-
-        Parameters
-        ----------
-        collision_mesh : :class:`compas_fab.robots.CollisionMesh`
-            Object containing the collision mesh to be appended.
-        options : dict
-            Dictionary containing the following key-value pairs:
-
-            - ``"concavity"``: (:obj:`bool`) When ``False`` (the default),
-              the mesh will be loaded as its convex hull for collision checking purposes.
-              When ``True``, a non-static mesh will be decomposed into convex parts using v-HACD.
-
-        Returns
-        -------
-        ``None``
-        """
-        mesh = collision_mesh.mesh
-        name = collision_mesh.id
-        frame = collision_mesh.frame
-        concavity = options.get("concavity", False)
-        if name in self.client.collision_objects:
-            body_id = self.client.convert_mesh_to_body(mesh, frame, concavity)
-            self.client.collision_objects[name].append(body_id)
-        else:
-            self.client.add_collision_mesh(collision_mesh, options)
diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py
index b7fb1c759..f6ad7db7c 100644
--- a/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py
+++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py
@@ -74,8 +74,6 @@ def forward_kinematics(self, robot_cell_state, group=None, options=None):
         planner = self  # type: PyBulletPlanner
         robot = client.robot
         group = group or robot.main_group_name
-        cached_robot_model = client.get_cached_robot_model(robot)
-        robot_body_id = client.get_uid(cached_robot_model)
 
         # Setting the entire robot cell state, including the robot configuration
         planner.set_robot_cell_state(robot_cell_state)
@@ -89,13 +87,13 @@ def forward_kinematics(self, robot_cell_state, group=None, options=None):
         if link_name:
             if link_name not in robot.get_link_names(group):
                 raise KeyError("Link name provided is not part of the group")
-            link_id = client._get_link_id_by_name(link_name, cached_robot_model)
-            fk_frame = client._get_link_frame(link_id, robot_body_id)
+            link_id = client.robot_link_puids[link_name]
+            fk_frame = client._get_link_frame(link_id, client.robot_puid)
 
         else:
             link_name = robot.get_end_effector_link_name(group)
-            link_id = client._get_link_id_by_name(link_name, cached_robot_model)
-            fk_frame = client._get_link_frame(link_id, robot_body_id)
+            link_id = client.robot_link_puids[link_name]
+            fk_frame = client._get_link_frame(link_id, client.robot_puid)
 
             # If no link name provided, and a tool is attached to the group, return the tool tip frame of the tool
             robot_cell = planner.robot_cell  # type: RobotCell
diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py
index 42c839623..9dd2e858a 100644
--- a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py
+++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py
@@ -108,9 +108,8 @@ def iter_inverse_kinematics_frame_target(self, target, start_state=None, group=N
         max_results = options.get("max_results", 100)
         link_name = options.get("link_name") or robot.get_end_effector_link_name(group)
 
-        cached_robot_model = client.get_cached_robot_model(robot)
-        body_id = client.get_uid(cached_robot_model)
-        link_id = client._get_link_id_by_name(link_name, cached_robot_model)
+        body_id = client.robot_puid
+        link_id = client.robot_link_puids[link_name]
 
         # Target frame
         target = self._scale_input_target(target)
@@ -123,19 +122,22 @@ def iter_inverse_kinematics_frame_target(self, target, start_state=None, group=N
 
         point, orientation = pose_from_frame(target_frame)
 
-        joints = cached_robot_model.get_configurable_joints()
-        joints.sort(key=lambda j: j.attr["pybullet"]["id"])
-        joint_names = [joint.name for joint in joints]
+        # Get list of keys (joint_name) from the joint_ids dict in the order of its values (puid)
+        joint_names_sorted, joint_ids_sorted = client.get_configurable_joint_names_and_puid()
 
-        lower_limits = [joint.limit.lower if joint.type != Joint.CONTINUOUS else 0 for joint in joints]
-        upper_limits = [joint.limit.upper if joint.type != Joint.CONTINUOUS else 2 * math.pi for joint in joints]
-
-        start_configuration = start_state.robot_configuration or robot.zero_configuration(group)
-        start_configuration = client.set_robot_configuration(start_configuration, group)
+        # Get joint limits in the same order as the joint_ids
+        lower_limits = []
+        upper_limits = []
+        for joint_name in joint_names_sorted:
+            joint = robot.get_joint_by_name(joint_name)
+            lower_limits.append(joint.limit.lower if joint.type != Joint.CONTINUOUS else 0)
+            upper_limits.append(joint.limit.upper if joint.type != Joint.CONTINUOUS else 2 * math.pi)
 
         # Rest pose is PyBullet's way of defining the initial guess for the IK solver
+        start_configuration = start_state.robot_configuration or robot.zero_configuration(group)
+        client.set_robot_configuration(start_configuration)  # Not sure if this line is needed.
         # The order of the values needs to match with pybullet's joint id order
-        rest_poses = self._get_rest_poses(joint_names, start_configuration)
+        rest_poses = [start_configuration[joint_name] for joint_name in joint_names_sorted]
 
         # Prepare Parameters for calling pybullet.calculateInverseKinematics
         ik_options = dict(
@@ -181,7 +183,7 @@ def iter_inverse_kinematics_frame_target(self, target, start_state=None, group=N
         if high_accuracy:
             ik_options.update(
                 dict(
-                    joints=joints,
+                    joint_ids_sorted=joint_ids_sorted,
                     threshold=options.get("high_accuracy_threshold", 1e-4),
                     max_iter=options.get("high_accuracy_max_iter", 20),
                 )
@@ -215,29 +217,26 @@ def iter_inverse_kinematics_frame_target(self, target, start_state=None, group=N
             # because the accurate IK solving says it's not close enough, we can retry
             # with a new randomized seed
             if close_enough:
-                yield joint_positions, joint_names
+                yield joint_positions, joint_names_sorted
 
-            # Randomize joints to get a different solution on the next iter
-            client._set_joint_positions(
-                [joint.attr["pybullet"]["id"] for joint in joints],
-                [random.uniform(*limits) for limits in zip(lower_limits, upper_limits)],
-                body_id,
-            )
+            # Randomize joint values to get a different solution on the next iter
+            for lower_limit, upper_limit, joint_id in zip(lower_limits, upper_limits, joint_ids_sorted):
+                random_value = random.uniform(lower_limit, upper_limit)
+                client._set_joint_position(joint_id, random_value, client.robot_puid)
 
-    def _accurate_inverse_kinematics(self, joints, threshold, max_iter, **kwargs):
+    def _accurate_inverse_kinematics(self, joint_ids_sorted, threshold, max_iter, **kwargs):
         # Based on these examples
         # https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_kinematics_husky_kuka.py#L81
         close_enough = False
         iter = 0
-        joint_ids = [joint.attr["pybullet"]["id"] for joint in joints]
         body_id = kwargs["bodyUniqueId"]
         link_id = kwargs["endEffectorLinkIndex"]
         target_position = kwargs["targetPosition"]
 
         while not close_enough and iter < max_iter:
             joint_poses = pybullet.calculateInverseKinematics(**kwargs)
-            for i in range(len(joint_ids)):
-                pybullet.resetJointState(body_id, joint_ids[i], joint_poses[i])
+            for i in range(len(joint_ids_sorted)):
+                pybullet.resetJointState(body_id, joint_ids_sorted[i], joint_poses[i])
 
             link_state = pybullet.getLinkState(body_id, link_id)
             new_pose = link_state[4]
@@ -256,9 +255,3 @@ def _accurate_inverse_kinematics(self, joints, threshold, max_iter, **kwargs):
             iter += 1
 
         return joint_poses, close_enough
-
-    def _get_rest_poses(self, joint_names, configuration):
-        name_value_map = {
-            configuration.joint_names[i]: configuration.joint_values[i] for i in range(len(configuration.joint_names))
-        }
-        return [name_value_map[name] for name in joint_names]
diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_attached_collision_mesh.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_attached_collision_mesh.py
deleted file mode 100644
index 698b5192c..000000000
--- a/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_attached_collision_mesh.py
+++ /dev/null
@@ -1,45 +0,0 @@
-from __future__ import absolute_import
-from __future__ import division
-from __future__ import print_function
-
-from compas_fab.backends.interfaces import RemoveAttachedCollisionMesh
-from compas_fab.utilities import LazyLoader
-
-pybullet = LazyLoader("pybullet", globals(), "pybullet")
-
-
-__all__ = [
-    "PyBulletRemoveAttachedCollisionMesh",
-]
-
-
-class PyBulletRemoveAttachedCollisionMesh(RemoveAttachedCollisionMesh):
-    """Callable to remove an attached collision mesh from the robot."""
-
-    def remove_attached_collision_mesh(self, id, options=None):
-        """Remove an attached collision mesh from the robot.
-
-        Parameters
-        ----------
-        id : str
-            Name of collision mesh to be removed.
-        options : dict
-            Dictionary containing the following key-value pairs:
-
-            - ``"robot"``: (:class:`compas_fab.robots.Robot`) Robot instance
-              to which the object should be attached.
-
-        Returns
-        -------
-        ``None``
-        """
-        robot = options["robot"]
-        self.client.ensure_cached_robot_model_geometry(robot)
-
-        cached_robot_model = self.client.get_cached_robot_model(robot)
-
-        # remove link and fixed joint
-        cached_robot_model.remove_link(id)
-        cached_robot_model.remove_joint(id + "_fixed_joint")
-
-        self.client.reload_from_cache(robot)
diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_collision_mesh.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_collision_mesh.py
deleted file mode 100644
index 7179b784b..000000000
--- a/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_collision_mesh.py
+++ /dev/null
@@ -1,39 +0,0 @@
-from __future__ import absolute_import
-from __future__ import division
-from __future__ import print_function
-
-from compas_fab.backends.interfaces import RemoveCollisionMesh
-from compas_fab.backends.pybullet.utils import LOG
-from compas_fab.utilities import LazyLoader
-
-pybullet = LazyLoader("pybullet", globals(), "pybullet")
-
-
-__all__ = [
-    "PyBulletRemoveCollisionMesh",
-]
-
-
-class PyBulletRemoveCollisionMesh(RemoveCollisionMesh):
-    """Callable to remove a collision mesh from the planning scene."""
-
-    def remove_collision_mesh(self, id, options=None):
-        """Remove a collision mesh from the planning scene.
-
-        Parameters
-        ----------
-        id : str
-            Name of collision mesh to be removed.
-        options : dict, optional
-            Unused parameter.
-
-        Returns
-        -------
-        ``None``
-        """
-        if id not in self.client.collision_objects:
-            LOG.warning("Collision object with name '{}' does not exist in scene.".format(id))
-            return
-
-        for body_id in self.client.collision_objects[id]:
-            pybullet.removeBody(body_id)
diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py
index 853385a11..922a7d67c 100644
--- a/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py
+++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py
@@ -2,6 +2,7 @@
 
 import compas
 from compas.geometry import Frame
+from compas_fab.robots import RobotCell
 
 if not compas.IPY:
     from typing import TYPE_CHECKING
@@ -16,7 +17,6 @@
         from compas_robots import Configuration  # noqa: F401
         from compas.geometry import Frame  # noqa: F401
         from compas_fab.backends.interfaces import ClientInterface  # noqa: F401
-        from compas_fab.robots import RobotCell  # noqa: F401
         from compas_fab.robots import RobotCellState  # noqa: F401
         from compas_fab.backends import PyBulletClient  # noqa: F401
 
@@ -36,6 +36,8 @@ def set_robot_cell(self, robot_cell, robot_cell_state=None, options=None):
         """
         client = self.client  # type: PyBulletClient
 
+        previous_robot_cell = client.robot_cell or RobotCell()
+
         # TODO: Check for new, modified and removed objects compared to the
         # TODO: previous robot cell state and update the PyBullet world accordingly
 
@@ -52,11 +54,15 @@ def set_robot_cell(self, robot_cell, robot_cell_state=None, options=None):
         for name, rigid_body in robot_cell.rigid_body_models.items():
             client.add_rigid_body(name, rigid_body)
             # client.convert_mesh_to_body(rigid_body.visual_meshes[0], Frame.worldXY())
-        # Update the robot cell in the client
-        self._robot_cell = robot_cell
 
+        # Feed the robot to the client
         if robot_cell.robot:
-            client.load_existing_robot(robot_cell.robot)
+            robot_cell.robot.ensure_geometry()
+            robot_cell.robot.ensure_semantics()
+            client.set_robot(robot_cell.robot)
+
+        # Update the robot cell in the planner
+        client._robot_cell = robot_cell
 
         # If a robot cell state is provided, update the client's robot cell state
         if robot_cell_state:
diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell_state.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell_state.py
index 68852cef1..1484bd091 100644
--- a/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell_state.py
+++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell_state.py
@@ -48,9 +48,10 @@ def set_robot_cell_state(self, robot_cell_state):
 
         # TODO: Check for modified object states and change those only
 
-        # Update the robot configuration
+        # Update the robot configuration if it is provided
         # Note robot_cell_state.robot_configuration is a full configuration
-        client.set_robot_configuration(robot_cell_state.robot_configuration)
+        if robot_cell_state.robot_configuration:
+            client.set_robot_configuration(robot_cell_state.robot_configuration)
 
         # Keep track of tool's base_frames during tool updates
         # They can be used later to update rigid body base frames that are attached to tools
diff --git a/src/compas_fab/backends/pybullet/client.py b/src/compas_fab/backends/pybullet/client.py
index 17241a446..cd1e19a93 100644
--- a/src/compas_fab/backends/pybullet/client.py
+++ b/src/compas_fab/backends/pybullet/client.py
@@ -24,6 +24,7 @@
 from compas_fab.robots import RobotLibrary
 from compas_fab.robots import RobotSemantics
 from compas_fab.robots import RobotCell
+from compas_fab.robots import RobotCellState
 from compas_fab.utilities import LazyLoader
 
 from . import const
@@ -39,8 +40,11 @@
     if TYPE_CHECKING:
         from typing import List  # noqa: F401
         from typing import Tuple  # noqa: F401
+        from typing import Optional  # noqa: F401
         from compas_robots import Configuration  # noqa: F401
+        from compas_robots.resources import AbstractMeshLoader  # noqa: F401
         from compas_fab.robots import RigidBody  # noqa: F401
+
         from compas_fab.backends.kinematics import AnalyticalInverseKinematics  # noqa: F401
         from compas_fab.backends.kinematics import AnalyticalPlanCartesianMotion  # noqa: F401
 
@@ -62,6 +66,7 @@ def __init__(self, connection_type):
         # type: (str) -> None
         self.client_id = None
         self.connection_type = connection_type
+        super(PyBulletBase, self).__init__()
 
     # -------------------------------------------------------------------
     # Functions for connecting and disconnecting from the PyBullet server
@@ -196,7 +201,18 @@ def __init__(self, connection_type="gui", verbose=False):
         super(PyBulletClient, self).__init__(connection_type)
         self.verbose = verbose
 
+        # Robot Cell
+        # The Pybullet client will not keep track of the Robot object directly but uses the one embedded in the RobotCell
+        self._robot_cell = None
+        self._robot_cell_state = None
+
         # PyBullet unique id
+        self.robot_puid = None
+        # Each robot joint has a unique id
+        self.robot_joint_puids = {}  # type: dict[str, int]
+        # Each robot link has a unique id
+        self.robot_link_puids = {}  # type: dict[str, int]
+
         # Each RigidBody can have multiple meshes, so we store a list of puids
         self.rigid_bodies_puids = {}  # type: dict[str, List[int]]
         # Each ToolModel is a single URDF, so we store a single puid
@@ -207,8 +223,30 @@ def __init__(self, connection_type="gui", verbose=False):
         self.disabled_collisions = set()
         self._cache_dir = None
 
+    @property
+    def robot_cell(self):
+        # type: () -> RobotCell
+        """The robot cell that is currently loaded in the PyBullet server."""
+        return self._robot_cell
+
+    @property
+    def robot(self):
+        # type: () -> Robot
+        """The robot that is currently loaded in the PyBullet server."""
+        return self.robot_cell.robot
+
+    @property
+    def robot_cell_state(self):
+        # type: () -> RobotCellState
+        """The state of the robot cell that is currently loaded in the PyBullet server.
+
+        There is typically no reason to access this directly, as the state is managed by the client.
+        However, it can be useful for debugging or introspection.
+        """
+        return self._robot_cell_state
+
     def __enter__(self):
-        self._cache_dir = tempfile.TemporaryDirectory()
+        self._cache_dir = tempfile.TemporaryDirectory(prefix="compas_fab")
         self.connect()
         return self
 
@@ -231,71 +269,55 @@ def step_simulation(self):
         """
         pybullet.stepSimulation(physicsClientId=self.client_id)
 
-    def load_ur5(self, load_geometry=False, concavity=False):
-        # type: (bool, bool) -> Robot
-        """ "Load a UR5 robot to PyBullet.
-
-        Parameters
-        ----------
-        load_geometry : :obj:`bool`, optional
-            Indicate whether to load the geometry of the robot or not.
-        concavity : :obj:`bool`, optional
-            When ``False`` (the default), the mesh will be loaded as its
-            convex hull for collision checking purposes.  When ``True``,
-            a non-static mesh will be decomposed into convex parts using v-HACD.
-
-        Returns
-        -------
-        :class:`compas_fab.robots.Robot`
-            A robot instance.
-        """
-        robot = RobotLibrary.ur5(client=self, load_geometry=load_geometry)
-        robot.ensure_semantics()
+    # ------------------------------------------
+    # Functions for loading and removing objects
+    # ------------------------------------------
 
-        robot.attributes["pybullet"] = {}
-        if load_geometry:
-            self.cache_robot(robot, concavity)
-        else:
-            robot.attributes["pybullet"]["cached_robot_model"] = robot.model
-            robot.attributes["pybullet"]["cached_robot_filepath"] = compas_fab.get(
-                "robot_library/ur5_robot/urdf/robot_description.urdf"
-            )
-
-        urdf_fp = robot.attributes["pybullet"]["cached_robot_filepath"]
-
-        self._load_robot_to_pybullet(urdf_fp, robot)
-        self.disabled_collisions = robot.semantics.disabled_collisions
+    def set_robot(self, robot, concavity=False):
+        # type: (Robot, Optional[bool]) -> Robot
+        """Load an existing robot to PyBullet.
 
-        return robot
+        This function is used by SetRobotCell and should not be called directly by user.
+        The robot must contain geometry and semantics.
 
-    def load_existing_robot(self, robot):
-        # type: (Robot) -> Robot
-        """Load an existing robot to PyBullet.
-        The robot must have its geometry and semantics loaded.
+        If a robot is already loaded, it will be replaced by the new robot.
 
         Parameters
         ----------
         robot : :class:`compas_fab.robots.Robot`
             The robot to be saved for use with PyBullet.
+        concavity : :obj:`bool`, optional
 
         Returns
         -------
         :class:`compas_fab.robots.Robot`
             A robot instance.
         """
-        robot.client = self
-        robot.attributes["pybullet"] = {}
+        if self.robot_puid is not None:
+            self.remove_robot()
+
+        urdf_fp = self.robot_model_to_urdf(robot.model, concavity=concavity)
+        self.robot_puid = robot_puid = self.pybullet_load_urdf(urdf_fp)
 
-        robot.ensure_geometry()
-        robot.ensure_semantics()
-        self.cache_robot(robot)
+        for id in range(self._get_num_joints(robot_puid)):
+            joint_name = self._get_joint_name(id, robot_puid)
+            self.robot_joint_puids[joint_name] = id
+            link_name = self._get_link_name(id, robot_puid)
+            self.robot_link_puids[link_name] = id
 
-        urdf_fp = robot.attributes["pybullet"]["cached_robot_filepath"]
-        self._load_robot_to_pybullet(urdf_fp, robot)
         self.disabled_collisions = robot.semantics.disabled_collisions
 
         return robot
 
+    def remove_robot(self):
+        """Remove the robot from the PyBullet server if it exists."""
+        if self.robot_puid is None:
+            return
+        pybullet.removeBody(self.robot_puid, physicsClientId=self.client_id)
+        self.robot_puid = None
+        self.robot_joint_puids = {}
+        self.robot_link_puids = {}
+
     def add_tool(self, name, tool_model):
         # type: (str, ToolModel) -> Robot
         """Load a ToolModel object to PyBullet.
@@ -308,91 +330,44 @@ def add_tool(self, name, tool_model):
             The tool_model to be loaded into PyBullet.
 
         """
-        file_path = self.cache_robot_model(tool_model)
+        file_path = self.robot_model_to_urdf(tool_model)
         pybullet_uid = self.pybullet_load_urdf(file_path)
         self.tools_puids[name] = pybullet_uid
 
-    def load_robot(self, urdf_file, resource_loaders=None, concavity=False, precision=None):
-        """Create a robot from URDF and load it into PyBullet.
-
-        Robot geometry of the robot can be loaded using the resource loaders.
-        Robot semantics are loaded separately using the :meth:`load_semantics` method.
+    def remove_tool(self, name):
+        # type: (str) -> None
+        """Remove a tool from the PyBullet server if it exists.
 
         Parameters
         ----------
-        urdf_file : :obj:`str` or file object
-            Absolute file path to the urdf file name or file object. The mesh file can be linked by either
-            `"package::"` or relative path.
-        resource_loaders : :obj:`list`
-            List of :class:`compas_robots.AbstractMeshLoader` for loading geometry of the robot.  That the
-            geometry of the robot model is loaded is required before adding or removing attached collision meshes
-            to or from the scene. Defaults to the empty list.
-        concavity : :obj:`bool`
-            When ``False`` (the default), the mesh will be loaded as its
-            convex hull for collision checking purposes.  When ``True``,
-            a non-static mesh will be decomposed into convex parts using v-HACD.
-        precision : int
-            Defines precision for importing/loading meshes. Defaults to ``compas.tolerance.TOL.precision``.
-
-        Returns
-        -------
-        :class:`compas_fab.robots.Robot`
-            The loaded robot instance.
-
-        Notes
-        -----
-        By default, PyBullet will use the convex hull of any mesh loaded from a URDF for collision detection.
-        Amending the link tag as ``<link concave="yes" name="<name of link>">`` will make the mesh concave
-        for static meshes (see this `example <https://github.com/bulletphysics/bullet3/blob/master/data/samurai.urdf>`_).
-        For non-static concave meshes, use the ``concavity`` flag.
+        name : :obj:`str`
+            The name of the tool.
         """
-        robot_model = RobotModel.from_urdf_file(urdf_file)
-        robot = Robot(robot_model, client=self)
-        robot.attributes["pybullet"] = {}
-        if resource_loaders:
-            robot_model.load_geometry(*resource_loaders, precision=precision)
-            self.cache_robot(robot, concavity)
-        else:
-            robot.attributes["pybullet"]["cached_robot_model"] = robot.model
-            robot.attributes["pybullet"]["cached_robot_filepath"] = urdf_file
-
-        urdf_fp = robot.attributes["pybullet"]["cached_robot_filepath"]
-
-        self._load_robot_to_pybullet(urdf_fp, robot)
+        if name not in self.tools_puids:
+            return
+        pybullet.removeBody(self.tools_puids[name], physicsClientId=self.client_id)
+        del self.tools_puids[name]
 
-        return robot
-
-    def load_semantics(self, robot, srdf_filename):
-        """Loads the semantic information of a robot.
+    def add_rigid_body(self, name, rigid_body, concavity=False, mass=const.STATIC_MASS):
+        # type: (str, RigidBody, bool, float) -> int
+        tmp_obj_path = os.path.join(self._cache_dir.name, "{}.obj".format(rigid_body.guid))
 
-        Parameters
-        ----------
-        robot : :class:`compas_fab.robots.Robot`
-            The robot to be saved for use with PyBullet.
-        srdf_filename  : :obj:`str` or file object
-            Absolute file path to the srdf file name.
-        """
-        cached_robot_model = self.get_cached_robot_model(robot)
-        robot.semantics = RobotSemantics.from_srdf_file(srdf_filename, cached_robot_model)
-        self.disabled_collisions = robot.semantics.disabled_collisions
+        # Rigid bodies can have multiple meshes in them, at the moment we join them together as a single mesh
+        # If there are problems with this, we can change it to exporting individual obj files per mesh to Pybullet
+        mesh = Mesh()
+        for m in rigid_body.get_collision_meshes:
+            mesh.join(m)
+        mesh.to_obj(tmp_obj_path)
 
-    def _load_robot_to_pybullet(self, urdf_file, robot):
-        """Load a robot to PyBullet via a temp URDF file."""
-        cached_robot_model = self.get_cached_robot_model(robot)
-        with redirect_stdout(enabled=not self.verbose):
-            pybullet_uid = pybullet.loadURDF(
-                urdf_file, useFixedBase=True, physicsClientId=self.client_id, flags=pybullet.URDF_USE_SELF_COLLISION
-            )
-            # From PyBullet Quickstart Guide:
-            # loadURDF returns a body unique id, a non-negative integer value. If the URDF file cannot be
-            # loaded, this integer will be negative and not a valid body unique id.
-            cached_robot_model.attr["uid"] = pybullet_uid
+        tmp_obj_path = self._handle_concavity(tmp_obj_path, self._cache_dir.name, concavity, mass)
+        pyb_body_id = self.body_from_obj(tmp_obj_path, concavity=concavity, mass=mass)
 
-        self._add_ids_to_robot_joints(cached_robot_model)
-        self._add_ids_to_robot_links(cached_robot_model)
+        # Record the body id in the dictionary
+        assert not pyb_body_id == -1, "Error in creating rigid body in PyBullet. Returning ID is -1."
+        self.rigid_bodies_puids[name] = [pyb_body_id]
 
-        self._robot = robot
-        self._robot_cell = RobotCell(robot)
+    def remove_rigid_body(self, name):
+        raise NotImplementedError("remove_rigid_body is not implemented yet.")
 
     def pybullet_load_urdf(self, urdf_file):
         # type: (str) -> int
@@ -432,80 +407,7 @@ def pybullet_load_urdf(self, urdf_file):
 
             return pybullet_uid
 
-    def reload_from_cache(self, robot):
-        """Reloads the PyBullet server with the robot's cached model.
-
-        Parameters
-        ----------
-        robot : :class:`compas_fab.robots.Robot`
-            The robot to be saved for use with PyBullet.
-
-        """
-        current_configuration = self.get_robot_configuration(robot)
-        cached_robot_model = self.get_cached_robot_model(robot)
-        cached_robot_filepath = self.get_cached_robot_filepath(robot)
-        robot_uid = self.get_uid(cached_robot_model)
-        pybullet.removeBody(robot_uid, physicsClientId=self.client_id)
-
-        cached_robot_model.to_urdf_file(cached_robot_filepath, prettify=True)
-        pybullet.setPhysicsEngineParameter(enableFileCaching=0)
-        self._load_robot_to_pybullet(cached_robot_filepath, robot)
-        pybullet.setPhysicsEngineParameter(enableFileCaching=1)
-
-        self.set_robot_configuration(current_configuration)
-        self.step_simulation()
-
-    def cache_robot(self, robot, concavity=False):
-        """Saves an editable copy of the robot's model and its meshes
-        for shadowing the state of the robot on the PyBullet server.
-
-        Parameters
-        ----------
-        robot : :class:`compas_fab.robots.Robot`
-            The robot to be saved for use with PyBullet.
-        concavity : :obj:`bool`
-            When ``False`` (the default), the mesh will be loaded as its
-            convex hull for collision checking purposes.  When ``True``,
-            a non-static mesh will be decomposed into convex parts using v-HACD.
-
-        Raises
-        ------
-        :exc:`Exception`
-            If geometry has not been loaded.
-
-        """
-        robot.ensure_geometry()
-        # write meshes to cache
-        address_dict = {}
-        # must work with given robot.model here because it has the geometry loaded
-        for link in robot.model.links:
-            for element in itertools.chain(link.visual, link.collision):
-                shape = element.geometry.shape
-                if isinstance(shape, MeshDescriptor):
-                    for mesh in shape.meshes:
-                        mesh_file_name = str(mesh.guid) + ".obj"
-                        fp = os.path.join(self._cache_dir.name, mesh_file_name)
-                        mesh.to_obj(fp)
-                        fp = self._handle_concavity(fp, self._cache_dir.name, concavity, 1, str(mesh.guid))
-                        address_dict[shape.filename] = fp
-
-        # create urdf with new mesh locations
-        urdf = URDF.from_robot(robot.model)
-        meshes = list(urdf.xml.root.iter("mesh"))
-        for mesh in meshes:
-            filename = mesh.attrib["filename"]
-            mesh.attrib["filename"] = address_dict[filename]
-
-        # write urdf
-        cached_robot_model_file_name = str(robot.model.guid) + ".urdf"
-        cached_robot_filepath = os.path.join(self._cache_dir.name, cached_robot_model_file_name)
-        urdf.to_file(cached_robot_filepath, prettify=True)
-        cached_robot_model = RobotModel.from_urdf_file(cached_robot_filepath)
-        robot.attributes["pybullet"]["cached_robot_model"] = cached_robot_model
-        robot.attributes["pybullet"]["cached_robot_filepath"] = cached_robot_filepath
-        robot.attributes["pybullet"]["robot_geometry_cached"] = True
-
-    def cache_robot_model(self, robot_model, concavity=False):
+    def robot_model_to_urdf(self, robot_model, concavity=False):
         """Converts a robot_model to a URDF package.
 
         Parameters
@@ -520,7 +422,7 @@ def cache_robot_model(self, robot_model, concavity=False):
         Return
         ------
         :obj:`str`
-            The file path to the cached robot model.
+            The file path to the robot model URDF package.
 
         Raises
         ------
@@ -529,9 +431,8 @@ def cache_robot_model(self, robot_model, concavity=False):
 
         """
         robot_model.ensure_geometry()
-        # write meshes to cache
-        address_dict = {}
-        # must work with given robot.model here because it has the geometry loaded
+        mesh_precision = 12
+
         for link in robot_model.links:
             for element in itertools.chain(link.visual, link.collision):
                 shape = element.geometry.shape
@@ -546,7 +447,8 @@ def cache_robot_model(self, robot_model, concavity=False):
                     # Join multiple meshes
                     if len(shape.meshes) > 1:
                         joined_mesh = Mesh()
-                        [joined_mesh.update(mesh) for mesh in shape.meshes]
+                        for mesh in shape.meshes:
+                            joined_mesh.join(mesh, False, precision=mesh_precision)
                     else:
                         joined_mesh = shape.meshes[0]
 
@@ -555,132 +457,24 @@ def cache_robot_model(self, robot_model, concavity=False):
                     fp = os.path.join(self._cache_dir.name, mesh_file_name)
                     joined_mesh.to_obj(fp)
                     fp = self._handle_concavity(fp, self._cache_dir.name, concavity, 1, str(joined_mesh.guid))
-                    address_dict[shape.filename] = fp
                     shape.filename = fp
 
         # create urdf with new mesh locations
         urdf = URDF.from_robot(robot_model)
 
-        # Obsolete way to replace mesh filenames in URDF
-        # mesh_tags = list(urdf.xml.root.iter("mesh"))
-        # for mesh in mesh_tags:
-        #     filename = mesh.attrib["filename"]
-        #     mesh.attrib["filename"] = address_dict[filename]
-
         # write urdf
-        cached_robot_model_file_name = str(robot_model.guid) + ".urdf"
-        cached_robot_filepath = os.path.join(self._cache_dir.name, cached_robot_model_file_name)
-        urdf.to_file(cached_robot_filepath, prettify=True)
-        cached_robot_model = RobotModel.from_urdf_file(cached_robot_filepath)
-
-        return cached_robot_filepath
-
-    @staticmethod
-    def ensure_cached_robot_model(robot):
-        """Checks if a :class:`compas_fab.robots.Robot` has been cached for use with PyBullet."""
-        if not robot.attributes["pybullet"]["cached_robot_model"]:
-            raise Exception("This method is only callable once the robot has been cached.")
-
-    @staticmethod
-    def ensure_cached_robot_model_geometry(robot):
-        """Checks if the geometry of a :class:`compas_fab.robots.Robot` has been cached for use with PyBullet."""
-        if not robot.attributes["pybullet"].get("robot_geometry_cached"):
-            raise Exception("This method is only callable once the robot with loaded geometry has been cached.")
-
-    def get_cached_robot_model(self, robot):
-        # type: (Robot) -> RobotModel
-        """Returns the editable copy of the robot's model for shadowing the state
-        of the robot on the PyBullet server.
-
-        Parameters
-        ----------
-        robot : :class:`compas_fab.robots.Robot`
-            The robot saved for use with PyBullet.
-
-        Returns
-        -------
-        :class:`compas_robots.RobotModel`
-
-        Raises
-        ------
-        :exc:`Exception`
-            If the robot has not been cached.
-
-        """
-        self.ensure_cached_robot_model(robot)
-        return robot.attributes["pybullet"]["cached_robot_model"]
-
-    def get_cached_robot_filepath(self, robot):
-        # type: (Robot) -> str
-        """Returns the filepath of the editable copy of the robot's model for shadowing the state
-        of the robot on the PyBullet server.
-
-        Parameters
-        ----------
-        robot : :class:`compas_fab.robots.Robot`
-            The robot saved for use with PyBullet.
-
-        Returns
-        -------
-        :obj:`str`
+        urdf_file_name = str(robot_model.guid) + ".urdf"
+        urdf_filepath = os.path.join(self._cache_dir.name, urdf_file_name)
 
-        Raises
-        ------
-        :exc:`Exception`
-            If the robot has not been cached.
-
-        """
-        self.ensure_cached_robot_model(robot)
-        return robot.attributes["pybullet"]["cached_robot_filepath"]
-
-    def get_uid(self, cached_robot_model):
-        # type: (RobotModel) -> int
-        """Returns the internal PyBullet id of the robot's model for shadowing the state
-        of the robot on the PyBullet server.
+        # Note: Set prettify to True for debug, otherwise filesize is big.
+        urdf.to_file(urdf_filepath, prettify=False)
 
-        Parameters
-        ----------
-        cached_robot_model : :class:`compas_robots.RobotModel`
-            The robot model saved for use with PyBullet.
+        return urdf_filepath
 
-        Returns
-        -------
-        :obj:`int`
+    # --------------------------------
+    # Functions for collision checking
+    # --------------------------------
 
-        """
-        return cached_robot_model.attr["uid"]
-
-    def _add_ids_to_robot_joints(self, cached_robot_model):
-        body_id = self.get_uid(cached_robot_model)
-        joint_ids = self._get_joint_ids(body_id)
-        for joint_id in joint_ids:
-            joint_name = self._get_joint_name(joint_id, body_id)
-            joint = cached_robot_model.get_joint_by_name(joint_name)
-            pybullet_attr = {"id": joint_id}
-            joint.attr.setdefault("pybullet", {}).update(pybullet_attr)
-
-    def _add_ids_to_robot_links(self, robot_model):
-        body_id = self.get_uid(robot_model)
-        joint_ids = self._get_joint_ids(body_id)
-        for link_id in joint_ids:
-            link_name = self._get_link_name(link_id, body_id)
-            link = robot_model.get_link_by_name(link_name)
-            pybullet_attr = {"id": link_id}
-            link.attr.setdefault("pybullet", {}).update(pybullet_attr)
-
-    def _get_joint_id_by_name(self, name, robot_model):
-        return robot_model.get_joint_by_name(name).attr["pybullet"]["id"]
-
-    def _get_joint_ids_by_name(self, names, robot_model):
-        return tuple(self._get_joint_id_by_name(name, robot_model) for name in names)
-
-    def _get_link_id_by_name(self, name, robot_model):
-        return robot_model.get_link_by_name(name).attr["pybullet"]["id"]
-
-    def _get_link_ids_by_name(self, names, robot_model):
-        return tuple(self._get_link_id_by_name(name, robot_model) for name in names)
-
-    # =======================================
     def check_collisions(self, robot, configuration=None):
         """Checks whether the current or given configuration is in collision.
 
@@ -696,11 +490,8 @@ def check_collisions(self, robot, configuration=None):
         ------
         :class:`compas_fab.backends.pybullet.DetectedCollision`
         """
-        cached_robot_model = self.get_cached_robot_model(robot)
-        body_id = self.get_uid(cached_robot_model)
         if configuration:
-            joint_ids = self._get_joint_ids_by_name(configuration.joint_names, cached_robot_model)
-            self._set_joint_positions(joint_ids, configuration.joint_values, body_id)
+            self.set_robot_configuration(configuration)
         self.check_collision_with_objects(robot)
         self.check_robot_self_collision(robot)
 
@@ -719,8 +510,7 @@ def check_collision_with_objects(self, robot):
         """
         for body2_name, body_ids in self.collision_objects.items():
             for body2_id in body_ids:
-                body1_id = self.get_uid(self.get_cached_robot_model(robot))
-                self._check_collision(body1_id, "robot", body2_id, body2_name)
+                self._check_collision(self.robot_puid, "robot", body2_id, body2_name)
 
     def check_robot_self_collision(self, robot):
         """Checks whether the robot and its attached collision objects with its current
@@ -737,22 +527,20 @@ def check_robot_self_collision(self, robot):
         ------
         :class:`compas_fab.backends.pybullet.DetectedCollision`
         """
-        cached_robot_model = self.get_cached_robot_model(robot)
-        robot_body_id = self.get_uid(cached_robot_model)
-        link_names = [link.name for link in cached_robot_model.iter_links() if link.collision]
+        link_names = list(self.robot_link_puids.keys())
         # Check for collisions between robot links
         for link_1_name, link_2_name in combinations(link_names, 2):
             if {link_1_name, link_2_name} in self.unordered_disabled_collisions:
                 continue
-            link_1_id = self._get_link_id_by_name(link_1_name, cached_robot_model)
-            link_2_id = self._get_link_id_by_name(link_2_name, cached_robot_model)
-            self._check_collision(robot_body_id, link_1_name, robot_body_id, link_2_name, link_1_id, link_2_id)
+            link_1_id = self.robot_link_puids[link_1_name]
+            link_2_id = self.robot_link_puids[link_2_name]
+            self._check_collision(self.robot_puid, link_1_name, self.robot_puid, link_2_name, link_1_id, link_2_id)
 
         # Check for collisions between robot links and attached collision objects
         for link_name in link_names:
             for object_name, object_body_ids in self.attached_collision_objects.items():
                 for object_body_id in object_body_ids:
-                    self._check_collision(robot_body_id, link_name, object_body_id, object_name)
+                    self._check_collision(self.robot_puid, link_name, object_body_id, object_name)
 
     def check_collision_objects_for_collision(self):
         """Checks whether any of the collision objects are colliding.
@@ -800,7 +588,30 @@ def _check_collision(self, body_1_id, body_1_name, body_2_id, body_2_name, link_
             LOG.warning("Collision between '{}' and '{}'".format(body_1_name, body_2_name))
             raise CollisionCheckInCollisionError(body_1_name, body_2_name)
 
-    # ======================================
+    # --------------------------------
+    # Functions related to puids
+    # --------------------------------
+
+    def get_configurable_joint_names_and_puid(self):
+        # type: () -> Tuple[List[str], List[int]]
+        """Returns the names and PyBullet unique ids of the configurable joints of the robot.
+
+        The two lists are ordered in the same way using the joint index.
+
+        Returns
+        -------
+        :obj:`tuple` of [:obj:`list` of :obj:`str`, :obj:`list` of :obj:`int`]
+            The first item is a list of joint names of the configurable joints.
+            The second item is a list of PyBullet unique ids of the configurable joints.
+        """
+        configurable_joint_names = self.robot.get_configurable_joint_names()
+        configurable_joint_puids = [self.robot_joint_puids[name] for name in configurable_joint_names]
+
+        configurable_joint_names.sort(key=lambda x: self.robot_joint_puids[x])
+        configurable_joint_puids.sort()
+
+        return configurable_joint_names, configurable_joint_puids
+
     def _get_base_frame(self, body_id):
         pose = pybullet.getBasePositionAndOrientation(body_id, physicsClientId=self.client_id)
         return frame_from_pose(pose)
@@ -830,10 +641,6 @@ def _get_joint_info(self, joint_id, body_id):
     def _get_num_joints(self, body_id):
         return pybullet.getNumJoints(body_id, physicsClientId=self.client_id)
 
-    def _get_joint_ids(self, body_id):
-        # type: (int) -> list[int]
-        return list(range(self._get_num_joints(body_id)))
-
     def _get_joint_name(self, joint_id, body_id):
         return self._get_joint_info(joint_id, body_id).jointName.decode("UTF-8")
 
@@ -842,6 +649,10 @@ def _get_link_name(self, link_id, body_id):
             return self._get_base_name(body_id)
         return self._get_joint_info(link_id, body_id).linkName.decode("UTF-8")
 
+    # ----------------------------------------
+    # Functions for configuration and frames
+    # ----------------------------------------
+
     def _get_link_frame(self, link_id, body_id):
         if link_id == const.BASE_LINK_ID:
             return self._get_base_frame(body_id)
@@ -849,7 +660,6 @@ def _get_link_frame(self, link_id, body_id):
         pose = (link_state.worldLinkFramePosition, link_state.worldLinkFrameOrientation)
         return frame_from_pose(pose)
 
-    # =======================================
     def _set_base_frame(self, frame, body_id):
         point, quaternion = pose_from_frame(frame)
         pybullet.resetBasePositionAndOrientation(body_id, point, quaternion, physicsClientId=self.client_id)
@@ -864,14 +674,17 @@ def _set_joint_positions(self, joint_ids, values, body_id):
             self._set_joint_position(joint_id, value, body_id)
 
     def _get_joint_positions(self, joint_ids, body_id):
+        # type: (List[int], int) -> List[float]
+        """Returns the joint positions of any robot-model-like object in the PyBullet server."""
         joint_states = self._get_joint_states(joint_ids, body_id)
         return [js.jointPosition for js in joint_states]
 
-    def set_robot_configuration(self, configuration, group=None):
-        # type: (Configuration, str) -> Configuration
+    def set_robot_configuration(self, configuration):
+        # type: (Configuration) -> None
         """Sets the robot's pose to the given configuration.
 
-        If visualization is needed, it should be followed by `step_simulation` to update the GUI.
+        Only the joint values in the configuration are set.
+        The other joint values are not changed.
 
         Parameters
         ----------
@@ -883,48 +696,14 @@ def set_robot_configuration(self, configuration, group=None):
             The planning group used for calculation. Defaults to the robot's
             main planning group.
 
-        Returns
-        -------
-        :class:`compas_fab.robots.Configuration`
-            The full configuration of the robot.
         """
-        robot = self.robot  # type: Robot
-        assert robot is not None, "PyBulletClient.robot is None. Robot must be loaded before setting configuration."
-
-        cached_robot_model = self.get_cached_robot_model(robot)
-        body_id = self.get_uid(cached_robot_model)
-        default_config = robot.zero_configuration()
-        full_configuration = robot.merge_group_with_full_configuration(configuration, default_config, group)
-        joint_ids = self._get_joint_ids_by_name(full_configuration.joint_names, cached_robot_model)
-        self._set_joint_positions(joint_ids, full_configuration.joint_values, body_id)
-        return full_configuration
-
-    # def set_robot_full_configuration(self, full_configuration, group=None):
-    #     # type: (Configuration, str) -> Configuration
-    #     """Sets the robot's pose to the given configuration.
-
-    #     If visualization is needed, it should be followed by `step_simulation` to update the GUI.
-
-    #     Parameters
-    #     ----------
-    #     robot : :class:`compas_fab.robots.Robot`
-    #         The robot to be configured.
-    #     full_configuration : :class:`compas_fab.robots.Configuration`
-    #         The configuration to be set, ``joint_names`` must be included in the configuration.
-    #         A full configuration is expected, i.e. all joint values are given.
-    #     group : :obj:`str`, optional
-    #         The planning group used for calculation. Defaults to the robot's
-    #         main planning group.
-
-    #     """
-    #     robot = self.robot # type: Robot
-    #     assert robot is not None, "PyBulletClient.robot is None. Robot must be loaded before setting configuration."
-
-    #     cached_robot_model = self.get_cached_robot_model(robot)
-    #     body_id = self.get_uid(cached_robot_model)
-
-    #     joint_ids = self._get_joint_ids_by_name(full_configuration.joint_names, cached_robot_model)
-    #     self._set_joint_positions(joint_ids, full_configuration.joint_values, body_id)
+        # Check if the robot has been loaded, careful, puid can be zero
+        assert (
+            self.robot_puid is not None
+        ), "PyBulletClient.robot_puid is None. Robot must be loaded before setting configuration."
+
+        for joint_name, joint_value in zip(configuration.joint_names, configuration.joint_values):
+            self._set_joint_position(self.robot_joint_puids[joint_name], joint_value, self.robot_puid)
 
     def get_robot_configuration(self, robot):
         # type: (Robot) -> Configuration
@@ -938,13 +717,13 @@ def get_robot_configuration(self, robot):
         -------
         :class:`compas_robots.Configuration`
         """
-        cached_robot_model = self.get_cached_robot_model(robot)
-        body_id = self.get_uid(cached_robot_model)
-        default_config = robot.zero_configuration()
-        joint_ids = self._get_joint_ids_by_name(default_config.joint_names, cached_robot_model)
-        joint_values = self._get_joint_positions(joint_ids, body_id)
-        default_config.joint_values = joint_values
-        return default_config
+        configuration = robot.zero_configuration()
+
+        joint_ids_ordered = [self.robot_joint_puids[joint_name] for joint_name in configuration.joint_names]
+        joint_values_ordered = self._get_joint_positions(joint_ids_ordered, self.robot_puid)
+        configuration.joint_values = joint_values_ordered
+
+        return configuration
 
     def set_tool_base_frame(self, tool_name, frame):
         # type: (str, Frame) -> None
@@ -984,24 +763,6 @@ def set_object_frame(self, body_id, frame):
     # This includes loading meshes via OBJ files, setting up visual and collision objects
     # ====================================================================================
 
-    def add_rigid_body(self, name, rigid_body, concavity=False, mass=const.STATIC_MASS):
-        # type: (str, RigidBody, bool, float) -> int
-        tmp_obj_path = os.path.join(self._cache_dir.name, "{}.obj".format(rigid_body.guid))
-
-        # Rigid bodies can have multiple meshes in them, at the moment we join them together as a single mesh
-        # If there are problems with this, we can change it to exporting individual obj files per mesh to Pybullet
-        mesh = Mesh()
-        for m in rigid_body.get_collision_meshes:
-            mesh.join(m)
-        mesh.to_obj(tmp_obj_path)
-
-        tmp_obj_path = self._handle_concavity(tmp_obj_path, self._cache_dir.name, concavity, mass)
-        pyb_body_id = self.body_from_obj(tmp_obj_path, concavity=concavity, mass=mass)
-
-        # Record the body id in the dictionary
-        assert not pyb_body_id == -1, "Error in creating rigid body in PyBullet. Returning ID is -1."
-        self.rigid_bodies_puids[name] = [pyb_body_id]
-
     def convert_mesh_to_body(self, mesh, frame, concavity=False, mass=const.STATIC_MASS):
         """Creates a pybullet body from a compas mesh and attaches it to the scene.
 
diff --git a/src/compas_fab/backends/pybullet/planner.py b/src/compas_fab/backends/pybullet/planner.py
index 364c839cf..d583d72f6 100644
--- a/src/compas_fab/backends/pybullet/planner.py
+++ b/src/compas_fab/backends/pybullet/planner.py
@@ -8,8 +8,8 @@
     from typing import TYPE_CHECKING
 
     if TYPE_CHECKING:
-        from compas_fab.backends.interfaces import ClientInterface  # noqa: F401
-        from compas_fab.robots import RobotCell  # noqa: F401
+        from compas_fab.backends import PyBulletClient  # noqa: F401
+        from compas_fab.robots import RobotCell
 
         # Load pybullet for type hinting
         import pybullet
@@ -29,7 +29,14 @@ class PyBulletPlanner(
     """Implement the planner backend interface for PyBullet."""
 
     def __init__(self, client):
-        self._client = client
+        self._client = client  # type: PyBulletClient
 
         # Initialize all mixins
         super(PyBulletPlanner, self).__init__()
+
+    @property
+    def robot_cell(self):
+        # type: () -> RobotCell
+
+        # The PyBulletClient keeps the robot cell in memory instead of the planner
+        return self._client.robot_cell
diff --git a/tests/backends/pybullet/test_pybullet_planner.py b/tests/backends/pybullet/test_pybullet_planner.py
index fe6dbc201..f18cb10a0 100644
--- a/tests/backends/pybullet/test_pybullet_planner.py
+++ b/tests/backends/pybullet/test_pybullet_planner.py
@@ -65,7 +65,6 @@ def validate_ik_with_fk(ik_target_frame, fk_result_frame):
 
 def _test_fk_with_pybullet_planner(robot, true_result):
     with PyBulletClient(connection_type="direct") as client:
-        robot = client.load_existing_robot(robot)
         planner = PyBulletPlanner(client)
         planner.set_robot_cell(RobotCell(robot))
 
@@ -122,7 +121,6 @@ def _test_pybullet_ik_fk_agreement(robot, ik_target_frames):
     ik_options = {"high_accuracy_max_iter": 50, "high_accuracy": True, "high_accuracy_threshold": 1e-5}
 
     with PyBulletClient(connection_type="direct") as client:
-        robot = client.load_existing_robot(robot)
         planner = PyBulletPlanner(client)
         planner.set_robot_cell(RobotCell(robot))
         planning_group = robot.main_group_name
@@ -243,7 +241,6 @@ def test_pybullet_ik_out_of_reach_ur5():
     ik_options = {"high_accuracy_max_iter": 20, "high_accuracy": True, "high_accuracy_threshold": 1e-5}
 
     with PyBulletClient(connection_type="direct") as client:
-        robot = client.load_existing_robot(robot)
         planner = PyBulletPlanner(client)
         planner.set_robot_cell(RobotCell(robot))
         planning_group = robot.main_group_name