Repository: kwesiRutledge/brom_drake-py (language: python)

Issue:
Initial DiagramWatcher Work



Discussion / hints:
## Welcome to [Codecov](https://codecov.io?utm_medium=referral&utm_source=github&utm_content=comment&utm_campaign=pr+comments&utm_term=Kwesi+Rutledge) :tada:

Once you merge this PR into your default branch, you're all set! Codecov will compare coverage reports and display results in all future pull requests.

Thanks for integrating Codecov - We've got you covered :open_umbrella:

Fix patch:
diff --git a/.github/workflows/preserve-examples.yml b/.github/workflows/preserve-examples.yml
index 3e95dd0..fb9943b 100644
--- a/.github/workflows/preserve-examples.yml
+++ b/.github/workflows/preserve-examples.yml
@@ -12,7 +12,8 @@ jobs:
           'examples/watcher/changing_plot_arrangement1/create_monitor_w_different_arrangement.py',
           'examples/conversion/suggested_use1/drakeify.py',
           'examples/conversion/converter_advanced_usage1/demo.py',
-          'examples/scenes/debug/show_me_this_model/suggested_use1.py',
+          'examples/scenes/debug/show_me_this_model/suggested_use1/show.py',
+          'examples/scenes/debug/show_me_this_model/show_collision_geometries1/show.py',
         ]
     steps:
       - name: Checkout
diff --git a/README.md b/README.md
index 855d76c..b87438d 100644
--- a/README.md
+++ b/README.md
@@ -11,6 +11,7 @@ Feature                    |  Code | Results
 :-------------------------:|:-------------------------:|:-------------------------:
 The Diagram Watcher (the `DiagramWatcher` will log + plot all output ports of your `Diagram` automatically) |`add_watcher_and_build()`| ![Creation of Brom Directory](./promo/BromWatcher0.gif)
 The Drake-ify feature (converts your URDF file into a form that Drake can consume) | `drakeify_my_urdf()` | ![Drakeify Example](./promo/BromDrakeifyURDF0.gif)
+Scenes (partially complete robot scenarios to test your algorithms) | (See Examples Directory or the Wiki) | ![ShowMeThisModel Example](./promo/show_me1.png)
 
 (More coming soon...)
 
@@ -91,6 +92,18 @@ watcher, _, _ = add_watcher_and_build(
 The above code tells the watcher to watch the port named `port_name` on the system named `system_name`.
 (If you don't know your system's name in Drake, then you can usually find it by using the `get_name()` method.)
 
+## Citation
+
+Feel free to cite this project if it helped with your work!
+
+```
+@misc{drake,
+  author = "Kwesi Rutledge and the Wrench Robotics Team",
+  title = "Brom: A Helper Library for the Drake Toolbox",
+  year = 2024,
+}
+```
+
 ## FAQs
 
 ### Why the name Brom?
@@ -99,6 +112,9 @@ The above code tells the watcher to watch the port named `port_name` on the syst
 [Inheritance](https://en.wikipedia.org/wiki/Eragon) series by Christopher Paolini.
 He is a wise mentor that helps Eragon (the protagonist) master dragons. ;)
 
+
+
+### How can I support this project?
  
 
 ## Related Work
@@ -118,4 +134,5 @@ Some other work in the open-source drake community:
 - [ ] Add Documentation
 - [ ] Create a method that makes each material in a URDF file have unique names if they have specific values
 - [ ] Add methods for saving the state names on state plots from diagram watcher
-- [ ] Add methods for over or under-approximating complex mehes with cylinders or circles or other shapes.
\ No newline at end of file
+- [X] Add methods for over or under-approximating complex mehes with cylinders or circles or other shapes.
+- [ ] Allow for the user to give "Drake-unfriendly" URDFs to `ShowMeThisModel` scene
\ No newline at end of file
diff --git a/examples/conversion/converter_advanced_usage1/demo.py b/examples/conversion/converter_advanced_usage1/demo.py
index 24f3272..0f50e61 100644
--- a/examples/conversion/converter_advanced_usage1/demo.py
+++ b/examples/conversion/converter_advanced_usage1/demo.py
@@ -17,7 +17,7 @@
 # Internal imports
 from brom_drake import robots
 from brom_drake.all import DrakeReadyURDFConverter, add_watcher_and_build
-from brom_drake.example_helpers import AddGround
+from brom_drake.utils import AddGround
 
 def main():
     # Setup
diff --git a/examples/conversion/suggested_use1/drakeify.py b/examples/conversion/suggested_use1/drakeify.py
index 3a0e0eb..198f4d8 100644
--- a/examples/conversion/suggested_use1/drakeify.py
+++ b/examples/conversion/suggested_use1/drakeify.py
@@ -16,7 +16,7 @@
 # Internal imports
 from brom_drake import robots
 from brom_drake.all import drakeify_my_urdf, add_watcher_and_build
-from brom_drake.example_helpers import AddGround
+from brom_drake.utils import AddGround
 
 def main():
     # Setup
diff --git a/examples/scenes/debug/show_me_this_model/show_collision_geometries1/show.py b/examples/scenes/debug/show_me_this_model/show_collision_geometries1/show.py
new file mode 100644
index 0000000..bea50e9
--- /dev/null
+++ b/examples/scenes/debug/show_me_this_model/show_collision_geometries1/show.py
@@ -0,0 +1,59 @@
+"""
+Description
+-----------
+This script contains an example of how to show
+a given urdf at the specified joint positions.
+"""
+
+import ipdb
+from importlib import resources as impresources
+import numpy as np
+import typer
+from pydrake.systems.analysis import Simulator
+
+# Internal imports
+from brom_drake import robots
+from brom_drake.all import drakeify_my_urdf
+from brom_drake.scenes.debug import ShowMeThisModel
+from brom_drake.file_manipulation.urdf import MeshReplacementStrategy
+
+
+def main():
+    # Setup
+    urdf_file_path = str(
+        impresources.files(robots) / "models/ur/ur10e.urdf"
+    )
+
+    # Convert the URDF
+    new_urdf_path = drakeify_my_urdf(
+        urdf_file_path,
+        overwrite_old_logs=True,
+        log_file_name="drakeify-my-urdf1.log",
+        collision_mesh_replacement_strategy=MeshReplacementStrategy.kWithMinimalEnclosingCylinder,
+    )
+
+    # Visualize the URDF using the "show-me-this-model" feature
+    time_step = 1e-3
+    scene = ShowMeThisModel(
+        str(new_urdf_path),
+        with_these_joint_positions=[0.0, 0.0, -np.pi/4.0, 0.0, 0.0, 0.0],
+        time_step=time_step,
+        show_collision_geometries=True,
+    )
+
+    # Build Diagram
+    diagram, diagram_context = scene.cast_scene_and_build()
+
+    # Set up simulation
+    simulator = Simulator(diagram, diagram_context)
+    simulator.set_target_realtime_rate(1.0)
+    simulator.set_publish_every_time_step(False)
+
+    # Run simulation
+    simulator.Initialize()
+    simulator.AdvanceTo(15.0)
+
+
+if __name__ == "__main__":
+    with ipdb.launch_ipdb_on_exception():
+        typer.run(main)
\ No newline at end of file
diff --git a/examples/scenes/debug/show_me_this_model/suggested_use1/readme.md b/examples/scenes/debug/show_me_this_model/suggested_use1/readme.md
new file mode 100644
index 0000000..9221add
--- /dev/null
+++ b/examples/scenes/debug/show_me_this_model/suggested_use1/readme.md
@@ -0,0 +1,8 @@
+# Suggested Use of the ShowMeThisModel scene
+
+Sometimes, you just want to see what a given model looks like in Drake. For the cases when you know that your model is
+"Drake-ready", then you can simply give it to this scene and it will quickly display it for you in a specified joint configuration.
+
+## Resources
+
+For more details, see [this section of the Wiki](https://github.com/kwesiRutledge/brom_drake-py/wiki/Scenes-%E2%80%90-ShowMeThisModel).
\ No newline at end of file
diff --git a/examples/scenes/debug/show_me_this_model/suggested_use1.py b/examples/scenes/debug/show_me_this_model/suggested_use1/show.py
similarity index 87%
rename from examples/scenes/debug/show_me_this_model/suggested_use1.py
rename to examples/scenes/debug/show_me_this_model/suggested_use1/show.py
index 3df22bb..eb3409c 100644
--- a/examples/scenes/debug/show_me_this_model/suggested_use1.py
+++ b/examples/scenes/debug/show_me_this_model/suggested_use1/show.py
@@ -2,7 +2,9 @@
 Description
 -----------
 This script contains an example of how to show
-a given urdf at the specified joint positions.
+a given urdf at some (optional) specified joint positions.
+For more info, refer to the README or the Wiki:
+https://github.com/kwesiRutledge/brom_drake-py/wiki/Scenes-%E2%80%90-ShowMeThisModel
 """
 
 import ipdb
@@ -16,7 +18,6 @@
 from brom_drake.all import drakeify_my_urdf
 from brom_drake.scenes.debug import ShowMeThisModel
 
-
 def main():
     # Setup
     urdf_file_path = str(
diff --git a/examples/scenes/motion_planning/offline/chem_lab1/example.py b/examples/scenes/motion_planning/offline/chem_lab1/example.py
new file mode 100644
index 0000000..1c1b3bf
--- /dev/null
+++ b/examples/scenes/motion_planning/offline/chem_lab1/example.py
@@ -0,0 +1,73 @@
+"""
+shelf1.py
+Description:
+
+    In this script, we use the Shelf scene to test a basic motion planning algorithms.
+    This shows one how to use the `easy_cast_and_build` method to simplify how to build
+    a scene.
+"""
+import ipdb
+import numpy as np
+from pydrake.all import (
+    Simulator,
+    RollPitchYaw, RigidTransform,
+)
+import typer
+
+# Internal imports
+from brom_drake.motion_planning.algorithms.rrt.connect import RRTConnectPlannerConfig, RRTConnectPlanner
+from brom_drake.scenes.motion_planning.offline import ChemLab1Scene
+
+def main(meshcat_port_number: int = 7001):
+    # Setup
+    if meshcat_port_number < 0:
+        meshcat_port_number = None # Use None for CI
+
+    easy_goal_position = np.array([+0.0, 1.0, 1.0])
+    goal_orientation = RollPitchYaw(np.pi / 2.0, np.pi / 2.0, 0.0).ToQuaternion()
+    goal_pose = RigidTransform(goal_orientation, easy_goal_position)
+
+    # Create the scene
+    scene = ChemLab1Scene(
+        meshcat_port_number=meshcat_port_number, # Use None for CI
+        goal_pose=goal_pose,
+    )
+
+    # Create a planner object which will be used to plan the motion
+    config = RRTConnectPlannerConfig(
+        steering_step_size=0.1,
+        prob_sample_goal=0.15,
+        max_iterations=int(1e4),
+        convergence_threshold=1e-3,
+    )
+    planner2 = RRTConnectPlanner(
+        scene.arm,
+        scene.plant,
+        scene.scene_graph,
+        config=config,
+    )
+
+    # To build the scene, we only need to provide a planning function
+    # (can come from anywhere, not just a BaseRRTPlanner object)
+    diagram, diagram_context = scene.easy_cast_and_build(
+        planner2.plan,
+        with_watcher=True,
+    )
+
+    print("Simulating...")
+
+    # Simulate the diagram
+    simulator = Simulator(diagram, diagram_context)
+    simulator.set_target_realtime_rate(1.0)
+
+    # Run simulation
+    simulator.Initialize()
+    simulator.AdvanceTo(0.1)
+    planned_trajectory = scene.plan_dispenser.planned_trajectory
+    print(planned_trajectory.end_time())
+    # return
+    simulator.AdvanceTo(planned_trajectory.end_time()+1.0)
+
+if __name__ == "__main__":
+    with ipdb.launch_ipdb_on_exception():
+        typer.run(main)
\ No newline at end of file
diff --git a/examples/scenes/motion_planning/offline/shelf/shelf1.py b/examples/scenes/motion_planning/offline/shelf/shelf1.py
new file mode 100644
index 0000000..289ad82
--- /dev/null
+++ b/examples/scenes/motion_planning/offline/shelf/shelf1.py
@@ -0,0 +1,73 @@
+"""
+shelf1.py
+Description:
+
+    In this script, we use the Shelf scene to test a basic motion planning algorithms.
+    This shows one how to use the `easy_cast_and_build` method to simplify how to build
+    a scene.
+"""
+import ipdb
+import numpy as np
+from pydrake.all import (
+    Simulator,
+    RollPitchYaw, RigidTransform,
+)
+import typer
+
+# Internal imports
+from brom_drake.motion_planning.algorithms.rrt.connect import RRTConnectPlannerConfig, RRTConnectPlanner
+from brom_drake.scenes.motion_planning.offline import ShelfPlanningScene
+
+def main(meshcat_port_number: int = 7001):
+    # Setup
+    if meshcat_port_number < 0:
+        meshcat_port_number = None # Use None for CI
+
+    easy_goal_position = np.array([+0.0, 1.0, 1.0])
+    goal_orientation = RollPitchYaw(np.pi / 2.0, np.pi / 2.0, 0.0).ToQuaternion()
+    goal_pose = RigidTransform(goal_orientation, easy_goal_position)
+
+    # Create the scene
+    scene = ShelfPlanningScene(
+        meshcat_port_number=meshcat_port_number, # Use None for CI
+        goal_pose=goal_pose,
+    )
+
+    # Create a planner object which will be used to plan the motion
+    config = RRTConnectPlannerConfig(
+        steering_step_size=0.01,
+        prob_sample_goal=0.025,
+        max_iterations=int(1e5),
+        convergence_threshold=1e-3,
+    )
+    planner2 = RRTConnectPlanner(
+        scene.arm,
+        scene.plant,
+        scene.scene_graph,
+        config=config,
+    )
+
+    # To build the scene, we only need to provide a planning function
+    # (can come from anywhere, not just a BaseRRTPlanner object)
+    diagram, diagram_context = scene.easy_cast_and_build(
+        planner2.plan,
+        with_watcher=True,
+    )
+
+    print("Simulating...")
+
+    # Simulate the diagram
+    simulator = Simulator(diagram, diagram_context)
+    simulator.set_target_realtime_rate(1.0)
+
+    # Run simulation
+    simulator.Initialize()
+    simulator.AdvanceTo(0.1)
+    planned_trajectory = scene.plan_dispenser.planned_trajectory
+    print(planned_trajectory.end_time())
+    # return
+    simulator.AdvanceTo(planned_trajectory.end_time()+1.0)
+
+if __name__ == "__main__":
+    with ipdb.launch_ipdb_on_exception():
+        typer.run(main)
\ No newline at end of file
diff --git a/examples/scenes/motion_planning/offline/shelf1.py b/examples/scenes/motion_planning/offline/shelf1.py
deleted file mode 100644
index acd4d0f..0000000
--- a/examples/scenes/motion_planning/offline/shelf1.py
+++ /dev/null
@@ -1,58 +0,0 @@
-"""
-shelf1.py
-Description:
-
-    In this script, we use the Shelf scene to test some basic motion planning algorithms.
-"""
-import ipdb
-from pydrake.all import Parser
-from pydrake.systems.analysis import Simulator
-import typer
-from pydrake.systems.framework import DiagramBuilder
-from pydrake.trajectories import PiecewisePolynomial
-
-from brom_drake.motion_planning.algorithms.rrt.base import BaseRRTPlannerConfig
-from brom_drake.motion_planning.systems.rrt_plan_generator import RRTPlanGenerator
-# Internal imports
-from brom_drake.scenes.motion_planning.offline import ShelfPlanningScene
-from brom_drake.scenes.roles import kOfflineMotionPlanner
-
-
-def main(use_meshcat: bool = True):
-    # Setup
-    scene = ShelfPlanningScene(
-        meshcat_port_number=None, # Use None for CI (so don't show)
-    )
-
-    # Create dummy cast
-    planner = RRTPlanGenerator(
-        plant=scene.plant,
-        scene_graph=scene.scene_graph,
-        rrt_config=BaseRRTPlannerConfig(
-            prob_sample_goal=0.3,
-            steering_step_size=0.05,
-            max_iterations=int(1e4),
-        ),
-    )
-    cast = [
-        (kOfflineMotionPlanner, planner)
-    ]
-
-    # Build and simulate
-    diagram, diagram_context = scene.cast_scene_and_build(cast)
-
-    # Simulate the diagram
-    simulator = Simulator(diagram, diagram_context)
-    simulator.set_target_realtime_rate(1.0)
-
-    # Run simulation
-    simulator.Initialize()
-    simulator.AdvanceTo(0.1)
-    planned_trajectory = scene.plan_dispenser.planned_trajectory
-    print(planned_trajectory.end_time())
-    # return
-    simulator.AdvanceTo(planned_trajectory.end_time()+1.0)
-
-if __name__ == "__main__":
-    with ipdb.launch_ipdb_on_exception():
-        typer.run(main)
\ No newline at end of file
diff --git a/promo/show_me1.png b/promo/show_me1.png
new file mode 100644
index 0000000..b2f7f8c
Binary files /dev/null and b/promo/show_me1.png differ
diff --git a/src/brom_drake/DiagramWatcher/DiagramWatcher.py b/src/brom_drake/DiagramWatcher/DiagramWatcher.py
index 80a9cbd..7586dee 100644
--- a/src/brom_drake/DiagramWatcher/DiagramWatcher.py
+++ b/src/brom_drake/DiagramWatcher/DiagramWatcher.py
@@ -21,8 +21,8 @@
 from brom_drake.DiagramTarget import DiagramTarget
 from brom_drake.PortWatcher.port_watcher import PortWatcher
 from brom_drake.PortWatcher.port_watcher_options import PortWatcherOptions
-from brom_drake.PortWatcher.port_watcher import FigureNamingConvention
-from brom_drake.DiagramWatcher.constants import INELIGIBLE_SYSTEM_TYPES, DEFAULT_PLOT_DIR
+from brom_drake.DiagramWatcher.constants import INELIGIBLE_SYSTEM_TYPES
+from brom_drake.directories import  DEFAULT_PLOT_DIR
 from brom_drake.DiagramWatcher.errors import UnrecognizedTargetError
 
 
diff --git a/src/brom_drake/DiagramWatcher/constants.py b/src/brom_drake/DiagramWatcher/constants.py
index 2564a8b..f106426 100644
--- a/src/brom_drake/DiagramWatcher/constants.py
+++ b/src/brom_drake/DiagramWatcher/constants.py
@@ -3,6 +3,4 @@
     VectorLogSink, ConstantVectorSource,
 )
 
-INELIGIBLE_SYSTEM_TYPES = [SceneGraph, VectorLogSink, ConstantVectorSource]
-
-DEFAULT_PLOT_DIR = "./brom/watcher_plots"
\ No newline at end of file
+INELIGIBLE_SYSTEM_TYPES = [SceneGraph, VectorLogSink, ConstantVectorSource]
\ No newline at end of file
diff --git a/src/brom_drake/PortWatcher/port_watcher.py b/src/brom_drake/PortWatcher/port_watcher.py
index ba66893..b5c61eb 100644
--- a/src/brom_drake/PortWatcher/port_watcher.py
+++ b/src/brom_drake/PortWatcher/port_watcher.py
@@ -12,12 +12,13 @@
 import matplotlib.pyplot as plt
 import os
 
+from pydrake.multibody.plant import MultibodyPlant
 from pydrake.systems.framework import OutputPort, PortDataType, DiagramBuilder, LeafSystem
 from pydrake.systems.primitives import LogVectorOutput
 from pydrake.systems.framework import Context
 
 # Internal Imports
-from brom_drake.DiagramWatcher.constants import DEFAULT_PLOT_DIR
+from brom_drake.directories import DEFAULT_PLOT_DIR
 from .port_watcher_options import PortWatcherOptions, FigureNamingConvention
 from .port_figure_arrangement import PortFigureArrangement
 
@@ -55,6 +56,54 @@ def __init__(
         self.logger = LogVectorOutput(output_port, builder)
         self.logger.set_name(logger_name)
 
+    def name_of_data_at_index(
+        self,
+        dim_index: int,
+        remove_spaces: bool = False,
+    ) -> str:
+        """
+        Description:
+            Returns the name of the data which is in index dim_index
+            of this vector-valued port.
+        :param dim_index:
+        :param remove_spaces: Whether to remove spaces from the name.
+        :return:
+        """
+        # Setup
+        n_dims = self.port.size()
+
+        # Input Processing
+        if dim_index >= n_dims:
+            raise ValueError(
+                f"dim_index ({dim_index}) is greater than the number of dimensions in the port ({n_dims})."
+            )
+
+        # Default name
+        name = f"Dim #{dim_index}"
+
+        # Only use full names, if we are NOT using the OnePlotPerPort config
+        if self.options.plot_arrangement == PortFigureArrangement.OnePlotPerPort:
+            return name
+
+        if self.system_is_multibody_plant():
+            # The multi-body plant has names for specific ports
+            if self.port.get_name() == "state":
+                # We can get the names of the state
+                state_names = self.system.GetStateNames()
+                name = state_names[dim_index]
+            else:
+                # Announce that we are using the default name
+                loguru.logger.warning(
+                    f"Using default name for data at index {dim_index} of port {self.port.get_name()} of system {self.system.get_name()}."
+                )
+
+        # Filter our spaces, if requested
+        if remove_spaces:
+            name = name.replace(" ", "_")
+
+        # Return name!
+        return name
+
     def plot_logger_data(
         self,
         diagram_context: Context,
@@ -94,6 +143,10 @@ def plot_logger_data(
                     log_times, log_data[port_index, :]
                 )
 
+                # Add axis titles and labels
+                ax_ii.set_xlabel("Time (s)")
+                ax_ii.set_title(self.name_of_data_at_index(port_index))
+
                 # Save figures and axes to lists
                 figs.append(fig_ii)
                 ax_grid.append([ax_ii])
@@ -134,7 +187,14 @@ def plot_logger_data_subplots(
 
         if n_rows == 1 and n_cols == 1:
             ax_list.plot(times, data[0, :])
-            ax_list.set_title(f"Dim #0")
+            ax_list.set_title(self.name_of_data_at_index(0))
+
+        elif (n_rows == 1) or (n_cols == 1):
+            for dim_index in range(n_dims):
+                ax_list[dim_index].plot(times, data[dim_index, :])
+                ax_list[dim_index].set_title(
+                    self.name_of_data_at_index(dim_index),
+                )
 
         else:
             for row_index in range(n_rows):
@@ -147,7 +207,7 @@ def plot_logger_data_subplots(
                         continue
 
                     ax_list[row_index, col_index].plot(times, data[dim_index, :])
-                    ax_list[row_index, col_index].set_title(f"Dim #{dim_index}")
+                    ax_list[row_index, col_index].set_title(self.name_of_data_at_index(dim_index))
 
         return fig, ax_list
 
@@ -189,7 +249,7 @@ def safe_system_name(self) -> str:
 
     def save_figures(self, diagram_context: Context):
         """
-        savefigures
+        save_figures
         Description:
             This function saves the figures.
         TODO(kwesi): Make it so that this function computes names + directory structure based on plot arrangement.
@@ -293,11 +353,19 @@ def figure_names_under_hierarchical_convention(self) -> List[str]:
 
         elif options.plot_arrangement == PortFigureArrangement.OnePlotPerDim:
             return [
-                f"{self.plot_dir}/system_{self.safe_system_name()}/port_{self.port.get_name()}/dim{ii}.{format}"
+                f"{self.plot_dir}/system_{self.safe_system_name()}/port_{self.port.get_name()}/{self.name_of_data_at_index(ii, remove_spaces=True)}.{format}"
                 for ii in range(self.port.size())
             ]
 
         else:
             raise NotImplementedError(
                 f"Invalid plot arrangement for figure naming convention {options.figure_naming_convention}: {options.plot_arrangement}."
-            )
\ No newline at end of file
+            )
+
+    def system_is_multibody_plant(self) -> bool:
+        """
+        Description:
+            Returns True if the system is a MultibodyPlant.
+        :return:
+        """
+        return type(self.system) == MultibodyPlant
\ No newline at end of file
diff --git a/src/brom_drake/all/add_watcher.py b/src/brom_drake/all/add_watcher.py
index 2551f38..4c54a6f 100644
--- a/src/brom_drake/all/add_watcher.py
+++ b/src/brom_drake/all/add_watcher.py
@@ -14,7 +14,7 @@
 
 from brom_drake.DiagramTarget import DiagramTarget
 from brom_drake.DiagramWatcher import DiagramWatcher
-from brom_drake.DiagramWatcher.constants import DEFAULT_PLOT_DIR
+from brom_drake.directories import DEFAULT_PLOT_DIR
 from brom_drake.PortWatcher.port_watcher_options import PortFigureArrangement
 from brom_drake.PortWatcher.port_watcher_options import FigureNamingConvention, PortWatcherOptions
 
diff --git a/src/brom_drake/directories.py b/src/brom_drake/directories.py
new file mode 100644
index 0000000..e0e54a0
--- /dev/null
+++ b/src/brom_drake/directories.py
@@ -0,0 +1,4 @@
+
+DEFAULT_BROM_DIR = "./brom"
+DEFAULT_PLOT_DIR = "./brom/watcher_plots"
+DEFAULT_BROM_MODELS_DIR = "./brom/models"
\ No newline at end of file
diff --git a/src/brom_drake/example_helpers/__init__.py b/src/brom_drake/example_helpers/__init__.py
index 1ca2614..7e3e250 100644
--- a/src/brom_drake/example_helpers/__init__.py
+++ b/src/brom_drake/example_helpers/__init__.py
@@ -1,6 +1,5 @@
-from .block_handler_system import BlockHandlerSystem, AddGround
+from .block_handler_system import BlockHandlerSystem
 
 __all__ = [
     "BlockHandlerSystem",
-    "AddGround",
 ]
\ No newline at end of file
diff --git a/src/brom_drake/example_helpers/block_handler_system.py b/src/brom_drake/example_helpers/block_handler_system.py
index 3534cdf..6240e7c 100644
--- a/src/brom_drake/example_helpers/block_handler_system.py
+++ b/src/brom_drake/example_helpers/block_handler_system.py
@@ -1,6 +1,5 @@
 from importlib import resources as impresources
 
-from pydrake.geometry import HalfSpace
 from pydrake.math import RollPitchYaw, RigidTransform, RotationMatrix
 from pydrake.multibody.math import SpatialVelocity
 from pydrake.multibody.parsing import Parser
@@ -13,7 +12,7 @@
 
 # Internal Imports
 from brom_drake import example_helpers as eh
-
+from brom_drake.utils import AddGround
 
 class BlockHandlerSystem(LeafSystem):
     def __init__(
@@ -116,34 +115,3 @@ def SetInitialBlockState(self,diagram_context):
             self.plant.GetBodyByName(self.block_body_name),
             SpatialVelocity(np.zeros(3),np.array([0.0,0.0,0.0])),
             self.plant.GetMyContextFromRoot(diagram_context))
-
-def AddGround(plant):
-    """
-    Add a flat ground with friction
-    """
-
-    # Constants
-    transparent_color = np.array([0.5, 0.5, 0.5, 0])
-    nontransparent_color = np.array([0.5, 0.5, 0.5, 0.1])
-
-    p_GroundOrigin = [0, 0.0, 0.0]
-    R_GroundOrigin = RotationMatrix.MakeXRotation(0.0)
-    X_GroundOrigin = RigidTransform(R_GroundOrigin,p_GroundOrigin)
-
-    # Set Up Ground on Plant
-
-    surface_friction = CoulombFriction(
-            static_friction = 0.7,
-            dynamic_friction = 0.5)
-    plant.RegisterCollisionGeometry(
-            plant.world_body(),
-            X_GroundOrigin,
-            HalfSpace(),
-            "ground_collision",
-            surface_friction)
-    plant.RegisterVisualGeometry(
-            plant.world_body(),
-            X_GroundOrigin,
-            HalfSpace(),
-            "ground_visual",
-            transparent_color)  # transparent
\ No newline at end of file
diff --git a/src/brom_drake/file_manipulation/urdf/DrakeReadyURDFConverter.py b/src/brom_drake/file_manipulation/urdf/DrakeReadyURDFConverter.py
index 937061d..be1e894 100644
--- a/src/brom_drake/file_manipulation/urdf/DrakeReadyURDFConverter.py
+++ b/src/brom_drake/file_manipulation/urdf/DrakeReadyURDFConverter.py
@@ -5,26 +5,36 @@
 arbitrary URDFs into URDFs that the Drake toolbox can
 support.
 """
+from enum import IntEnum
 import os
 from copy import copy, deepcopy
-from typing import Tuple
 
 import trimesh
 from doctest import UnexpectedException
 
 import loguru
-from multiprocessing.managers import Value
+import numpy as np
 from pathlib import Path
+from pydrake.all import RigidTransform, RotationMatrix, RollPitchYaw
 import xml.etree.ElementTree as ET
 
+# Internal Imports
+from brom_drake.directories import DEFAULT_BROM_MODELS_DIR
 from .mesh_file_converter import MeshFileConverter
-# Internal Supports
 from .util import (
     does_drake_parser_support,
     URDF_CONVERSION_LOG_LEVEL_NAME, URDF_CONVERSION_LEVEL,
     tree_contains_transmission_for_joint,
     create_transmission_element_for_joint,
 )
+from brom_drake.file_manipulation.urdf.shapes.cylinder import CylinderDefinition
+
+class MeshReplacementStrategy(IntEnum):
+    """
+    The different strategies for replacing mesh files.
+    """
+    kWithObj = 1
+    kWithMinimalEnclosingCylinder = 2
 
 
 class DrakeReadyURDFConverter:
@@ -35,17 +45,19 @@ def __init__(
         overwrite_old_models: bool = False,
         overwrite_old_logs: bool = False,
         log_file_name: str = "conversion.log",
+        collision_mesh_replacement_strategy: MeshReplacementStrategy = MeshReplacementStrategy.kWithObj,
     ):
         # Setup
         self.original_urdf_filename = original_urdf_filename
         self.output_urdf_file_path = output_urdf_file_path
         self.log_file_name = log_file_name
+        self.collision_mesh_replacement_strategy = collision_mesh_replacement_strategy
 
         # Define conversion data directory in brom
         # (If it already exists, then we will overwrite it with the proper flag set)
-        self.models_dir = Path("./brom/models/")
-        if overwrite_old_logs and os.path.exists(self.models_dir / self.log_file_name):
-            os.remove(self.models_dir / self.log_file_name)
+        self.models_dir = Path(DEFAULT_BROM_MODELS_DIR)
+        if overwrite_old_logs and os.path.exists(self.output_file_directory() / self.log_file_name):
+            os.remove(self.output_file_directory() / self.log_file_name)
         if overwrite_old_models:
             self.clean_up_models_dir()
         os.makedirs(self.models_dir, exist_ok=True)
@@ -106,6 +118,80 @@ def configure_logger(self):
             filter=lambda record: record['level'].name == URDF_CONVERSION_LOG_LEVEL_NAME,
         )
 
+    def convert_collision_element(self, collision_elt: ET.Element) -> ET.Element:
+        """
+        Description
+        -----------
+        This method will convert the collision element in the URDF file.
+        :param collision_elt:
+        :return:
+        """
+        # Setup
+        new_elt = deepcopy(collision_elt)
+
+        # Algorithm
+        self.log(
+            f"Converting element with tag \"{collision_elt.tag}\" using strategy {self.collision_mesh_replacement_strategy}."
+        )
+
+        # Check to see if the collision element has a mesh element
+        mesh_elt = new_elt.find("geometry/mesh")
+        if mesh_elt is None:
+            self.log(
+                f"Collision element does not contain a mesh element. No modification needed..."
+            )
+            return new_elt
+
+        # If the mesh element exists, then we will try to convert it depending on the strategy
+        replacement_strategy = self.collision_mesh_replacement_strategy
+        if replacement_strategy == MeshReplacementStrategy.kWithObj:
+            new_mesh_elt = self.convert_mesh_element(mesh_elt)
+            new_elt.find("geometry").remove(mesh_elt)
+            new_elt.find("geometry").append(new_mesh_elt)
+        elif replacement_strategy == MeshReplacementStrategy.kWithMinimalEnclosingCylinder:
+            new_elt = self.replace_element_with_enclosing_cylinder(new_elt)
+        else:
+            raise ValueError(
+                f"Invalid mesh replacement strategy: {replacement_strategy}"
+            )
+
+        return new_elt
+    
+    def convert_visual_element(self, visual_elt: ET.Element) -> ET.Element:
+        """
+        Description
+        -----------
+        This method will convert the collision element in the URDF file.
+
+        For now, this is very similar to the collision element handling, but it may
+        change in the future.
+
+        :param visual_elt: The visual element that we want to convert.
+        :return:
+        """
+        # Setup
+        new_elt = deepcopy(visual_elt)
+
+        # Algorithm
+        self.log(
+            f"Converting element with tag \"{visual_elt.tag}\"."
+        )
+
+        # Check to see if the collision element has a mesh element
+        mesh_elt = new_elt.find("geometry/mesh")
+        if mesh_elt is None:
+            self.log(
+                f"Collision element does not contain a mesh element. No modification needed..."
+            )
+            return new_elt
+
+        # If the mesh element exists, then we will try to convert it
+        new_mesh_elt = self.convert_mesh_element(mesh_elt)
+        new_elt.find("geometry").remove(mesh_elt)
+        new_elt.find("geometry").append(new_mesh_elt)
+        
+        return new_elt
+
     def convert_urdf(self) -> Path:
         """
         Description
@@ -199,18 +285,18 @@ def convert_tree(
         # Setup
         root = deepcopy(current_tree.getroot())
 
-        # Modify Root if it contains a mesh or file input
+        # Modify root, if necessary
         new_root = self.convert_tree_element(root)
 
-        # Remove all children from the new root
-        for child in new_root.findall("*"):
-            new_root.remove(child)
-
-        # Modify Children
-        for child in root:
+        # Modify Children of the root
+        initial_children = list(new_root)
+        for child in initial_children:
             new_child = self.convert_tree(
                 ET.ElementTree(child)
             )
+            new_root.remove(child)
+
+            # Add new child to new root
             new_root.append(new_child.getroot())
 
         return ET.ElementTree(new_root)
@@ -234,8 +320,10 @@ def convert_tree_element(
         new_elt = deepcopy(elt)
 
         # Convert file paths for mesh elements if necessary
-        if new_elt.tag == "mesh":
-            new_elt = self.convert_mesh_element(new_elt)
+        if new_elt.tag == "collision":
+            new_elt = self.convert_collision_element(new_elt)
+        elif new_elt.tag == "visual":
+            new_elt = self.convert_visual_element(new_elt)
         elif new_elt.tag == "transmission":
             # Ignore transmission elements during the conversion
             return new_elt
@@ -244,7 +332,7 @@ def convert_tree_element(
         else:
             # If the element is not a mesh element, then we will just return a copy of it.
             self.log(
-                f"Found an element with tag \"{elt.tag}\" that is not a mesh element. No modification needed..."
+                f"Found an element with tag \"{new_elt.tag}\" (originally \"{elt.tag}\") that is not a mesh element. No modification needed..."
             )
         # Otherwise, just return a copy of the previous element.
         return new_elt
@@ -275,7 +363,7 @@ def create_obj_to_replace_mesh_file(
         # Clip off all parts of path that include the exported output
         new_mesh_path = str(new_mesh_path).replace(str(self.output_file_directory()), "")
 
-        return "./" + str(new_mesh_path)
+        return "." + str(new_mesh_path)
 
 
     def handle_joint_element(self, joint_elt: ET.Element):
@@ -347,4 +435,87 @@ def output_urdf_file_name(self) -> str:
 
             return f"{original_name}.drake.urdf"
         else:
-            return Path(self.output_urdf_file_path).name
\ No newline at end of file
+            return Path(self.output_urdf_file_path).name
+
+    def replace_element_with_enclosing_cylinder(self, collision_elt: ET.Element) -> ET.Element:
+        """
+        Description
+        -----------
+        This method will replace the geometry element with an enclosing cylinder.
+        :param collision_elt: The target element (can be collision) that we want to replace.
+        :return:
+        """
+        # Setup
+        new_elt = deepcopy(collision_elt)
+        original_urdf_dir = Path(self.original_urdf_filename).parent
+
+        # Save the initial pose of the element
+        origin_elt = new_elt.find("origin")
+        pose0 = RigidTransform(
+            R=RollPitchYaw(np.array(
+                [float(elt) for elt in origin_elt.attrib["rpy"].split(" ")]
+            )).ToRotationMatrix(),
+            p=np.array(
+                [float(elt) for elt in origin_elt.attrib["xyz"].split(" ")],
+            ),
+        )
+
+        # Extract the mesh element and the filename that it references
+        mesh_elt = new_elt.find("geometry/mesh")
+        mesh_file_name = mesh_elt.attrib["filename"]
+
+        # Load the mesh from the mesh element
+        converter = MeshFileConverter(
+            mesh_file_path=mesh_file_name,
+            urdf_dir=Path(original_urdf_dir),
+            new_urdf_dir=self.output_file_directory(),
+        )
+        mesh = trimesh.load_mesh(
+            str(converter.urdf_dir / converter.true_mesh_file_path())
+        )
+
+        # Create the enclosing cylinder
+        cylinder_result = trimesh.bounds.minimum_cylinder(mesh, sample_count=12)
+        cylinder_shape = CylinderDefinition(
+            radius=cylinder_result['radius'],
+            length=cylinder_result['height'],
+        )
+
+        # Remove the mesh element and pose element from the collision element, if they exist
+        geometry_elt = new_elt.find("geometry")
+        if mesh_elt is not None:
+            new_elt.remove(geometry_elt)
+
+        origin_elt = new_elt.find("origin")
+        if origin_elt is not None:
+            new_elt.remove(origin_elt)
+
+        # Add the origin element to the collision element
+        transform = cylinder_result['transform']
+        transform_as_matrix = np.array(transform)
+        transform_as_rt = RigidTransform(
+            RotationMatrix(transform_as_matrix[:3, :3]),
+            transform_as_matrix[:3, 3],
+        )
+
+        pose_as_rt = pose0.multiply(transform_as_rt)
+
+        ET.SubElement(
+            new_elt,
+            "origin",
+            {
+                "xyz": f"{pose_as_rt.translation()[0]} {pose_as_rt.translation()[1]} {pose_as_rt.translation()[2]}",
+                "rpy": f"{pose_as_rt.rotation().ToRollPitchYaw().roll_angle()} {pose_as_rt.rotation().ToRollPitchYaw().pitch_angle()} {pose_as_rt.rotation().ToRollPitchYaw().yaw_angle()}",
+            },
+        )
+
+        # Create a new geometry element and add the cylinder element to it
+        new_elt.append(ET.Element("geometry"))
+        cylinder_shape.add_geometry_to_element(new_elt.find("geometry"))
+
+        return new_elt
+
+
+
+
+
diff --git a/src/brom_drake/file_manipulation/urdf/__init__.py b/src/brom_drake/file_manipulation/urdf/__init__.py
index 0efcf1b..91cfcd0 100644
--- a/src/brom_drake/file_manipulation/urdf/__init__.py
+++ b/src/brom_drake/file_manipulation/urdf/__init__.py
@@ -1,4 +1,4 @@
-from .DrakeReadyURDFConverter import DrakeReadyURDFConverter
+from .DrakeReadyURDFConverter import DrakeReadyURDFConverter, MeshReplacementStrategy
 from .drakeify import drakeify_my_urdf
 from .util import (
     tree_contains_transmission_for_joint,
@@ -7,6 +7,7 @@
 
 __all__ = [
     'DrakeReadyURDFConverter',
+    'MeshReplacementStrategy',
     'drakeify_my_urdf',
     'tree_contains_transmission_for_joint',
     'create_transmission_element_for_joint',
diff --git a/src/brom_drake/file_manipulation/urdf/drakeify.py b/src/brom_drake/file_manipulation/urdf/drakeify.py
index 93e87ab..6853ab5 100644
--- a/src/brom_drake/file_manipulation/urdf/drakeify.py
+++ b/src/brom_drake/file_manipulation/urdf/drakeify.py
@@ -10,12 +10,14 @@
 from pathlib import Path
 
 # Internal Imports
-from .DrakeReadyURDFConverter import DrakeReadyURDFConverter
+from .DrakeReadyURDFConverter import DrakeReadyURDFConverter, MeshReplacementStrategy
 
 def drakeify_my_urdf(
     urdf_file_path: Union[str, Path],
     overwrite_old_logs: bool = False,
+    overwrite_old_models: bool = False,
     log_file_name: str = "conversion.log",
+    collision_mesh_replacement_strategy: MeshReplacementStrategy = MeshReplacementStrategy.kWithObj,
 ) -> Path:
     """
     Description:
@@ -41,7 +43,9 @@ def drakeify_my_urdf(
     converter = DrakeReadyURDFConverter(
         urdf_file_path,
         overwrite_old_logs=overwrite_old_logs,
+        overwrite_old_models=overwrite_old_models,
         log_file_name=log_file_name,
+        collision_mesh_replacement_strategy=collision_mesh_replacement_strategy,
     )
 
     # Convert the URDF
diff --git a/src/brom_drake/file_manipulation/urdf/mesh_file_converter.py b/src/brom_drake/file_manipulation/urdf/mesh_file_converter.py
index d2638d0..2292f21 100644
--- a/src/brom_drake/file_manipulation/urdf/mesh_file_converter.py
+++ b/src/brom_drake/file_manipulation/urdf/mesh_file_converter.py
@@ -147,7 +147,8 @@ def find_package_directory_including_mesh(
         candidate_path = original_urdf_dir
         search_depth = 1
         while not package_found:
-            print(f"candidate_path: {candidate_path}")
+            # print(f"candidate_path: {candidate_path}")
+            
             # Check to see if "package.xml" exists in the directory
             if (candidate_path / "package.xml").exists():
                 self.log(f"Found package path at {candidate_path}.")
diff --git a/src/brom_drake/file_manipulation/urdf/simple_shape_urdfs/__init__.py b/src/brom_drake/file_manipulation/urdf/shapes/__init__.py
similarity index 100%
rename from src/brom_drake/file_manipulation/urdf/simple_shape_urdfs/__init__.py
rename to src/brom_drake/file_manipulation/urdf/shapes/__init__.py
diff --git a/src/brom_drake/file_manipulation/urdf/shapes/box.py b/src/brom_drake/file_manipulation/urdf/shapes/box.py
new file mode 100644
index 0000000..b806c28
--- /dev/null
+++ b/src/brom_drake/file_manipulation/urdf/shapes/box.py
@@ -0,0 +1,33 @@
+from dataclasses import dataclass
+import numpy as np
+from typing import Union
+import xml.etree.ElementTree as ET
+
+# Internal Import
+from .shape_definition import ShapeDefinition, ShapeEnum
+
+@dataclass
+class BoxDefinition(ShapeDefinition):
+    size: Union[float, list, tuple, np.ndarray]
+
+    def add_geometry_to_element(self, target_element: ET.Element):
+        # Setup
+
+        # Convert the size to a numpy array
+        if isinstance(self.size, (list, tuple, np.ndarray)):
+            self.size = np.array(self.size)
+        elif isinstance(self.size, float):
+            self.size = np.array([self.size, self.size, self.size])
+        else:
+            raise ValueError(f"Size {self.size} (type \"{type(self.size)}\") not supported.")
+
+        # Return the element
+        return ET.SubElement(
+            target_element,
+            "box",
+            {"size": f"{self.size[0]} {self.size[1]} {self.size[2]}"},
+        )
+
+    @property
+    def type(self):
+        return ShapeEnum.kBox
\ No newline at end of file
diff --git a/src/brom_drake/file_manipulation/urdf/shapes/cylinder.py b/src/brom_drake/file_manipulation/urdf/shapes/cylinder.py
new file mode 100644
index 0000000..e51d204
--- /dev/null
+++ b/src/brom_drake/file_manipulation/urdf/shapes/cylinder.py
@@ -0,0 +1,21 @@
+from dataclasses import dataclass
+import xml.etree.ElementTree as ET
+
+# Internal Import
+from .shape_definition import ShapeDefinition, ShapeEnum
+
+@dataclass
+class CylinderDefinition(ShapeDefinition):
+    radius: float
+    length: float
+
+    def add_geometry_to_element(self, target_element: ET.Element):
+        return ET.SubElement(
+            target_element,
+            "cylinder",
+            {"radius": f"{self.radius}", "length": f"{self.length}"},
+        )
+
+    @property
+    def type(self):
+        return ShapeEnum.kCylinder
\ No newline at end of file
diff --git a/src/brom_drake/file_manipulation/urdf/shapes/shape_definition.py b/src/brom_drake/file_manipulation/urdf/shapes/shape_definition.py
new file mode 100644
index 0000000..0e7300b
--- /dev/null
+++ b/src/brom_drake/file_manipulation/urdf/shapes/shape_definition.py
@@ -0,0 +1,25 @@
+from dataclasses import dataclass
+from enum import IntEnum
+import numpy as np
+from typing import Union
+import xml.etree.ElementTree as ET
+
+
+class ShapeEnum(IntEnum):
+    """
+    The different simple shapes that can be created.
+    """
+    kBox = 1
+    kSphere = 2
+    kCylinder = 3
+    kCapsule = 4
+
+# Shape Definition Can be any one of these
+@dataclass
+class ShapeDefinition:
+    def add_geometry_to_element(self, target_element: ET.Element):
+        raise NotImplementedError("This method must be implemented in the subclass.")
+
+    @property
+    def type(self):
+        raise NotImplementedError("This method must be implemented in the subclass.")
diff --git a/src/brom_drake/file_manipulation/urdf/shapes/sphere.py b/src/brom_drake/file_manipulation/urdf/shapes/sphere.py
new file mode 100644
index 0000000..69e933e
--- /dev/null
+++ b/src/brom_drake/file_manipulation/urdf/shapes/sphere.py
@@ -0,0 +1,20 @@
+from dataclasses import dataclass
+import xml.etree.ElementTree as ET
+
+# Internal Import
+from .shape_definition import ShapeDefinition, ShapeEnum
+
+@dataclass
+class SphereDefinition(ShapeDefinition):
+    radius: float
+
+    def add_geometry_to_element(self, target_element: ET.Element):
+        return ET.SubElement(
+            target_element,
+            "sphere",
+            {"radius": f"{self.radius}"},
+        )
+
+    @property
+    def type(self):
+        return ShapeEnum.kSphere
\ No newline at end of file
diff --git a/src/brom_drake/file_manipulation/urdf/simple_shape_urdfs/shape_definition.py b/src/brom_drake/file_manipulation/urdf/simple_shape_urdfs/shape_definition.py
deleted file mode 100644
index 721c6f7..0000000
--- a/src/brom_drake/file_manipulation/urdf/simple_shape_urdfs/shape_definition.py
+++ /dev/null
@@ -1,22 +0,0 @@
-from dataclasses import dataclass
-from enum import IntEnum
-from typing import Union
-
-
-class ShapeEnum(IntEnum):
-    """
-    The different simple shapes that can be created.
-    """
-    kBox = 1
-    kSphere = 2
-    kCylinder = 3
-    kCapsule = 4
-
-@dataclass
-class SphereDefinition:
-    radius: float
-    type: ShapeEnum = ShapeEnum.kSphere
-
-
-# Shape Definition Can be any one of these
-ShapeDefinition = Union[SphereDefinition]
\ No newline at end of file
diff --git a/src/brom_drake/file_manipulation/urdf/simple_writer/__init__.py b/src/brom_drake/file_manipulation/urdf/simple_writer/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/brom_drake/file_manipulation/urdf/simple_writer/inertia_definition.py b/src/brom_drake/file_manipulation/urdf/simple_writer/inertia_definition.py
new file mode 100644
index 0000000..1b55a09
--- /dev/null
+++ b/src/brom_drake/file_manipulation/urdf/simple_writer/inertia_definition.py
@@ -0,0 +1,34 @@
+from dataclasses import dataclass
+
+@dataclass
+class InertiaDefinition:
+    """
+    A dataclass that defines the inertia of a simple shape.
+    """
+    ixx: float = 1.0
+    ixy: float = 0.0
+    ixz: float = 0.0
+    iyy: float = 1.0
+    iyz: float = 0.0
+    izz: float = 1.0
+
+    def as_list(self) -> list:
+        """
+        Convert the inertia to a list.
+        :return: A list of the inertia values.
+        """
+        return [self.ixx, self.ixy, self.ixz, self.iyy, self.iyz, self.izz]
+
+    def as_map(self) -> dict:
+        """
+        Convert the inertia to a map.
+        :return: A map of the inertia values.
+        """
+        return {
+            "ixx": f"{self.ixx}",
+            "ixy": f"{self.ixy}",
+            "ixz": f"{self.ixz}",
+            "iyy": f"{self.iyy}",
+            "iyz": f"{self.iyz}",
+            "izz": f"{self.izz}",
+        }
\ No newline at end of file
diff --git a/src/brom_drake/file_manipulation/urdf/simple_shape_urdfs/urdf_definition.py b/src/brom_drake/file_manipulation/urdf/simple_writer/urdf_definition.py
similarity index 69%
rename from src/brom_drake/file_manipulation/urdf/simple_shape_urdfs/urdf_definition.py
rename to src/brom_drake/file_manipulation/urdf/simple_writer/urdf_definition.py
index 5380b72..ee83e18 100644
--- a/src/brom_drake/file_manipulation/urdf/simple_shape_urdfs/urdf_definition.py
+++ b/src/brom_drake/file_manipulation/urdf/simple_writer/urdf_definition.py
@@ -10,42 +10,11 @@
 import xml.etree.ElementTree as ET
 
 import numpy as np
+from pydrake.math import RigidTransform
 
 # Internal Imports
-from .shape_definition import ShapeEnum, SphereDefinition, ShapeDefinition
-
-@dataclass
-class InertiaDefinition:
-    """
-    A dataclass that defines the inertia of a simple shape.
-    """
-    ixx: float = 1.0
-    ixy: float = 0.0
-    ixz: float = 0.0
-    iyy: float = 1.0
-    iyz: float = 0.0
-    izz: float = 1.0
-
-    def as_list(self) -> list:
-        """
-        Convert the inertia to a list.
-        :return: A list of the inertia values.
-        """
-        return [self.ixx, self.ixy, self.ixz, self.iyy, self.iyz, self.izz]
-
-    def as_map(self) -> dict:
-        """
-        Convert the inertia to a map.
-        :return: A map of the inertia values.
-        """
-        return {
-            "ixx": f"{self.ixx}",
-            "ixy": f"{self.ixy}",
-            "ixz": f"{self.ixz}",
-            "iyy": f"{self.iyy}",
-            "iyz": f"{self.iyz}",
-            "izz": f"{self.izz}",
-        }
+from brom_drake.file_manipulation.urdf.shapes.shape_definition import ShapeEnum, ShapeDefinition
+from brom_drake.file_manipulation.urdf.simple_writer.inertia_definition import InertiaDefinition
 
 @dataclass
 class SimpleShapeURDFDefinition:
@@ -58,6 +27,7 @@ class SimpleShapeURDFDefinition:
     create_collision: bool = True # Whether to create the collision elements of the urdf
     mass: float = 1.0
     inertia: InertiaDefinition = None
+    pose: RigidTransform = RigidTransform()
 
     def as_urdf(self) -> ET.Element:
         """
@@ -93,11 +63,7 @@ def add_inertial_elements_to(self, link_elt: ET.Element):
             inertia_def = InertiaDefinition()
 
         # Create all child links for inertial elements
-        origin_elt = ET.SubElement(
-            inertial_link,
-            "origin",
-            {"xyz": "0 0 0", "rpy": "0 0 0"},
-        )
+        self.add_origin_element_to(inertial_link)
         mass = ET.SubElement(inertial_link, "mass", {"value": f"{self.mass}"})
         inertia = ET.SubElement(
             inertial_link,
@@ -121,18 +87,11 @@ def add_visual_elements_to(self, link_elt: ET.Element):
             color = np.array([0.0, 1.0, 0.0, 0.8])
 
         # Create all child links for visual elements
-        origin_elt = ET.SubElement(
-            visual_link,
-            "origin",
-            {"xyz": "0 0 0", "rpy": "0 0 0"},
-        )
+        self.add_origin_element_to(visual_link)
         geometry = ET.SubElement(visual_link, "geometry")
 
         # Add the shape to the geometry element
-        if self.shape.type == ShapeEnum.kSphere:
-            sphere = ET.SubElement(geometry, "sphere", {"radius": f"{self.shape.radius}"})
-        else:
-            raise ValueError(f"Shape {self.shape} not supported yet.")
+        shape = self.shape.add_geometry_to_element(geometry)
 
         # Add the material to the visual element
         material = ET.SubElement(visual_link, "material", {"name": "blue"})
@@ -142,9 +101,6 @@ def add_visual_elements_to(self, link_elt: ET.Element):
             {"rgba": f"{color[0]} {color[1]} {color[2]} {color[3]}"},
         )
 
-        # Add visual elements to link
-        link_elt.append(visual_link)
-
     def add_collision_elements_to(self, link_elt: ET.Element):
         """
         Add the collision elements to the link element.
@@ -153,22 +109,35 @@ def add_collision_elements_to(self, link_elt: ET.Element):
         """
         # Setup
         collision_link = ET.SubElement(link_elt, "collision")
-        origin_elt = ET.SubElement(
-            collision_link,
-            "origin",
-            {"xyz": "0 0 0", "rpy": "0 0 0"},
-        )
+
+        # Add pose to collision element
+        self.add_origin_element_to(collision_link)
         geometry = ET.SubElement(collision_link, "geometry")
 
         # Add the shape to the geometry element
-        if self.shape.type == ShapeEnum.kSphere:
-            sphere = ET.SubElement(geometry, "sphere", {"radius": f"{self.shape.radius}"})
-        else:
-            raise ValueError(f"Shape {self.shape} not supported yet.")
+        self.shape.add_geometry_to_element(geometry)
 
         # Add collision elements to link
         link_elt.append(collision_link)
 
+    def add_origin_element_to(self, target_element: ET.Element):
+        """
+        Add the origin element to the target element.
+        :param target_element: The element to which the origin element will be added.
+        :return: Nothing, but modifies the target_element in place.
+        """
+        # Setup
+        translation = self.pose.translation()
+        rot_as_rpy = self.pose.rotation().ToRollPitchYaw()
+        origin_elt = ET.SubElement(
+            target_element,
+            "origin",
+            {
+                "xyz": f"{translation[0]} {translation[1]} {translation[2]}",
+                "rpy": f"{rot_as_rpy.roll_angle()} {rot_as_rpy.pitch_angle()} {rot_as_rpy.yaw_angle()}",
+            },
+        )
+
     def write_to_file(self, file_path: str):
         """
         Write the URDF to a file.
@@ -180,6 +149,9 @@ def write_to_file(self, file_path: str):
         tree = ET.ElementTree(root)
         ET.indent(tree, space="\t", level=0)
 
+        # Create the directory if it doesn't exist
+        os.makedirs(os.path.dirname(file_path), exist_ok=True)
+
         # Write to file
         tree.write(file_path, xml_declaration=True)
 
diff --git a/src/brom_drake/motion_planning/algorithms/motion_planner.py b/src/brom_drake/motion_planning/algorithms/motion_planner.py
index 53e092b..c3e93d6 100644
--- a/src/brom_drake/motion_planning/algorithms/motion_planner.py
+++ b/src/brom_drake/motion_planning/algorithms/motion_planner.py
@@ -56,15 +56,15 @@ def check_collision_in_config(
         # Note: This method will be inaccurate if using arbitrary, nonconvex meshes!
         # https://drake.mit.edu/pydrake/pydrake.geometry.html?highlight=queryobject#pydrake.geometry.QueryObject.HasCollisions
         query_object = self.scene_graph.get_query_output_port().Eval(scene_graph_context)
-        return query_object.HasCollisions()
+        # return query_object.HasCollisions()
 
+        # Alternative method to check for collisions
+        closest_points = query_object.ComputeSignedDistancePairwiseClosestPoints()
+        eps0 = 1e-2
+        for pair in closest_points:
+            if pair.distance < eps0:
+                return True
+            
+        # Otherwise return false
+        return False
 
-
-
-    def plan_motion(self):
-        """
-        Description:
-            This function plans the motion for the robot.
-        """
-        # Motion planning logic goes here
-        pass
\ No newline at end of file
diff --git a/src/brom_drake/motion_planning/algorithms/rrt/base.py b/src/brom_drake/motion_planning/algorithms/rrt/base.py
index 73fc128..47af9c5 100644
--- a/src/brom_drake/motion_planning/algorithms/rrt/base.py
+++ b/src/brom_drake/motion_planning/algorithms/rrt/base.py
@@ -14,8 +14,8 @@
 
 @dataclass
 class BaseRRTPlannerConfig:
-    steering_step_size: float = 0.1
-    prob_sample_goal: float = 0.075
+    steering_step_size: float = 0.05
+    prob_sample_goal: float = 0.25
     max_iterations: int = int(1e4)
     convergence_threshold: float = 1e-3
 
@@ -153,6 +153,12 @@ def plan(
             # Check if we have reached the goal
             if np.linalg.norm(q_new - q_goal) < self.config.convergence_threshold:
                 print(f"Goal reached after {iteration} iterations!")
+                print(f"Check all points in path: {rrt.nodes}")
+                for node in rrt.nodes:
+                    print(f"Node {node}: {rrt.nodes[node]}")
+                    collision_check_value = collision_check_fcn(rrt.nodes[node]['q'])
+                    if collision_check_value:
+                        print(f"Collision check node: {collision_check_fcn(rrt.nodes[node]['q'])}")
                 return rrt, True
 
         # If we exit the loop without finding a path to the goal,
diff --git a/src/brom_drake/motion_planning/algorithms/rrt/connect.py b/src/brom_drake/motion_planning/algorithms/rrt/connect.py
new file mode 100644
index 0000000..4deca9a
--- /dev/null
+++ b/src/brom_drake/motion_planning/algorithms/rrt/connect.py
@@ -0,0 +1,238 @@
+"""
+connect.py
+"""
+from dataclasses import dataclass
+from typing import Tuple, Callable
+
+import networkx as nx
+import numpy as np
+from pydrake.geometry import SceneGraph
+from pydrake.multibody.plant import MultibodyPlant
+from pydrake.multibody.tree import ModelInstanceIndex
+# Internal Imports
+from brom_drake.motion_planning.algorithms.motion_planner import MotionPlanner
+
+@dataclass
+class RRTConnectPlannerConfig:
+    steering_step_size: float = 0.025
+    prob_sample_goal: float = 0.05
+    max_iterations: int = int(1e4)
+    convergence_threshold: float = 1e-3
+
+class RRTConnectPlanner(MotionPlanner):
+    def __init__(
+        self,
+        robot_model_idx: ModelInstanceIndex,
+        plant: MultibodyPlant,
+        scene_graph: SceneGraph,
+        config: RRTConnectPlannerConfig = None,
+    ):
+        super().__init__(robot_model_idx, plant, scene_graph)
+        # Input Processing
+
+        # Setup
+        # self.dim_q = plant.num_actuated_dofs(robot_model_idx)
+
+        self.config = config
+        if self.config is None:
+            self.config = RRTConnectPlannerConfig()
+
+        # Prepare for planning
+        # self.joint_limits = self.get_joint_limits()  # Initialize joint limits array
+
+    def connect(
+        self,
+        nearest_node_idx: int,
+        q_target: np.ndarray,
+        rrt: nx.DiGraph,
+        collision_check_fcn: Callable[[np.ndarray], bool] = None,
+    ) -> np.ndarray:
+        # Input Processing
+        if collision_check_fcn is None:
+            collision_check_fcn = self.check_collision_in_config
+
+        # Setup
+        nearest_node = rrt.nodes[nearest_node_idx]
+
+        # Steer from nearest node to random configuration
+        q_current = nearest_node['q']
+        last_node_idx = nearest_node_idx
+
+        n_loops = 0
+        while not np.isclose(q_current, q_target).all():
+            # Create next node
+            q_new = self.steer(q_current, q_target)
+
+            # Check for collisions
+            if collision_check_fcn(q_new):
+                break
+
+            # If the next point is collision free, then add new node to the tree
+            rrt.add_node(rrt.number_of_nodes(), q=q_new)
+            rrt.add_edge(last_node_idx, rrt.number_of_nodes()-1)
+
+            # Prepare for next iteration
+            last_node_idx = rrt.number_of_nodes()-1
+            q_current = q_new
+
+            n_loops += 1
+
+        # print(f"Number of loops in connection: {n_loops}")
+        # print(f"RRT Size: {rrt.number_of_nodes()}")
+
+        return q_current # Return the last configuration we visited
+
+
+    @property
+    def dim_q(self) -> int:
+        if not self.plant.is_finalized():
+            raise ValueError("Plant has not been finalized yet! Can not compute num_actuated_dofs().")
+
+        return self.plant.num_actuated_dofs(self.robot_model_idx)
+
+    @property
+    def joint_limits(self) -> np.ndarray:
+        """
+        Description:
+            This function retrieves the joint limits of the robot.
+        """
+        # Setup
+        joint_indicies = self.plant.GetJointIndices(self.robot_model_idx)
+
+        joint_limits = np.zeros((0, 2))
+        for ii, joint_index in enumerate(joint_indicies):
+            # Check to see if joint is actuated; if not, then ignore
+            joint_ii = self.plant.get_joint(joint_index)
+            joint_can_move = joint_ii.can_rotate() or joint_ii.can_translate()
+            if not joint_can_move:
+                continue
+
+            lower_limits = self.plant.get_joint(joint_index).position_lower_limits()
+            if len(lower_limits) == 0:
+                lower_limits = [-np.inf]
+
+            upper_limits = self.plant.get_joint(joint_index).position_upper_limits()
+            if len(upper_limits) == 0:
+                upper_limits = [np.inf]
+
+            # Append limits to the joint_limits array
+            joint_limits = np.vstack((
+                joint_limits,
+                [lower_limits[0], upper_limits[0]]
+            ))
+
+        return joint_limits
+
+    def find_nearest_node(
+        self,
+        rrt: nx.DiGraph,
+        q_random: np.ndarray
+    ) -> Tuple[nx.classes.reportviews.NodeView, int]:
+        """
+        Description:
+            This function finds the nearest node in the RRT to the random configuration.
+        """
+        nearest_node = None
+        min_distance = float('inf')
+
+        for node in rrt.nodes:
+            distance = np.linalg.norm(rrt.nodes[node]['q'] - q_random)
+            if distance < min_distance:
+                min_distance = distance
+                nearest_node_idx = node
+                nearest_node = rrt.nodes[node]
+
+        return nearest_node, nearest_node_idx
+
+    def plan(
+        self,
+        q_start: np.ndarray,
+        q_goal: np.ndarray,
+        collision_check_fcn: Callable[[np.ndarray], bool] = None,
+    ) -> Tuple[nx.DiGraph, bool]:
+        """
+        Description:
+            This function executes the RRT planning algorithm.
+        """
+        # Input Processing
+        q_start = q_start.flatten()
+        q_goal = q_goal.flatten()
+        if q_start.shape[0] != self.dim_q or q_goal.shape[0] != self.dim_q:
+            raise ValueError(
+                f"Start configuration shape ({q_start.shape}) and goal configuration " +
+                f"shape ({q_goal.shape}) must match the dimension of the robot ({self.dim_q})."
+            )
+
+        if collision_check_fcn is None:
+            collision_check_fcn =  self.check_collision_in_config
+
+        # Setup
+        rrt = nx.DiGraph()
+        rrt.add_node(0, q=q_start) # Make this graph contain all configurations
+        prob_sample_goal = self.config.prob_sample_goal
+
+        # RRT Planner
+        for iteration in range(self.config.max_iterations):
+            # Sample random configuration OR goal
+            if np.random.rand() < prob_sample_goal:
+                q_random = q_goal.copy()
+            else:
+                q_random = self.sample_random_configuration()
+
+            # Find nearest node in the tree
+            nearest_node, nearest_node_idx = self.find_nearest_node(rrt, q_random)
+
+            q_new = self.connect(nearest_node_idx, q_random, rrt, collision_check_fcn)
+
+            # Check if we have reached the goal
+            if np.linalg.norm(q_new - q_goal) < self.config.convergence_threshold:
+                print(f"Goal reached after {iteration} iterations of RRT-Connect!")
+                # print(f"Check all points in path: {rrt.nodes}")
+                # for node in rrt.nodes:
+                #     print(f"Node {node}: {rrt.nodes[node]}")
+                #     collision_check_value = collision_check_fcn(rrt.nodes[node]['q'])
+                #     if collision_check_value:
+                #         print(f"Collision check node: {collision_check_fcn(rrt.nodes[node]['q'])}")
+                return rrt, True
+
+        # If we exit the loop without finding a path to the goal,
+        # return the RRT and indicate failure
+        print("Max iterations reached without finding a path to the goal.")
+        return rrt, False
+
+
+
+    def sample_random_configuration(self) -> np.ndarray:
+        """
+        Description:
+            This function samples a random configuration within the joint limits.
+        """
+        return np.random.uniform(
+            self.joint_limits[:, 0],
+            self.joint_limits[:, 1]
+        )
+
+    def steer(
+        self,
+        nearest_node,
+        q_random: np.ndarray
+    ) -> np.ndarray:
+        """
+        Description:
+            This function steers from the nearest node towards the random configuration.
+        """
+        # Setup
+        step_size = self.config.steering_step_size
+        q_nearest = nearest_node
+
+        # Calculate the direction vector
+        direction = q_random - q_nearest
+        distance = np.linalg.norm(direction)
+
+        # Either:
+        # 1. Move a full step towards the random configuration
+        # 2. Or, if the distance is less than the step size, return the random configuration
+        if distance < step_size:
+            return q_random
+
+        return q_nearest + step_size * (direction / distance)
diff --git a/src/brom_drake/motion_planning/systems/prototypical_planner.py b/src/brom_drake/motion_planning/systems/prototypical_planner.py
index eeec059..b29c369 100644
--- a/src/brom_drake/motion_planning/systems/prototypical_planner.py
+++ b/src/brom_drake/motion_planning/systems/prototypical_planner.py
@@ -41,10 +41,8 @@ def __init__(
         # Define Input Ports
         self.root_context = None  # NOTE: This should be assigned by the user before calling!
         self.plan = None
-        self.robot_model_idx = None  # Will be provided by input port
         self.create_input_ports()
 
-
         # Define Output Ports
         self.create_output_ports()
 
@@ -76,7 +74,17 @@ def check_collision_in_config(
 
         # Check for collisions using the query object
         query_object = self.scene_graph.get_query_output_port().Eval(scene_graph_context)
-        return query_object.HasCollisions()
+        # return query_object.HasCollisions()
+
+        # Alternative method to check for collisions
+        closest_points = query_object.ComputePointPairPenetration()
+        eps0 = 1e-2
+        for pair in closest_points:
+            if pair.depth > eps0:
+                return True
+            
+        # Otherwise return false
+        return False
 
     def compute_plan_if_not_available(self, context, output: AbstractValue):
         """
@@ -86,19 +94,10 @@ def compute_plan_if_not_available(self, context, output: AbstractValue):
         # Print
         if self.plan is None:
             # Compute plan
-            p_WStart_vec = self.GetInputPort("start_pose").Eval(context)
-            p_WGoal_vec = self.GetInputPort("goal_pose").Eval(context)
-            self.robot_model_idx = self.GetInputPort("robot_model_index").EvalAbstract(context).get_value()
-
-            print(self.robot_model_idx)
-
-            q_start = self.solve_pose_ik_problem(
-                p_WStart_vec,
-            )
-
-            q_goal = self.solve_pose_ik_problem(
-                p_WGoal_vec,
-            )
+            q_start = self.GetInputPort("start_configuration").Eval(context)
+            q_goal = self.GetInputPort("goal_configuration").Eval(context)
+            if self.robot_model_idx is None:
+                self.robot_model_idx = self.GetInputPort("robot_model_index").EvalAbstract(context).get_value()
 
             if self.root_context is None:
                 raise ValueError("Plant context is not initialized yet!")
@@ -127,13 +126,19 @@ def create_input_ports(self):
         Description:
             This function creates the input ports for the RRT planner.
         """
+        # Setup
+        N = len(
+            self.plant.GetActuatedJointIndices(self.robot_model_idx)
+        )
+
+        # Define outputs
         self.DeclareVectorInputPort(
-            "start_pose",
-            BasicVector(np.zeros((7,))),
+            "start_configuration",
+            BasicVector(np.zeros((N,))),
         )
         self.DeclareVectorInputPort(
-            "goal_pose",
-            BasicVector(np.zeros((7,))),
+            "goal_configuration",
+            BasicVector(np.zeros((N,))),
         )
         self.DeclareAbstractInputPort(
             "robot_model_index",
@@ -200,39 +205,6 @@ def define_pose_ik_problem(
 
         return ik_problem
 
-    def solve_pose_ik_problem(
-        self,
-        input_pose_vec: np.ndarray
-    ) -> np.ndarray:
-        """
-        Description:
-        :param input_pose_vec:
-        :return:
-        """
-        # Setup
-
-        # Define Problem
-        ik_problem = self.define_pose_ik_problem(input_pose_vec)
-
-        # Solve problem
-        ik_program = ik_problem.prog()
-        ik_result = Solve(ik_program)
-
-        assert ik_result.get_solution_result() == SolutionResult.kSolutionFound, \
-            f"Solution result was {ik_result.get_solution_result()}; need SolutionResult.kSolutionFound to make RRT Plan!"
-
-        q_solution = ik_result.get_x_val()
-
-        # Extract only the positions that correspond to our robot's joints
-        robot_joint_names = self.plant.GetPositionNames(self.robot_model_idx, add_model_instance_prefix=True)
-        all_joint_names = self.plant.GetPositionNames()
-        q_out_list = []
-        for ii, joint_name in enumerate(all_joint_names):
-            if joint_name in robot_joint_names:
-                q_out_list.append(q_solution[ii])
-
-        return np.array(q_out_list)
-
     @property
     def n_actuated_dof(self) -> int:
         """
diff --git a/src/brom_drake/motion_planning/systems/rrt_plan_generator.py b/src/brom_drake/motion_planning/systems/rrt_plan_generator.py
index b1092b5..71c9b0e 100644
--- a/src/brom_drake/motion_planning/systems/rrt_plan_generator.py
+++ b/src/brom_drake/motion_planning/systems/rrt_plan_generator.py
@@ -32,7 +32,8 @@ def __init__(
         plant: MultibodyPlant,
         scene_graph: SceneGraph,
         robot_model_idx: ModelInstanceIndex = None,
-        rrt_config: BaseRRTPlannerConfig = None
+        rrt_config: BaseRRTPlannerConfig = None,
+        dim_config: int = None,
     ):
         """
         Description:
@@ -47,6 +48,8 @@ def __init__(
 
         # Compute dof using robot_model_idx
         self.dim_q = None
+        if dim_config is not None:
+            self.set_dimension(dim_config)
 
         # Set Up Input and Output Ports
         self.root_context = None # NOTE: This should be assigned by the user before calling!
@@ -61,19 +64,12 @@ def compute_plan_if_not_available(self, context, output: AbstractValue):
         # Print
         if self.plan is None:
             # Compute plan
-            p_WStart_vec = self.GetInputPort("start_pose").Eval(context)
-            p_WGoal_vec = self.GetInputPort("goal_pose").Eval(context)
+            q_start = self.GetInputPort("start_configuration").Eval(context)
+            q_goal = self.GetInputPort("goal_configuration").Eval(context)
             self.robot_model_idx = self.GetInputPort("robot_model_index").EvalAbstract(context).get_value()
 
-            print(self.robot_model_idx)
-
-            q_start = self.solve_pose_ik_problem(
-                p_WStart_vec,
-            )
-
-            q_goal = self.solve_pose_ik_problem(
-                p_WGoal_vec,
-            )
+            assert self.dim_q == self.n_actuated_dof, \
+                f"Dimension of configuration space ({self.dim_q}) does not match the number of actuated DOF ({self.n_actuated_dof})!"
 
             base_rrt = BaseRRTPlanner(
                 self.robot_model_idx,
@@ -107,12 +103,12 @@ def create_input_ports(self):
             This function creates the input ports for the RRT planner.
         """
         self.DeclareVectorInputPort(
-            "start_pose",
-            BasicVector(np.zeros((7,))),
+            "start_configuration",
+            BasicVector(np.zeros((self.dim_q,))),
         )
         self.DeclareVectorInputPort(
-            "goal_pose",
-            BasicVector(np.zeros((7,))),
+            "goal_configuration",
+            BasicVector(np.zeros((self.dim_q,))),
         )
         self.DeclareAbstractInputPort(
             "robot_model_index",
@@ -127,7 +123,7 @@ def create_output_ports(self):
         # Setup
 
         # Create output for plan
-        sample_plan = np.zeros((0, self.n_actuated_dof))
+        sample_plan = np.zeros((0, self.dim_q))
         self.DeclareAbstractOutputPort(
             "motion_plan",
             lambda: AbstractValue.Make(sample_plan),
diff --git a/src/brom_drake/robots/models/beaker/beaker.stl b/src/brom_drake/robots/models/beaker/beaker.stl
new file mode 100644
index 0000000..c4a6b38
Binary files /dev/null and b/src/brom_drake/robots/models/beaker/beaker.stl differ
diff --git a/src/brom_drake/robots/models/beaker/beaker.urdf b/src/brom_drake/robots/models/beaker/beaker.urdf
new file mode 100644
index 0000000..9cb5457
--- /dev/null
+++ b/src/brom_drake/robots/models/beaker/beaker.urdf
@@ -0,0 +1,18 @@
+<robot name="beaker">
+    <link name="beaker_base_link">
+        <visual>
+            <geometry>
+                <mesh filename="beaker.stl" scale="0.01 0.01 0.01"/>
+            </geometry>
+            <material name="">
+                <color rgba="0.0 0.1 0.5 0.4"/>
+            </material>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="beaker.stl" scale="0.01 0.01 0.01"/>
+            </geometry>
+        </collision>
+    </link>
+
+</robot>
diff --git a/src/brom_drake/robots/models/beaker/readme.md b/src/brom_drake/robots/models/beaker/readme.md
new file mode 100644
index 0000000..a8bf5fa
--- /dev/null
+++ b/src/brom_drake/robots/models/beaker/readme.md
@@ -0,0 +1,3 @@
+# Beaker
+
+Beaker by Saeed Kazemi on Grabcad: https://grabcad.com/library/beaker
\ No newline at end of file
diff --git a/src/brom_drake/robots/models/centrifuge/README.txt b/src/brom_drake/robots/models/centrifuge/README.txt
new file mode 100644
index 0000000..0646ff3
--- /dev/null
+++ b/src/brom_drake/robots/models/centrifuge/README.txt
@@ -0,0 +1,8 @@
+# Model of a Centrifuge
+
+This model is a 3D printable model of a centrifuge. The model is designed to be printed in PLA plastic.
+We may approximate its complicated shape for the purpose of simulation.
+
+## Source of Files
+
+F.Lab's DIYbio Centrifuge by F_Lab_TH on Thingiverse: https://www.thingiverse.com/thing:1175393
\ No newline at end of file
diff --git a/src/brom_drake/robots/models/centrifuge/meshes/V2_Body.stl b/src/brom_drake/robots/models/centrifuge/meshes/V2_Body.stl
new file mode 100644
index 0000000..b51ae6d
Binary files /dev/null and b/src/brom_drake/robots/models/centrifuge/meshes/V2_Body.stl differ
diff --git a/src/brom_drake/robots/models/centrifuge/meshes/V2_Cover.stl b/src/brom_drake/robots/models/centrifuge/meshes/V2_Cover.stl
new file mode 100644
index 0000000..4cf4522
Binary files /dev/null and b/src/brom_drake/robots/models/centrifuge/meshes/V2_Cover.stl differ
diff --git a/src/brom_drake/robots/models/centrifuge/meshes/V2_Feet.stl b/src/brom_drake/robots/models/centrifuge/meshes/V2_Feet.stl
new file mode 100644
index 0000000..0500511
Binary files /dev/null and b/src/brom_drake/robots/models/centrifuge/meshes/V2_Feet.stl differ
diff --git a/src/brom_drake/robots/models/centrifuge/meshes/V2_MotorMount.stl b/src/brom_drake/robots/models/centrifuge/meshes/V2_MotorMount.stl
new file mode 100644
index 0000000..9356ac3
Binary files /dev/null and b/src/brom_drake/robots/models/centrifuge/meshes/V2_MotorMount.stl differ
diff --git a/src/brom_drake/robots/models/centrifuge/meshes/V2_Rotor.stl b/src/brom_drake/robots/models/centrifuge/meshes/V2_Rotor.stl
new file mode 100644
index 0000000..77f730f
Binary files /dev/null and b/src/brom_drake/robots/models/centrifuge/meshes/V2_Rotor.stl differ
diff --git a/src/brom_drake/robots/models/centrifuge/meshes/V2_case_Cover.stl b/src/brom_drake/robots/models/centrifuge/meshes/V2_case_Cover.stl
new file mode 100644
index 0000000..fdb8d60
Binary files /dev/null and b/src/brom_drake/robots/models/centrifuge/meshes/V2_case_Cover.stl differ
diff --git a/src/brom_drake/robots/models/centrifuge/meshes/V2_knob.stl b/src/brom_drake/robots/models/centrifuge/meshes/V2_knob.stl
new file mode 100644
index 0000000..f7f782c
Binary files /dev/null and b/src/brom_drake/robots/models/centrifuge/meshes/V2_knob.stl differ
diff --git a/src/brom_drake/robots/models/centrifuge/partially-built.urdf b/src/brom_drake/robots/models/centrifuge/partially-built.urdf
new file mode 100644
index 0000000..e7a1de2
--- /dev/null
+++ b/src/brom_drake/robots/models/centrifuge/partially-built.urdf
@@ -0,0 +1,409 @@
+<robot name="centrifuge">
+    <link name="base_footprint">
+        <visual>
+            <geometry>
+                <mesh filename="meshes/V2_Body.stl"/>
+            </geometry>
+        </visual>
+        <inertial>
+            <origin
+                xyz="0 0 0"
+                rpy="0 0 0" />
+            <mass value="0.1" />
+            <inertia
+                ixx="1"
+                ixy="0"
+                ixz="0"
+                iyy="1"
+                iyz="0"
+                izz="1" />
+        </inertial>
+    </link>
+
+    <link name="bookcase">
+        <visual>
+            <geometry>
+                <mesh filename="bookcase.stl"/>
+            </geometry>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="bookcase.stl"/>
+            </geometry>
+        </collision>
+    </link>
+
+    # Copied from wheels.urdf.
+    <link name="r_wheel_link">
+        <collision>
+          <origin xyz="-0.0 -0.0025 0.0" />
+          <geometry>
+            <box size="0.04 0.005 0.48" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="-0.0 -0.0025 0.0" />
+          <material name="c">
+            <color rgba="0.7 0.7 0.7 1.0" />
+          </material>
+          <geometry>
+            <box size="0.04 0.005 0.48" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 0.015 0.19625" />
+          <geometry>
+            <box size="0.02 0.03 0.0025" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 0.015 0.19625" />
+          <material name="c">
+            <color rgba="0.7 0.7 0.7 1.0" />
+          </material>
+          <geometry>
+            <box size="0.02 0.03 0.0025" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 0.015 -0.19625" />
+          <geometry>
+            <box size="0.02 0.03 0.0025" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 0.015 -0.19625" />
+          <material name="c">
+            <color rgba="0.7 0.7 0.7 1.0" />
+          </material>
+          <geometry>
+            <box size="0.02 0.03 0.0025" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 -0.0125 0.22" />
+          <geometry>
+            <cylinder length="0.015" radius="0.02" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 -0.0125 0.22" />
+          <material name="c">
+            <color rgba="0.7 0.7 0.7 1.0" />
+          </material>
+          <geometry>
+            <cylinder length="0.015" radius="0.02" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 -0.0125 -0.22" />
+          <geometry>
+            <cylinder length="0.015" radius="0.02" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 -0.0125 -0.22" />
+          <material name="c">
+            <color rgba="0.7 0.7 0.7 1.0" />
+          </material>
+          <geometry>
+            <cylinder length="0.015" radius="0.02" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 -0.075 -0.24" />
+          <geometry>
+            <cylinder length="0.026" radius="0.035" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 -0.075 -0.24" />
+          <material name="c2">
+            <color rgba="1.0 1.0 1.0 1.0" />
+          </material>
+          <geometry>
+            <cylinder length="0.026" radius="0.035" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 -0.075 -0.24" />
+          <geometry>
+            <cylinder length="0.025" radius="0.0375" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 -0.075 -0.24" />
+          <material name="c3">
+            <color rgba="0.5 0.5 0.5 1.0" />
+          </material>
+          <geometry>
+            <cylinder length="0.025" radius="0.0375" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 -0.045 -0.22" />
+          <geometry>
+            <box size="0.03 0.05 0.05" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 -0.045 -0.22" />
+          <material name="c">
+            <color rgba="0.7 0.7 0.7 1.0" />
+          </material>
+          <geometry>
+            <box size="0.03 0.05 0.05" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 -0.075 0.24" />
+          <geometry>
+            <cylinder length="0.026" radius="0.035" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 -0.075 0.24" />
+          <material name="c2">
+            <color rgba="1.0 1.0 1.0 1.0" />
+          </material>
+          <geometry>
+            <cylinder length="0.026" radius="0.035" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 -0.075 0.24" />
+          <geometry>
+            <cylinder length="0.025" radius="0.0375" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 -0.075 0.24" />
+          <material name="c3">
+            <color rgba="0.5 0.5 0.5 1.0" />
+          </material>
+          <geometry>
+            <cylinder length="0.025" radius="0.0375" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 -0.045 0.22" />
+          <geometry>
+            <box size="0.03 0.05 0.05" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 -0.045 0.22" />
+          <material name="c">
+            <color rgba="0.7 0.7 0.7 1.0" />
+          </material>
+          <geometry>
+            <box size="0.03 0.05 0.05" />
+          </geometry>
+        </visual>
+    </link>
+
+    # Copied from wheels.urdf.
+    <link name="l_wheel_link">
+        <collision>
+          <origin xyz="-0.0 -0.0025 0.0" />
+          <geometry>
+            <box size="0.04 0.005 0.48" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="-0.0 -0.0025 0.0" />
+          <material name="c">
+            <color rgba="0.7 0.7 0.7 1.0" />
+          </material>
+          <geometry>
+            <box size="0.04 0.005 0.48" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 0.015 0.19625" />
+          <geometry>
+            <box size="0.02 0.03 0.0025" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 0.015 0.19625" />
+          <material name="c">
+            <color rgba="0.7 0.7 0.7 1.0" />
+          </material>
+          <geometry>
+            <box size="0.02 0.03 0.0025" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 0.015 -0.19625" />
+          <geometry>
+            <box size="0.02 0.03 0.0025" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 0.015 -0.19625" />
+          <material name="c">
+            <color rgba="0.7 0.7 0.7 1.0" />
+          </material>
+          <geometry>
+            <box size="0.02 0.03 0.0025" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 -0.0125 0.22" />
+          <geometry>
+            <cylinder length="0.015" radius="0.02" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 -0.0125 0.22" />
+          <material name="c">
+            <color rgba="0.7 0.7 0.7 1.0" />
+          </material>
+          <geometry>
+            <cylinder length="0.015" radius="0.02" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 -0.0125 -0.22" />
+          <geometry>
+            <cylinder length="0.015" radius="0.02" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 -0.0125 -0.22" />
+          <material name="c">
+            <color rgba="0.7 0.7 0.7 1.0" />
+          </material>
+          <geometry>
+            <cylinder length="0.015" radius="0.02" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 -0.075 -0.24" />
+          <geometry>
+            <cylinder length="0.026" radius="0.035" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 -0.075 -0.24" />
+          <material name="c2">
+            <color rgba="1.0 1.0 1.0 1.0" />
+          </material>
+          <geometry>
+            <cylinder length="0.026" radius="0.035" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 -0.075 -0.24" />
+          <geometry>
+            <cylinder length="0.025" radius="0.0375" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 -0.075 -0.24" />
+          <material name="c3">
+            <color rgba="0.5 0.5 0.5 1.0" />
+          </material>
+          <geometry>
+            <cylinder length="0.025" radius="0.0375" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 -0.045 -0.22" />
+          <geometry>
+            <box size="0.03 0.05 0.05" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 -0.045 -0.22" />
+          <material name="c">
+            <color rgba="0.7 0.7 0.7 1.0" />
+          </material>
+          <geometry>
+            <box size="0.03 0.05 0.05" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 -0.075 0.24" />
+          <geometry>
+            <cylinder length="0.026" radius="0.035" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 -0.075 0.24" />
+          <material name="c2">
+            <color rgba="1.0 1.0 1.0 1.0" />
+          </material>
+          <geometry>
+            <cylinder length="0.026" radius="0.035" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 -0.075 0.24" />
+          <geometry>
+            <cylinder length="0.025" radius="0.0375" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 -0.075 0.24" />
+          <material name="c3">
+            <color rgba="0.5 0.5 0.5 1.0" />
+          </material>
+          <geometry>
+            <cylinder length="0.025" radius="0.0375" />
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0.0 -0.045 0.22" />
+          <geometry>
+            <box size="0.03 0.05 0.05" />
+          </geometry>
+        </collision>
+        <visual>
+          <origin xyz="0.0 -0.045 0.22" />
+          <material name="c">
+            <color rgba="0.7 0.7 0.7 1.0" />
+          </material>
+          <geometry>
+            <box size="0.03 0.05 0.05" />
+          </geometry>
+        </visual>
+    </link>
+
+    <joint
+        name="base_to_l_wheel"
+        type="fixed">
+      <origin
+          xyz="0.738 0.2 0.1125"
+          rpy="1.57 0 0"/>
+      <parent
+          link="base_footprint" />
+      <child
+          link="l_wheel_link" />
+    </joint>
+
+    <joint
+        name="base_to_bookcase"
+        type="fixed">
+      <origin
+          xyz="0.0 0.0 0.1125" />
+      <parent
+          link="base_footprint" />
+      <child
+          link="bookcase" />
+    </joint>
+
+    <joint
+        name="base_to_r_wheel"
+        type="fixed">
+      <origin
+          xyz="0 0.2 0.1125"
+          rpy="1.57 0 0" />
+      <parent
+          link="base_footprint" />
+      <child
+          link="r_wheel_link" />
+    </joint>
+
+</robot>
diff --git a/src/brom_drake/robots/models/cupboard/cupboard.sdf b/src/brom_drake/robots/models/cupboard/cupboard.sdf
index 8fe4f75..2970640 100644
--- a/src/brom_drake/robots/models/cupboard/cupboard.sdf
+++ b/src/brom_drake/robots/models/cupboard/cupboard.sdf
@@ -3,10 +3,10 @@
   <model name="cupboard">
     <link name="cupboard_body">
       <visual name="right_wall">
-        <pose> 0 0.292 0 0 0 0</pose>
+        <pose> 0 0.35 0 0 0 0</pose>
         <geometry>
           <box>
-            <size>0.3 0.016 0.783</size>
+            <size>0.3 0.016 1.205</size>
           </box>
         </geometry>
         <material>
@@ -14,10 +14,10 @@
         </material>
       </visual>
       <visual name="left_wall">
-        <pose> 0 -0.292 0 0 0 0</pose>
+        <pose> 0 -0.35 0 0 0 0</pose>
         <geometry>
           <box>
-            <size>0.3 0.016 0.783</size>
+            <size>0.3 0.016 1.205</size>
           </box>
         </geometry>
         <material>
@@ -25,28 +25,28 @@
         </material>
       </visual>
       <collision name="right_wall">
-        <pose> 0 0.292 0 0 0 0</pose>
+        <pose> 0 0.35 0 0 0 0</pose>
         <geometry>
           <box>
-            <size>0.3 0.016 0.783</size>
+            <size>0.3 0.016 1.205</size>
           </box>
         </geometry>
       </collision>
       <collision name="left_wall">
-        <pose> 0 -0.292 0 0 0 0</pose>
+        <pose> 0 -0.35 0 0 0 0</pose>
         <geometry>
           <box>
-            <size>0.3 0.016 0.783</size>
+            <size>0.3 0.016 1.205</size>
           </box>
         </geometry>
       </collision>
     </link>
     <link name="top_and_bottom">
       <visual name="bottom">
-        <pose> 0 0 -0.3995 0 0 0</pose>
+        <pose> 0 0 -0.6005 0 0 0</pose>
         <geometry>
           <box>
-            <size>0.3 0.6 0.016</size>
+            <size>0.3 0.7 0.016</size>
           </box>
         </geometry>
         <material>
@@ -54,10 +54,10 @@
         </material>
       </visual>
       <visual name="top">
-        <pose> 0 0 0.3995 0 0 0</pose>
+        <pose> 0 0 0.6005 0 0 0</pose>
         <geometry>
           <box>
-            <size>0.3 0.6 0.016</size>
+            <size>0.3 0.7 0.016</size>
           </box>
         </geometry>
         <material>
@@ -65,10 +65,10 @@
         </material>
       </visual>
       <visual name="shelf_lower">
-        <pose> 0 0 -0.13115 0 0 0</pose>
+        <pose> 0 0 -0.23115 0 0 0</pose>
         <geometry>
           <box>
-            <size>0.3 0.6 0.016</size>
+            <size>0.3 0.7 0.016</size>
           </box>
         </geometry>
         <material>
@@ -76,10 +76,10 @@
         </material>
       </visual>
       <visual name="shelf_upper">
-        <pose> 0 0 0.13115 0 0 0</pose>
+        <pose> 0 0 0.23115 0 0 0</pose>
         <geometry>
           <box>
-            <size>0.3 0.6 0.016</size>
+            <size>0.3 0.7 0.016</size>
           </box>
         </geometry>
         <material>
@@ -87,34 +87,34 @@
         </material>
       </visual>
       <collision name="bottom">
-        <pose> 0 0 -0.3995 0 0 0</pose>
+        <pose> 0 0 -0.6005 0 0 0</pose>
         <geometry>
           <box>
-            <size>0.3 0.6 0.016</size>
+            <size>0.3 0.7 0.016</size>
           </box>
         </geometry>
       </collision>
       <collision name="top">
-        <pose> 0 0 0.3995 0 0 0</pose>
+        <pose> 0 0 0.6005 0 0 0</pose>
         <geometry>
           <box>
-            <size>0.3 0.6 0.016</size>
+            <size>0.3 0.7 0.016</size>
           </box>
         </geometry>
       </collision>
       <collision name="shelf_lower">
-        <pose> 0 0 -0.13115 0 0 0</pose>
+        <pose> 0 0 -0.23115 0 0 0</pose>
         <geometry>
           <box>
-            <size>0.3 0.6 0.016</size>
+            <size>0.3 0.7 0.016</size>
           </box>
         </geometry>
       </collision>
       <collision name="shelf_upper">
-        <pose> 0 0 0.13115 0 0 0</pose>
+        <pose> 0 0 0.23115 0 0 0</pose>
         <geometry>
           <box>
-            <size>0.3 0.6 0.016</size>
+            <size>0.3 0.7 0.016</size>
           </box>
         </geometry>
       </collision>
@@ -140,7 +140,7 @@
       </axis>
     </joint>
     <link name="left_door">
-      <pose> 0.158 -0.1445 0 0 0 0</pose>
+      <pose> 0.158 -0.175 0 0 0 0</pose>
       <inertial>
         <mass>1</mass>
         <inertia>
@@ -167,7 +167,7 @@
       <visual name="slab">
         <geometry>
           <box>
-            <size>0.016 0.279 0.815</size>
+            <size>0.016 0.34 1.205</size>
           </box>
         </geometry>
         <material>
@@ -186,7 +186,7 @@
       <collision name="slab">
         <geometry>
           <box>
-            <size>0.016 0.279 0.815</size>
+            <size>0.016 0.34 1.205</size>
           </box>
         </geometry>
       </collision>
@@ -207,7 +207,7 @@
       </axis>
     </joint>
     <link name="right_door">
-      <pose> 0.158 0.1445 0 0 0 0</pose>
+      <pose> 0.158 0.175 0 0 0 0</pose>
       <inertial>
         <mass>1</mass>
         <inertia>
@@ -234,7 +234,7 @@
       <visual name="slab">
         <geometry>
           <box>
-            <size>0.016 0.279 0.815</size>
+            <size>0.016 0.34 1.205</size>
           </box>
         </geometry>
         <material>
@@ -253,7 +253,7 @@
       <collision name="slab">
         <geometry>
           <box>
-            <size>0.016 0.279 0.815</size>
+            <size>0.016 0.34 1.205</size>
           </box>
         </geometry>
       </collision>
diff --git a/src/brom_drake/robots/stations/kinematic/ur10e_station.py b/src/brom_drake/robots/stations/kinematic/ur10e_station.py
index 5ee8795..1334827 100644
--- a/src/brom_drake/robots/stations/kinematic/ur10e_station.py
+++ b/src/brom_drake/robots/stations/kinematic/ur10e_station.py
@@ -2,7 +2,8 @@
 from pathlib import Path
 
 import numpy as np
-from pydrake.geometry import SceneGraph, Meshcat, MeshcatVisualizer
+from pydrake.geometry import SceneGraph, Meshcat, MeshcatVisualizer, MeshcatVisualizerParams
+from pydrake.geometry import Role as DrakeRole
 from pydrake.math import RollPitchYaw, RigidTransform, RotationMatrix
 from pydrake.multibody.parsing import Parser
 from pydrake.multibody.plant import MultibodyPlant
@@ -10,11 +11,11 @@
 from pydrake.systems.framework import Diagram, DiagramBuilder
 from pydrake.systems.primitives import Demultiplexer
 
-from brom_drake.control import IdealJointPositionController
 # Local imports
 from brom_drake.robots.gripper_type import GripperType
+from brom_drake.control import IdealJointPositionController
 from brom_drake.control.grippers.gripper_controller import GripperController
-from brom_drake.file_manipulation.urdf import DrakeReadyURDFConverter
+from brom_drake.file_manipulation.urdf import DrakeReadyURDFConverter, MeshReplacementStrategy
 
 from brom_drake import robots
 
@@ -86,7 +87,11 @@ def AddArm(self):
         if (not expected_arm_urdf_path.exists()) or self.force_conversion_of_original_urdf:
             # If drake-compatible URDF does not exist, then we need to convert
             # the original URDF to a drake-compatible URDF.
-            arm_urdf = DrakeReadyURDFConverter(original_arm_urdf_path).convert_urdf()
+            arm_urdf = DrakeReadyURDFConverter(
+                original_arm_urdf_path,
+                overwrite_old_logs=True,
+                collision_mesh_replacement_strategy=MeshReplacementStrategy.kWithMinimalEnclosingCylinder,
+            ).convert_urdf()
             arm_urdf = str(arm_urdf)
         else:
             # Otherwise, let's just read the drake-compatible URDF
@@ -152,7 +157,12 @@ def Add2f85Gripper(self):
     def ConnectToMeshcatVisualizer(self, port=None):
         self.meshcat = Meshcat(port)
         m = MeshcatVisualizer(self.meshcat)
-        m.AddToBuilder(self.builder, self.scene_graph, self.meshcat)
+        m.AddToBuilder(
+            self.builder, self.scene_graph, self.meshcat,
+            # params=MeshcatVisualizerParams(
+            #         role=DrakeRole.kProximity,
+            #     ),
+        )
 
         print("Open %s in a browser to view the meshcat visualizer." % self.meshcat.web_url())
 
diff --git a/src/brom_drake/scenes/debug/show_me/show_me_this_model.py b/src/brom_drake/scenes/debug/show_me/show_me_this_model.py
index ce70a46..eb87417 100644
--- a/src/brom_drake/scenes/debug/show_me/show_me_this_model.py
+++ b/src/brom_drake/scenes/debug/show_me/show_me_this_model.py
@@ -2,7 +2,8 @@
 
 import numpy as np
 from manipulation.scenarios import AddMultibodyTriad
-from pydrake.geometry import Meshcat, MeshcatVisualizer
+from pydrake.geometry import Meshcat, MeshcatVisualizer, MeshcatVisualizerParams
+from pydrake.geometry import Role as DrakeRole
 from pydrake.multibody.parsing import Parser
 from pydrake.multibody.plant import AddMultibodyPlantSceneGraph
 from pydrake.systems.framework import DiagramBuilder, Diagram, Context
@@ -23,8 +24,9 @@ def __init__(
         path_to_model: str,
         with_these_joint_positions: List[float] = None,
         base_link_name: str = None,
-        time_step: float = 1e3,
+        time_step: float = 1e-3,
         meshcat_port_number: int = 7001, # Usually turn off for CI (i.e., make it None)
+        show_collision_geometries: bool = False,
         **kwargs,
     ):
         super().__init__(**kwargs)
@@ -35,6 +37,7 @@ def __init__(
         self.base_link_name = base_link_name
         self.time_step = time_step
         self.meshcat_port_number = meshcat_port_number
+        self.show_collision_geometries = show_collision_geometries
 
         # If the base link name is not provided,
         # then we will try to do a smart search for it later.
@@ -121,8 +124,21 @@ def add_all_secondary_cast_members_to_builder(self):
         # Connect to Meshcat, if requested
         if self.meshcat_port_number is not None:
             self.meshcat = Meshcat(port=self.meshcat_port_number)  # Object provides an interface to Meshcat
-            m_visualizer = MeshcatVisualizer(self.meshcat)
-            m_visualizer.AddToBuilder(self.builder, self.scene_graph, self.meshcat)
+            m_visualizer = MeshcatVisualizer(
+                self.meshcat,
+            )
+            params = MeshcatVisualizerParams(
+                role=DrakeRole.kIllustration,
+            )
+            if self.show_collision_geometries:
+                params = MeshcatVisualizerParams(
+                    role=DrakeRole.kProximity,
+                )
+
+            m_visualizer.AddToBuilder(
+                self.builder, self.scene_graph, self.meshcat,
+                params=params,
+            )
 
         # Finalize plant and connect it to system
         self.plant.Finalize()
diff --git a/src/brom_drake/scenes/ids.py b/src/brom_drake/scenes/ids.py
index c3f917c..bc1408b 100644
--- a/src/brom_drake/scenes/ids.py
+++ b/src/brom_drake/scenes/ids.py
@@ -4,4 +4,5 @@
 class SceneID(Enum):
     kNotDefined = "NotDefined"
     kShowMeThisModel = "ShowMeThisModel"
-    kShelfPlanning1 = "ShelfPlanning1"
\ No newline at end of file
+    kShelfPlanning1 = "ShelfPlanning1"
+    kChemLab1 = "ChemLab-Planning1"
\ No newline at end of file
diff --git a/src/brom_drake/scenes/motion_planning/offline/__init__.py b/src/brom_drake/scenes/motion_planning/offline/__init__.py
index 30152ab..3bf5245 100644
--- a/src/brom_drake/scenes/motion_planning/offline/__init__.py
+++ b/src/brom_drake/scenes/motion_planning/offline/__init__.py
@@ -1,3 +1,7 @@
+from .chem_lab1 import ChemLab1Scene
 from .shelf import ShelfPlanningScene
 
-__all__ = ["ShelfPlanningScene"]
\ No newline at end of file
+__all__ = [
+    "ShelfPlanningScene",
+    "ChemLab1Scene",
+]
\ No newline at end of file
diff --git a/src/brom_drake/scenes/motion_planning/offline/chem_lab1.py b/src/brom_drake/scenes/motion_planning/offline/chem_lab1.py
new file mode 100644
index 0000000..b3d898e
--- /dev/null
+++ b/src/brom_drake/scenes/motion_planning/offline/chem_lab1.py
@@ -0,0 +1,478 @@
+
+from importlib import resources as impresources
+import networkx as nx
+import numpy as np
+from pydrake.all import (
+    MultibodyPlant, Parser,
+    CollisionFilterDeclaration, GeometrySet,
+)
+from pydrake.common.eigen_geometry import Quaternion
+from pydrake.math import RollPitchYaw, RigidTransform
+from pydrake.systems.framework import Diagram, Context
+from typing import Callable, Tuple
+
+from brom_drake.directories import DEFAULT_BROM_MODELS_DIR
+from brom_drake.file_manipulation.urdf import drakeify_my_urdf
+from brom_drake.file_manipulation.urdf.shapes.box import BoxDefinition
+from brom_drake.file_manipulation.urdf.simple_writer.urdf_definition import SimpleShapeURDFDefinition, \
+    InertiaDefinition
+from brom_drake.motion_planning.systems.open_loop_plan_dispenser import OpenLoopPlanDispenser
+import brom_drake.robots as robots
+from brom_drake.robots.stations.kinematic import UR10eStation as KinematicUR10eStation
+from brom_drake.scenes import SceneID
+from brom_drake.scenes.roles import Role
+from brom_drake.scenes.types import KinematicMotionPlanningScene
+from brom_drake.utils import Performer, AddGround
+
+
+class ChemLab1Scene(KinematicMotionPlanningScene):
+    """
+    Description:
+        This scene is the first in the chemistry lab series.
+    """
+    def __init__(
+        self,
+        time_step=1e-3,
+        meshcat_port_number: int = 7000,
+        plan_execution_speed: float = 0.2,
+        table_length: float = 0.6,
+        table_width: float = 2.0,
+        table_height: float = 0.1,
+        shelf_pose: RigidTransform = None,
+        **kwargs,
+    ):
+        """
+        Description:
+            Constructor for the ChemLab1Scene class.
+        :param meshcat_port_number:
+        """
+        # Superclass constructor
+        super().__init__(**kwargs)
+
+        # Input Processing
+        self.time_step = time_step
+        self.meshcat_port_number = meshcat_port_number
+        self.plan_execution_speed = plan_execution_speed
+        self.table_height, self.table_width = table_height, table_width
+        self.table_length = table_length
+
+        # Set shelf pose
+        self.shelf_pose = shelf_pose
+        if self.shelf_pose is None:
+            self.pose_WorldShelf = RigidTransform(
+                RollPitchYaw(0.0, 0.0, -np.pi/2.0).ToQuaternion(),
+                np.array([0.0, 0.75, 0.66]),
+            )
+
+        # Set Beaker Pose
+        self.pose_WorldBeaker = RigidTransform(
+            RollPitchYaw(np.pi/2.0, 0.0, 0.0).ToQuaternion(),
+            np.array([-0.6, 0.45, 0.075]),
+        )
+
+        # Set Holder Pose
+        self.pose_WorldHolder = RigidTransform(
+            RollPitchYaw(np.pi/2.0, 0.0, 0.0).ToQuaternion(),
+            np.array([self.table_width*0.5*0.7, 0.6+self.table_length/4., self.table_height-0.015]),
+        )
+
+        # Set station
+        self.station = KinematicUR10eStation(
+            time_step=self.time_step,
+            meshcat_port_number=self.meshcat_port_number,
+        )
+        self.arm = self.station.arm
+        self.robot_model_idx_ = self.arm
+
+        # Set Names of Plant and scene graph
+        self.plant = self.station.plant
+        self.scene_graph = self.station.scene_graph
+
+        self.plant.set_name(f"ChemLabScene1_Plant")
+        self.scene_graph.set_name(f"ChemLabScene1_SceneGraph")
+
+        # Define placeholder variables for models
+        self.test_tube_holder1 = None
+
+    def add_all_secondary_cast_members_to_builder(self):
+        """
+        Description
+        -----------
+        This method adds all secondary cast members to the builder.
+        The secondary cast members in the scene are the:
+        - Table, where the robot exists
+        - Test Tube Holders
+        - ...
+        :return:
+        """
+        # Call the superclass method
+        super().add_all_secondary_cast_members_to_builder()
+
+        # Setup
+
+        # Create the table and the items on it
+        self.add_table()
+        self.add_test_tube_holders()
+        self.add_beaker()
+        self.add_shelf()
+        AddGround(self.plant)
+
+        # Add the station
+        self.builder.AddSystem(
+            self.station,
+        )
+
+        self.station.Finalize()
+
+        # Add the motion planning components
+        self.add_robot_source_system()
+        self.add_motion_planning_components()
+        self.add_start_and_goal_sources_to_builder()
+
+    def add_beaker(self):
+        """
+        Description
+        -----------
+        This method adds the beaker to the scene.
+        """
+        # Setup
+        plant = self.plant
+        urdf_file_path = str(
+            impresources.files(robots) / "models/beaker/beaker.urdf"
+        )
+
+        # Use Drakeify my urdf to create the beaker
+        new_beaker_urdf = drakeify_my_urdf(urdf_file_path)
+
+        # Add the beaker to the plant
+        model_idcs = Parser(plant=plant).AddModels(str(new_beaker_urdf))
+        self.beaker_model_index = model_idcs[0]
+
+        # Weld the beaker to the world frame
+        plant.WeldFrames(
+            plant.world_frame(),
+            plant.GetFrameByName("beaker_base_link", self.beaker_model_index),
+            self.pose_WorldBeaker,
+        )
+
+    def add_motion_planning_components(self):
+        """
+        Add the motion planning components to the builder.
+        :return:
+        """
+        # Setup
+        n_actuated_dof = self.plant.num_actuated_dofs()
+
+        # Add the Plan Dispenser and connect it to the station
+        self.plan_dispenser = self.builder.AddSystem(
+            OpenLoopPlanDispenser(n_actuated_dof, self.plan_execution_speed)
+        )
+
+        self.builder.Connect(
+            self.plan_dispenser.GetOutputPort("point_in_plan"),
+            self.station.GetInputPort("desired_joint_positions"),
+        )
+
+    def add_shelf(self):
+        """
+        Add the shelf to the scene.
+        :return:
+        """
+        # Setup
+        plant = self.plant
+        urdf_file_path = str(
+            impresources.files(robots) / "models/cupboard/cupboard.sdf"
+        )
+
+        # Add the shelf to the plant
+        model_idcs = Parser(plant=plant).AddModels(urdf_file_path)
+        self.shelf_model_index = model_idcs[0]
+
+        # Weld the bookshelf to the world frame
+        plant.WeldFrames(
+            plant.world_frame(),
+            plant.GetFrameByName("cupboard_body", self.shelf_model_index),
+            self.pose_WorldShelf,
+        )
+
+    def add_table(self):
+        """
+        Description
+        -----------
+        This method adds the table to the scene.
+        The table will be a simple shape that the robot will interact with.
+        :return:
+        """
+        # Setup
+
+        # Create a new shape urdf
+        table_defn = SimpleShapeURDFDefinition(
+            name="table",
+            shape=BoxDefinition(
+                size=(self.table_width, self.table_length, self.table_height),
+            ),
+            mass=100.0, # kg
+            inertia=InertiaDefinition(
+                ixx=10.0,
+                iyy=10.0,
+                izz=10.0,
+            ),
+            color=np.array([0.1, 0.1, 0.1, 0.5]),
+        )
+        table_urdf_path = DEFAULT_BROM_MODELS_DIR + "/table/table.urdf"
+        table_defn.write_to_file(table_urdf_path)
+
+        # Add the table to the scene
+        table_model_index = Parser(self.plant).AddModels(table_urdf_path)[0]
+
+        # Weld the table to the world frame
+        table_pose = RigidTransform(
+            RollPitchYaw(0.0, 0.0, 0.0).ToQuaternion(),
+            np.array([0.0, 0.6, 0.0]),
+        )
+        self.plant.WeldFrames(
+            self.plant.world_frame(),
+            self.plant.GetFrameByName("table_base_link", table_model_index),
+            table_pose,
+        )
+
+    def add_test_tube_holders(self):
+        """
+        Description
+        -----------
+        This method adds the test tube holders to the scene.
+        :return:
+        """
+        # Setup
+        test_tube_urdf1 = str(
+            impresources.files(robots) / "models/test_tube_holder/test_tube_holder.urdf"
+        )
+
+        # Use Drakeify my urdf to create the test tube holders
+        new_test_tube_holder_urdf1 = drakeify_my_urdf(test_tube_urdf1)
+
+        self.test_tube_holder1 = Parser(self.plant).AddModels(
+            str(new_test_tube_holder_urdf1)
+        )[0]
+
+        # Weld the test tube holders to the world frame
+        self.plant.WeldFrames(
+            self.plant.world_frame(),
+            self.plant.GetFrameByName("test_tube_holder_base_link", self.test_tube_holder1),
+            self.pose_WorldHolder,
+        )
+
+
+    def cast_scene_and_build(
+        self,
+        cast: Tuple[Role, Performer] = [],
+        with_watcher: bool = False,
+    ) -> Tuple[Diagram, Context]:
+        """
+        Description
+        -----------
+        Modifies the normal cast_scene_and_build, so that
+        we share the context of the plant with the appropriate
+        parts of the system.
+        :param cast:
+        :param with_watcher: A Boolean that determines whether to add a watcher to the diagram.
+        :return:
+        """
+        diagram, diagram_context = super().cast_scene_and_build(
+            cast=cast,
+            with_watcher=with_watcher,
+        )
+
+        # Configure the scene graph for collision detection
+        self.configure_collision_filter(
+            diagram.GetSubsystemContext(
+                self.scene_graph, diagram_context,
+            )
+        )
+
+        # Connect arm controller to the appropriate plant_context
+        self.station.arm_controller.plant_context = diagram.GetSubsystemContext(
+            self.station.arm_controller.plant, diagram_context,
+        )
+
+        for role_ii, performer_ii in cast:
+            if role_ii.name == "OfflineMotionPlanner":
+                performer_ii.set_internal_root_context(
+                    diagram_context
+                )
+
+        return diagram, diagram_context
+    
+    def configure_collision_filter(self, scene_graph_context: Context):
+        """
+        Description
+        -----------
+        This method configures the collision filter for the scene.
+        :param scene_graph_context:
+        :return:
+        """
+        # Setup
+        scene_graph = self.scene_graph
+        shelf_model_index = self.shelf_model_index
+
+        # Ignore self collisions of the shelf, by:
+        # - Getting the model instance index of the shelf
+        # - Collecting the Geometry IDs of all the geometries in the shelf
+        shelf_geometry_ids = []
+        for body_index in self.plant.GetBodyIndices(shelf_model_index):
+            # Get the geometry IDs
+            shelf_geometry_ids.extend(
+                self.plant.GetCollisionGeometriesForBody(
+                    self.plant.get_body(body_index)
+                )
+            )
+            # print(self.plant.get_body(body_index))
+
+        self.shelf_geometry_ids = shelf_geometry_ids
+
+        # Apply the collision filter
+        scene_graph.collision_filter_manager(scene_graph_context).Apply(
+            CollisionFilterDeclaration().ExcludeWithin(
+                GeometrySet(self.shelf_geometry_ids)
+            )
+        )
+
+        # Ignore collisions between the robot links
+        # TODO: Actually implement this for adjacent links? Or is this not necessary?
+        arm_geometry_ids = []
+        for body_index in self.plant.GetBodyIndices(self.arm):
+            arm_geometry_ids.extend(
+                self.plant.GetCollisionGeometriesForBody(
+                    self.plant.get_body(body_index)
+                )
+            )
+
+        self.arm_geometry_ids = arm_geometry_ids
+
+        # Apply the collision filter
+        scene_graph.collision_filter_manager(scene_graph_context).Apply(
+            CollisionFilterDeclaration().ExcludeWithin(
+                GeometrySet(self.arm_geometry_ids)
+            )
+        )
+    
+    def easy_cast_and_build(
+        self,
+        planning_algorithm: Callable[
+            [np.ndarray, np.ndarray, Callable[[np.ndarray], bool]],
+            Tuple[nx.DiGraph, np.ndarray],
+        ],
+        with_watcher: bool = False,
+    ) -> Tuple[Diagram, Context]:
+        """
+        Description
+        -----------
+        This function is used to easily cast and build the scene.
+        :param planning_algorithm: The algorithm that we will use to
+        plan the motion.
+        :param with_watcher: A Boolean that determines whether to add a watcher to the diagram.
+        :return:
+        """
+        # Setup
+
+        # Use Base class implementation to start
+        diagram, diagram_context = super().easy_cast_and_build(
+            planning_algorithm,
+            with_watcher=with_watcher,
+        )
+
+        # Configure the scene graph for collision detection
+        self.configure_collision_filter(
+            diagram.GetSubsystemContext(
+                self.scene_graph, diagram_context,
+            )
+        )
+
+        # Connect arm controller to the appropriate plant_context
+        self.station.arm_controller.plant_context = diagram.GetSubsystemContext(
+            self.station.arm_controller.plant, diagram_context,
+        )
+
+        self.performers[0].set_internal_root_context(
+            diagram_context
+        )
+
+        return diagram, diagram_context
+
+    @property
+    def goal_configuration(self):
+        """
+        Get the goal pose. This should be defined by the subclass.
+        :return:
+        """
+        if self.goal_config_ is None:
+            beaker_to_goal_translation = np.array([+0.0, 0.2, 0.0]) 
+            beaker_to_goal_orientation = RollPitchYaw(0., 0., 0.0).ToQuaternion()
+            X_BeakerGoal = RigidTransform(  
+                beaker_to_goal_orientation,
+                beaker_to_goal_translation,
+            )
+
+            pose_WorldGoal = self.pose_WorldBeaker.multiply(X_BeakerGoal)
+
+            # Use Inverse Kinematics to get the goal configuration of the robot
+            self.goal_config_ = self.solve_pose_ik_problem(
+                pose_WorldGoal,
+            )
+
+            return self.goal_config_
+        else:
+            return self.goal_config_
+
+    @property
+    def goal_pose(self) -> RigidTransform:
+        """
+        Description
+        -----------
+        Get the goal pose. This should be defined by the subclass.
+        """
+        # Setup
+
+        # Algorithm
+        if self.goal_pose_ is None:
+            beaker_to_goal_translation = np.array([+0.0, 0.2, 0.0]) 
+            beaker_to_goal_orientation = RollPitchYaw(0., 0., 0.0).ToQuaternion()
+            X_BeakerGoal = RigidTransform(  
+                beaker_to_goal_orientation,
+                beaker_to_goal_translation,
+            )
+
+            pose_WorldGoal = self.pose_WorldBeaker.multiply(X_BeakerGoal)
+            self.goal_pose_ = pose_WorldGoal
+
+            return self.goal_pose_
+        else:
+            return self.goal_pose_
+
+    @property
+    def id(self) -> SceneID:
+        return SceneID.kShelfPlanning1
+        
+    # TODO(kwesi): Implement start_configuration method.
+
+    @property
+    def start_pose(self) -> RigidTransform:
+        """
+        Get the start pose. This should be defined by the subclass.
+        :return:
+        """
+        # Setup
+
+        if self.start_pose_ is None:
+            # Define Start Pose
+            holder_to_start_translation = np.array([+0.0, 0.2, 0.025])
+            holder_to_start_orientation = Quaternion(1, 0, 0, 0)
+            X_HolderStart = RigidTransform(
+                holder_to_start_orientation,
+                holder_to_start_translation,
+            )
+            pose_WorldStart = self.pose_WorldHolder.multiply(X_HolderStart)
+            self.start_pose_ = pose_WorldStart
+            return self.start_pose_
+        else:
+            return self.start_pose_
diff --git a/src/brom_drake/scenes/motion_planning/offline/shelf.py b/src/brom_drake/scenes/motion_planning/offline/shelf.py
index 92ffa2e..c65e190 100644
--- a/src/brom_drake/scenes/motion_planning/offline/shelf.py
+++ b/src/brom_drake/scenes/motion_planning/offline/shelf.py
@@ -14,16 +14,15 @@
 
 # Internal Imports
 import brom_drake.robots as robots
-from brom_drake.example_helpers import AddGround
 from brom_drake.motion_planning.systems.open_loop_plan_dispenser import OpenLoopPlanDispenser
 from brom_drake.robots.stations.kinematic import UR10eStation as KinematicUR10eStation
 from brom_drake.scenes import SceneID
 from brom_drake.scenes.roles import Role
-from brom_drake.scenes.types.motion_planning import OfflineMotionPlanningScene
-from brom_drake.utils import Performer
+from brom_drake.scenes.types.motion_planning import KinematicMotionPlanningScene
+from brom_drake.utils import Performer, GroundShape, AddGround
 
 
-class ShelfPlanningScene(OfflineMotionPlanningScene):
+class ShelfPlanningScene(KinematicMotionPlanningScene):
     def __init__(
         self,
         time_step=1e-3,
@@ -48,7 +47,7 @@ def __init__(
                 np.array([0.0, 1.0, 0.6]),
             )
 
-        self.desired_cupboard_positions = np.array([0.0, 0.0])
+        self.desired_cupboard_positions = np.array([0.2, 0.3])
 
         # Create containers for meshcat and other values we will set later
         self.meshcat = None
@@ -60,6 +59,7 @@ def __init__(
             meshcat_port_number=self.meshcat_port_number,
         )
         self.arm = self.station.arm
+        self.robot_model_idx_ = self.station.arm
 
         # Set Names of Plant and scene graph
         self.plant = self.station.plant
@@ -77,6 +77,9 @@ def add_all_secondary_cast_members_to_builder(self):
         Add all secondary cast members to the builder.
         :return:
         """
+        # Call the parent class method
+        super().add_all_secondary_cast_members_to_builder()
+
         # Setup
         self.add_ur10e_station() # Use the UR10e station's plant + scene graph for all other objects
 
@@ -85,17 +88,15 @@ def add_all_secondary_cast_members_to_builder(self):
 
         # Add the ground as well as the start and goal locations
         AddGround(self.plant)
-        self.add_start_and_goal_to_this_plant(self.plant)
-
         self.station.Finalize()
 
         # Add The Motion Planning Components (e.g., the interpolator)
-        self.add_start_source_system()
-        self.add_goal_source_system()
         self.add_robot_source_system()
 
         self.add_motion_planning_components()
 
+        self.add_start_and_goal_sources_to_builder()
+
         # Connect motion planning components to station
         # self.connect_motion_planning_components()
 
@@ -126,24 +127,6 @@ def add_motion_planning_components(self):
             self.station.GetInputPort("desired_joint_positions"),
         )
 
-    def add_robot_source_system(self):
-        """
-        Description
-        -----------
-        This method adds a source for providing the motion planner
-        with the model index for the robot that we are trying to control.
-        :return:
-        """
-        # Setup
-
-        # Create AbstractValueSource
-        robot_source_system = ConstantValueSource(
-            AbstractValue.Make(self.station.arm)
-        )
-        robot_source_system.set_name("robot_model_index_source")
-
-        self.builder.AddSystem(robot_source_system)
-
     def add_shelf(self, plant: MultibodyPlant):
         """
         Add the shelf to the scene.
@@ -178,6 +161,7 @@ def add_ur10e_station(self):
     def cast_scene_and_build(
         self,
         cast: Tuple[Role, Performer] = [],
+        with_watcher: bool = False,
     ) -> Tuple[Diagram, Context]:
         """
         Description
@@ -186,9 +170,13 @@ def cast_scene_and_build(
         we share the context of the plant with the appropriate
         parts of the system.
         :param cast:
+        :param with_watcher: A Boolean that determines whether to add a watcher to the diagram.
         :return:
         """
-        diagram, diagram_context = super().cast_scene_and_build(cast=cast)
+        diagram, diagram_context = super().cast_scene_and_build(
+            cast=cast,
+            with_watcher=with_watcher,
+        )
 
         # Configure the scene graph for collision detection
         self.configure_collision_filter(
@@ -225,22 +213,41 @@ def configure_collision_filter(self, scene_graph_context: Context):
         # Ignore self collisions of the shelf, by:
         # - Getting the model instance index of the shelf
         # - Collecting the Geometry IDs of all the geometries in the shelf
-        geometry_ids = []
+        shelf_geometry_ids = []
         for body_index in self.plant.GetBodyIndices(shelf_model_index):
             # Get the geometry IDs
-            geometry_ids.extend(
+            shelf_geometry_ids.extend(
+                self.plant.GetCollisionGeometriesForBody(
+                    self.plant.get_body(body_index)
+                )
+            )
+            # print(self.plant.get_body(body_index))
+
+        self.shelf_geometry_ids = shelf_geometry_ids
+
+        # Apply the collision filter
+        scene_graph.collision_filter_manager(scene_graph_context).Apply(
+            CollisionFilterDeclaration().ExcludeWithin(
+                GeometrySet(self.shelf_geometry_ids)
+            )
+        )
+
+        # Ignore collisions between the robot links
+        # TODO: Actually implement this for adjacent links? Or is this not necessary?
+        arm_geometry_ids = []
+        for body_index in self.plant.GetBodyIndices(self.arm):
+            arm_geometry_ids.extend(
                 self.plant.GetCollisionGeometriesForBody(
                     self.plant.get_body(body_index)
                 )
             )
-            print(self.plant.get_body(body_index))
 
-        self.geometry_ids_to_ignore = geometry_ids
+        self.arm_geometry_ids = arm_geometry_ids
 
         # Apply the collision filter
         scene_graph.collision_filter_manager(scene_graph_context).Apply(
             CollisionFilterDeclaration().ExcludeWithin(
-                GeometrySet(self.geometry_ids_to_ignore)
+                GeometrySet(self.arm_geometry_ids)
             )
         )
 
@@ -260,22 +267,28 @@ def connect_motion_planning_components(
 
     def easy_cast_and_build(
         self,
-            planning_algorithm: Callable[
-                [np.ndarray, np.ndarray, Callable[[np.ndarray], bool]],
-                Tuple[nx.DiGraph, np.ndarray],
-            ],
+        planning_algorithm: Callable[
+            [np.ndarray, np.ndarray, Callable[[np.ndarray], bool]],
+            Tuple[nx.DiGraph, np.ndarray],
+        ],
+        with_watcher: bool = False,
     ) -> Tuple[Diagram, Context]:
         """
         Description
         -----------
         This function is used to easily cast and build the scene.
         :param planning_algorithm: The algorithm that we will use to
+        plan the motion.
+        :param with_watcher: A Boolean that determines whether to add a watcher to the diagram.
         :return:
         """
         # Setup
 
         # Use Base class implementation to start
-        diagram, diagram_context = super().easy_cast_and_build(planning_algorithm)
+        diagram, diagram_context = super().easy_cast_and_build(
+            planning_algorithm,
+            with_watcher=with_watcher,
+        )
 
         # Configure the scene graph for collision detection
         self.configure_collision_filter(
@@ -301,20 +314,31 @@ def goal_pose(self):
         Get the goal pose. This should be defined by the subclass.
         :return:
         """
-        goal_position = np.array([+0.1, 1.0, 0.55]) # np.array([+0.5, 0.7, 0.65])
-        goal_orientation = RollPitchYaw(np.pi/2.0, np.pi/2.0, 0.0).ToQuaternion()
-        return RigidTransform(goal_orientation, goal_position)
+        if self.goal_pose_ is None:
+            goal_position = np.array([+0.0, 1.0, 0.6])
+            goal_orientation = RollPitchYaw(np.pi / 2.0, np.pi / 2.0, 0.0).ToQuaternion()
+            self.goal_pose_ = RigidTransform(goal_orientation, goal_position)
+            return self.goal_pose_
+        else:
+            return self.goal_pose_
+
 
     @property
     def id(self) -> SceneID:
         return SceneID.kShelfPlanning1
 
     @property
-    def start_pose(self):
+    def start_pose(self) -> RigidTransform:
         """
+        Description
+        -----------
         Get the start pose. This should be defined by the subclass.
         :return:
         """
-        start_position = np.array([+0.3, 0.1, 1.2])
-        start_orientation = Quaternion(1, 0, 0, 0)
-        return RigidTransform(start_orientation, start_position)
\ No newline at end of file
+        if self.start_pose_ is None:
+            start_position = np.array([+0.3, 0.1, 1.2])
+            start_orientation = Quaternion(1, 0, 0, 0)
+            self.start_pose_ = RigidTransform(start_orientation, start_position)
+            return self.start_pose_
+        else:
+            return self.start_pose_
\ No newline at end of file
diff --git a/src/brom_drake/scenes/roles/__init__.py b/src/brom_drake/scenes/roles/__init__.py
index 7967a12..7cf4fa9 100644
--- a/src/brom_drake/scenes/roles/__init__.py
+++ b/src/brom_drake/scenes/roles/__init__.py
@@ -1,8 +1,8 @@
 from brom_drake.scenes.roles.role import Role, RolePortAssignment
-from brom_drake.scenes.roles.offline_motion_planner import kOfflineMotionPlanner
+from brom_drake.scenes.roles.motion_planners.kinematic import kKinematicMotionPlanner
 
 __all__ = [
     "Role",
-    "kOfflineMotionPlanner",
+    "kKinematicMotionPlanner",
     "RolePortAssignment",
 ]
\ No newline at end of file
diff --git a/src/brom_drake/scenes/roles/motion_planners/__init__.py b/src/brom_drake/scenes/roles/motion_planners/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/brom_drake/scenes/roles/offline_motion_planner.py b/src/brom_drake/scenes/roles/motion_planners/kinematic.py
similarity index 90%
rename from src/brom_drake/scenes/roles/offline_motion_planner.py
rename to src/brom_drake/scenes/roles/motion_planners/kinematic.py
index 7e00101..309ab54 100644
--- a/src/brom_drake/scenes/roles/offline_motion_planner.py
+++ b/src/brom_drake/scenes/roles/motion_planners/kinematic.py
@@ -6,16 +6,16 @@
 from brom_drake.scenes.roles import Role, RolePortAssignment
 from brom_drake.scenes.roles.role_port_assignment import PairingType
 
-kOfflineMotionPlanner = Role(
+kKinematicMotionPlanner = Role(
     name="OfflineMotionPlanner",
     description="This role is used to define the offline motion planning role.",
     port_assignments=[
         RolePortAssignment(
-            "start_pose", "start_pose",
+            "start_configuration", "start_configuration",
             pairing_type=PairingType.kInput,
         ),
         RolePortAssignment(
-            "goal_pose", "goal_pose",
+            "goal_configuration", "goal_configuration",
             pairing_type=PairingType.kInput,
         ),
         RolePortAssignment(
diff --git a/src/brom_drake/scenes/types/__init__.py b/src/brom_drake/scenes/types/__init__.py
index 00f77b8..2634cba 100644
--- a/src/brom_drake/scenes/types/__init__.py
+++ b/src/brom_drake/scenes/types/__init__.py
@@ -1,8 +1,8 @@
 from .base import BaseScene, Performer
-from .motion_planning import OfflineMotionPlanningScene
+from .motion_planning import KinematicMotionPlanningScene
 
 __all__ = [
     "BaseScene",
-    "OfflineMotionPlanningScene",
+    "KinematicMotionPlanningScene",
     "Performer",
 ]
\ No newline at end of file
diff --git a/src/brom_drake/scenes/types/base.py b/src/brom_drake/scenes/types/base.py
index f1cd628..6fb6187 100644
--- a/src/brom_drake/scenes/types/base.py
+++ b/src/brom_drake/scenes/types/base.py
@@ -23,6 +23,9 @@ def __init__(self, **kwargs):
         # Save a list of the performers
         self.performers = []
 
+        # Create an extra place to save DiagramWatcher objects
+        self.watcher = None
+
     def add_all_secondary_cast_members_to_builder(self):
         """
         Description
@@ -79,22 +82,40 @@ def cast_scene(
         for role, performer in cast:
             self.fill_role(role, performer)
 
-    def cast_scene_and_build(
+    def build_scene(
         self,
-        cast: Tuple[Role, Performer] = [],
-    ) -> Tuple[Diagram, Context]:
+        with_watcher: bool = True,
+    ):
+        """
+        Description
+        -----------
+        This method builds the scene.
+        It assumes that all components have been added to the builder.
+        :param with_watcher: A Boolean that determines whether to add a watcher to the diagram.
+        :return:
+        """
         # Setup
         builder = self.builder
-        self.cast_scene(cast)
 
         # Build the diagram
-        # self.diagram = builder.Build()
-        # self.diagram_context = self.diagram.CreateDefaultContext()
-
-        watcher, self.diagram, self.diagram_context = add_watcher_and_build(builder)
+        if with_watcher:
+            self.watcher, self.diagram, self.diagram_context = add_watcher_and_build(builder)
+        else:
+            self.diagram = builder.Build()
+            self.diagram_context = self.diagram.CreateDefaultContext()
 
         return self.diagram, self.diagram_context
 
+    def cast_scene_and_build(
+        self,
+        cast: Tuple[Role, Performer] = [],
+        with_watcher: bool = True,
+    ) -> Tuple[Diagram, Context]:
+        # Setup
+        self.cast_scene(cast)
+
+        return self.build_scene(with_watcher=with_watcher)
+
     @property
     def id(self) -> SceneID:
         """
diff --git a/src/brom_drake/scenes/types/motion_planning/__init__.py b/src/brom_drake/scenes/types/motion_planning/__init__.py
index 09d364a..83f10b4 100644
--- a/src/brom_drake/scenes/types/motion_planning/__init__.py
+++ b/src/brom_drake/scenes/types/motion_planning/__init__.py
@@ -1,3 +1,3 @@
-from .offline import OfflineMotionPlanningScene
+from .kinematic import KinematicMotionPlanningScene
 
-__all__ = ["OfflineMotionPlanningScene"]
\ No newline at end of file
+__all__ = ["KinematicMotionPlanningScene"]
\ No newline at end of file
diff --git a/src/brom_drake/scenes/types/motion_planning/offline.py b/src/brom_drake/scenes/types/motion_planning/kinematic.py
similarity index 52%
rename from src/brom_drake/scenes/types/motion_planning/offline.py
rename to src/brom_drake/scenes/types/motion_planning/kinematic.py
index c5497ff..f83e535 100644
--- a/src/brom_drake/scenes/types/motion_planning/offline.py
+++ b/src/brom_drake/scenes/types/motion_planning/kinematic.py
@@ -4,6 +4,11 @@
 
 import networkx as nx
 import numpy as np
+from pydrake.all import (
+    RotationMatrix, Quaternion, RollPitchYaw,
+    Solve, SolutionResult,
+    InverseKinematics, ModelInstanceIndex,
+)
 from pydrake.common.value import AbstractValue
 from pydrake.math import RigidTransform
 from pydrake.multibody.parsing import Parser
@@ -11,27 +16,69 @@
 from pydrake.systems.framework import Diagram, Context
 from pydrake.systems.primitives import ConstantVectorSource, ConstantValueSource
 
-from brom_drake.all import add_watcher_and_build
+# Internal Imports
 from brom_drake.motion_planning.systems.prototypical_planner import PrototypicalPlannerSystem
 from brom_drake.scenes.types import BaseScene
-from brom_drake.scenes.roles import kOfflineMotionPlanner, Role
-from brom_drake.file_manipulation.urdf.simple_shape_urdfs.shape_definition import SphereDefinition
-from brom_drake.file_manipulation.urdf.simple_shape_urdfs.urdf_definition import SimpleShapeURDFDefinition
+from brom_drake.scenes.roles import kKinematicMotionPlanner, Role
+from brom_drake.file_manipulation.urdf.shapes.sphere import SphereDefinition
+from brom_drake.file_manipulation.urdf.simple_writer.urdf_definition import SimpleShapeURDFDefinition
 from brom_drake.utils import Performer
 
 
-class OfflineMotionPlanningScene(BaseScene):
+class KinematicMotionPlanningScene(BaseScene):
     def __init__(
         self,
-        time_step: float = 1e-3,
+        start_configuration: np.ndarray = None,
+        start_pose: RigidTransform = None,
+        goal_configuration: np.ndarray = None,
+        goal_pose: RigidTransform = None,
         **kwargs,
     ):
         super().__init__(**kwargs)
 
+        # Start and Goal Configurations
+        self.start_config_ = start_configuration
+        self.goal_config_ = goal_configuration
+
+        # Start and Goal Poses
+        self.start_pose_ = start_pose
+        self.goal_pose_ = goal_pose
+
+        # Create placeholder for the plant
+        self.plant = None
+
+        # Create placeholder for the robot model index
+        self.robot_model_idx_ = None
+
     def add_all_secondary_cast_members_to_builder(self):
-        pass
+        """
+        Description
+        -----------
+        This method will add the start and goal poses to the builder.
+        :return:
+        """
+        # Add visual elements for the start and goal poses
+        self.add_start_and_goal_to_plant(self.plant)
+
+    def add_robot_source_system(self):
+        """
+        Description
+        -----------
+        This method adds a source for providing the motion planner
+        with the model index for the robot that we are trying to control.
+        :return:
+        """
+        # Setup
+
+        # Create AbstractValueSource
+        robot_source_system = ConstantValueSource(
+            AbstractValue.Make(self.robot_model_idx_)
+        )
+        robot_source_system.set_name("robot_model_index_source")
+
+        self.builder.AddSystem(robot_source_system)
 
-    def add_start_and_goal_to_this_plant(self, plant: MultibodyPlant):
+    def add_start_and_goal_to_plant(self, plant: MultibodyPlant):
         """
         Description
         -----------
@@ -88,21 +135,29 @@ def add_start_and_goal_to_this_plant(self, plant: MultibodyPlant):
             self.goal_pose,
         )
 
+    def add_start_and_goal_sources_to_builder(self):
+        """
+        Description
+        -----------
+        This method will add the start and goal sources to the builder.
+        """
+        # Input Processing
+        assert self.plant.is_finalized(), "Plant must be finalized before adding start and goal sources."
+
+        # Add the start and goal poses to the builder
+        self.add_start_source_system()
+        self.add_goal_source_system()
+
     def add_start_source_system(self):
         """
         Add a source system for the start pose.
         :return:
         """
         # Setup
-        X_start = self.start_pose
-        p_start = np.hstack((
-            X_start.translation(),
-            X_start.rotation().ToQuaternion().wxyz()
-        ))
 
         # Adds a source system for the start pose to the builder
-        start_source_system = ConstantVectorSource(p_start)
-        start_source_system.set_name("start_pose")
+        start_source_system = ConstantVectorSource(self.start_configuration)
+        start_source_system.set_name("start_configuration")
 
         self.builder.AddSystem(start_source_system)
 
@@ -112,15 +167,10 @@ def add_goal_source_system(self):
         :return:
         """
         # Setup
-        X_goal = self.goal_pose
-        p_goal = np.hstack((
-            X_goal.translation(),
-            X_goal.rotation().ToQuaternion().wxyz()
-        ))
 
         # Adds a source system for the goal pose to the builder
-        goal_source_system = ConstantVectorSource(p_goal)
-        goal_source_system.set_name("goal_pose")
+        goal_source_system = ConstantVectorSource(self.goal_configuration)
+        goal_source_system.set_name("goal_configuration")
 
         self.builder.AddSystem(goal_source_system)
 
@@ -147,49 +197,6 @@ def cast_scene(
                 role_ii, performer_ii
             )
 
-    def easy_cast_and_build(
-        self,
-        planning_algorithm: Callable[
-            [np.ndarray, np.ndarray, Callable[[np.ndarray], bool]],
-            Tuple[nx.DiGraph, np.ndarray],
-        ],
-    ) -> Tuple[Diagram, Context]:
-        """
-        Description
-        -----------
-        This function is used to easily cast and build the scene.
-        :param planning_algorithm: The algorithm that we will use to
-        :return:
-        """
-        # Setup
-
-        # Add all elements to the builder
-        self.add_all_secondary_cast_members_to_builder()
-
-        # Create a planner from the algorithm
-        prototypical_planner = PrototypicalPlannerSystem(
-            self.plant, self.scene_graph,
-            planning_algorithm,
-        )
-
-        # Cast the scene using the prototypical planner
-        planner_role = self.suggested_roles()[0]
-
-        # Fulfill each role-performer pair in the casting_call list
-        self.fill_role(planner_role, prototypical_planner)
-
-        self.create_optional_outputs_if_necessary(
-            planner_role, prototypical_planner
-        )
-
-        # Build
-        # self.diagram = builder.Build()
-        # self.diagram_context = self.diagram.CreateDefaultContext()
-
-        watcher, self.diagram, self.diagram_context = add_watcher_and_build(self.builder)
-
-        return self.diagram, self.diagram_context
-
     def create_optional_outputs_if_necessary(
         self,
         role: Role, # This role will always be the Motion Planner, won't it?
@@ -265,13 +272,111 @@ def create_optional_outputs_if_necessary(
             systems_list[0].GetInputPort(last_assignment.external_target_name)
         )
 
+    def define_pose_ik_problem(
+        self,
+        pose_WorldTarget: RigidTransform,
+        target_frame_name: str,
+        eps0: float = 2.5e-2,
+        orientation_cost_scaling: float = 0.25,
+    ) -> InverseKinematics:
+        """
+        Description
+        -----------
+        Sets up the inverse kinematics problem for the start pose
+        input to theis function.
+        :return:
+        """
+        # Setup
+
+        # Create IK Problem
+        ik_problem = InverseKinematics(self.plant)
+
+        # Add Pose Target
+        ik_problem.AddPositionConstraint(
+            self.plant.world_frame(),
+            pose_WorldTarget.translation(),
+            self.plant.GetFrameByName(target_frame_name),
+            (- np.ones((3,)) * eps0).reshape((-1, 1)),
+            (+ np.ones((3,)) * eps0).reshape((-1, 1)),
+        )
+
+        # TODO(kwesi): Add OrientationCosntraint
+        # ik_problem.AddOrientationConstraint(
+        #     self.plant.world_frame(),
+        #     RotationMatrix(Quaternion(input_pose_vec[3:]).rotation()),
+        #     self.plant.GetFrameByName("ft_frame"),
+        #     RotationMatrix.Identity(),
+        #     0.25,
+        # )
+
+        ik_problem.AddOrientationCost(
+            self.plant.world_frame(),
+            pose_WorldTarget.rotation(),
+            self.plant.GetFrameByName(target_frame_name),
+            RotationMatrix.Identity(),
+            orientation_cost_scaling,
+        )
+
+        return ik_problem
+    
+    def easy_cast_and_build(
+        self,
+        planning_algorithm: Callable[
+            [np.ndarray, np.ndarray, Callable[[np.ndarray], bool]],
+            Tuple[nx.DiGraph, np.ndarray],
+        ],
+        with_watcher: bool = True,
+    ) -> Tuple[Diagram, Context]:
+        """
+        Description
+        -----------
+        This function is used to easily cast and build the scene.
+        :param planning_algorithm: The algorithm that we will use to
+        plan the motion.
+        :param with_watcher: A Boolean that determines whether to add a watcher to the diagram.
+        :return:
+        """
+        # Setup
+
+        # Add all elements to the builder
+        self.add_all_secondary_cast_members_to_builder()
+
+        # Create a planner from the algorithm
+        prototypical_planner = PrototypicalPlannerSystem(
+            self.plant, self.scene_graph,
+            planning_algorithm,
+            robot_model_idx=self.robot_model_index,
+        )
+
+        # Cast the scene using the prototypical planner
+        planner_role = self.suggested_roles()[0]
+
+        # Fulfill each role-performer pair in the casting_call list
+        self.fill_role(planner_role, prototypical_planner)
+
+        self.create_optional_outputs_if_necessary(
+            planner_role, prototypical_planner
+        )
+
+        # Build
+        return self.build_scene(with_watcher=with_watcher)
+
     @property
-    def start_pose(self) -> RigidTransform:
+    def goal_configuration(self) -> np.ndarray:
         """
-        Get the start pose. This should be defined by the subclass.
+        Get the goal pose. This should be defined by the subclass.
         :return:
         """
-        return None
+        if self.goal_config_ is not None:
+            return self.goal_config_
+        elif self.goal_pose_ is not None:
+            # Use the goal pose to get the goal configuration
+            # Using the IK solver (potentially buggy because default ik problem ignores obstacles)
+            return self.solve_pose_ik_problem(self.goal_pose_)
+        else:
+            raise NotImplementedError(
+                "This function should be implemented by the subclass."
+            )
 
     @property
     def goal_pose(self) -> RigidTransform:
@@ -279,7 +384,95 @@ def goal_pose(self) -> RigidTransform:
         Get the goal pose. This should be defined by the subclass.
         :return:
         """
-        return None
+        if self.goal_pose_ is not None:
+            return self.goal_pose_
+        else:
+            raise NotImplementedError(
+                "This function should be implemented by the subclass."
+            )
+
+    @property
+    def robot_model_index(self) -> ModelInstanceIndex:
+        """
+        Get the robot model index. This should be defined by the subclass.
+        :return:
+        """
+        if self.robot_model_idx_ is not None:
+            return self.robot_model_idx_
+        else:        
+            raise NotImplementedError(
+                "This function should be implemented by the subclass."
+            )
+
+    @property
+    def start_configuration(self) -> np.ndarray:
+        """
+        Description:
+            This function returns the start and goal poses as a single array.
+        """
+        if self.start_config_ is not None:
+            return self.start_config_
+        elif self.start_pose_ is not None:
+            # Use the start pose to get the start configuration
+            # Using the IK solver (potentially buggy because default ik problem ignores obstacles)
+            return self.solve_pose_ik_problem(self.start_pose_)
+        else:
+            raise NotImplementedError(
+                "This function should be implemented by the subclass."
+            )
+    
+    @property
+    def start_pose(self) -> RigidTransform:
+        """
+        Get the start pose. This should be defined by the subclass.
+        :return:
+        """
+        if self.start_pose_ is not None:
+            return self.start_pose_
+        else:
+            raise NotImplementedError(
+                "This function should be implemented by the subclass."
+            )
 
     def suggested_roles(self):
-        return [kOfflineMotionPlanner]
\ No newline at end of file
+        return [kKinematicMotionPlanner]
+
+    def solve_pose_ik_problem(
+        self,
+        pose_WorldTarget: RigidTransform,
+        frame_name: str = "ft_frame",
+    ) -> np.ndarray:
+        """
+        Description:
+        :param input_pose_vec: An n-dimensional vector
+        :return:
+        """
+        # Input Processing
+        assert self.plant is not None, "Plant must be defined before solving IK problem!"
+
+        # Setup
+
+        # Define Problem
+        ik_problem = self.define_pose_ik_problem(
+            pose_WorldTarget,
+            frame_name,
+        )
+
+        # Solve problem
+        ik_program = ik_problem.prog()
+        ik_result = Solve(ik_program)
+
+        assert ik_result.get_solution_result() == SolutionResult.kSolutionFound, \
+            f"Solution result was {ik_result.get_solution_result()}; need SolutionResult.kSolutionFound to make RRT Plan!"
+
+        q_solution = ik_result.get_x_val()
+
+        # Extract only the positions that correspond to our robot's joints
+        robot_joint_names = self.plant.GetPositionNames(self.robot_model_index, add_model_instance_prefix=True)
+        all_joint_names = self.plant.GetPositionNames()
+        q_out_list = []
+        for ii, joint_name in enumerate(all_joint_names):
+            if joint_name in robot_joint_names:
+                q_out_list.append(q_solution[ii])
+
+        return np.array(q_out_list)
\ No newline at end of file
diff --git a/src/brom_drake/utils/__init__.py b/src/brom_drake/utils/__init__.py
index 6cbdc68..42f9530 100644
--- a/src/brom_drake/utils/__init__.py
+++ b/src/brom_drake/utils/__init__.py
@@ -3,9 +3,12 @@
     find_all_systems_with_output_port,
 )
 from .constants import Performer
+from .ground import AddGround, GroundShape
 
 __all__ = [
     "find_all_systems_with_input_port",
     "find_all_systems_with_output_port",
     "Performer",
+    "AddGround",
+    "GroundShape",
 ]
\ No newline at end of file
diff --git a/src/brom_drake/utils/ground.py b/src/brom_drake/utils/ground.py
new file mode 100644
index 0000000..e07fd0c
--- /dev/null
+++ b/src/brom_drake/utils/ground.py
@@ -0,0 +1,58 @@
+from enum import IntEnum
+import numpy as np
+from pydrake.all import (
+    MultibodyPlant,
+    RotationMatrix, RigidTransform,
+    CoulombFriction,
+    HalfSpace, Box,
+)
+
+# Class Definition
+class GroundShape(IntEnum):
+    """
+    Description:
+        This class defines the different shapes that the ground can have.
+    """
+    kHalfSpace = 0
+    kBox = 1
+
+def AddGround(
+    plant: MultibodyPlant,
+    shape: GroundShape = GroundShape.kBox,
+):
+    """
+    Add a flat ground with friction
+    """
+
+    # Constants
+    transparent_color = np.array([0.5, 0.5, 0.5, 0])
+    nontransparent_color = np.array([0.5, 0.5, 0.5, 0.1])
+
+    p_GroundOrigin = [0, 0.0, 0.0]
+    R_GroundOrigin = RotationMatrix.MakeXRotation(0.0)
+    X_GroundOrigin = RigidTransform(R_GroundOrigin,p_GroundOrigin)
+
+    # Input Processing
+    if shape == GroundShape.kHalfSpace:
+        shape = HalfSpace()
+    elif shape == GroundShape.kBox:
+        shape = Box(10, 10, 0.1)
+    else:
+        raise ValueError(f"Invalid shape: {shape}!")  
+
+    # Set Up Ground on Plant
+    surface_friction = CoulombFriction(
+            static_friction = 0.7,
+            dynamic_friction = 0.5)
+    plant.RegisterCollisionGeometry(
+            plant.world_body(),
+            X_GroundOrigin,
+            shape,
+            "ground_collision",
+            surface_friction)
+    plant.RegisterVisualGeometry(
+            plant.world_body(),
+            X_GroundOrigin,
+            shape,
+            "ground_visual",
+            transparent_color)  # transparent
\ No newline at end of file


Test patch:
diff --git a/src/brom_drake/robots/models/test_tube_holder/API_test_tube_holder_v0.0.5.stl b/src/brom_drake/robots/models/test_tube_holder/API_test_tube_holder_v0.0.5.stl
new file mode 100644
index 0000000..8ef7174
Binary files /dev/null and b/src/brom_drake/robots/models/test_tube_holder/API_test_tube_holder_v0.0.5.stl differ
diff --git a/src/brom_drake/robots/models/test_tube_holder/readme.md b/src/brom_drake/robots/models/test_tube_holder/readme.md
new file mode 100644
index 0000000..18cdd7f
--- /dev/null
+++ b/src/brom_drake/robots/models/test_tube_holder/readme.md
@@ -0,0 +1,3 @@
+# Test Tube Holder
+
+API Test Tube Rack by Beadsworth on Thingiverse: https://www.thingiverse.com/thing:3887137
\ No newline at end of file
diff --git a/src/brom_drake/robots/models/test_tube_holder/test_tube_holder.urdf b/src/brom_drake/robots/models/test_tube_holder/test_tube_holder.urdf
new file mode 100644
index 0000000..670d53c
--- /dev/null
+++ b/src/brom_drake/robots/models/test_tube_holder/test_tube_holder.urdf
@@ -0,0 +1,15 @@
+<robot name="test-tube-holder">
+    <link name="test_tube_holder_base_link">
+        <visual>
+            <geometry>
+                <mesh filename="API_test_tube_holder_v0.0.5.stl" scale="0.001 0.001 0.001"/>
+            </geometry>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="API_test_tube_holder_v0.0.5.stl" scale="0.001 0.001 0.001"/>
+            </geometry>
+        </collision>
+    </link>
+
+</robot>
diff --git a/tests/PortWatcher/PortWatcher_test.py b/tests/PortWatcher/PortWatcher_test.py
index 2bb43fe..28ac461 100644
--- a/tests/PortWatcher/PortWatcher_test.py
+++ b/tests/PortWatcher/PortWatcher_test.py
@@ -4,6 +4,7 @@
 
     This
 """
+from importlib import resources as impresources
 import shutil
 import unittest
 import os
@@ -18,6 +19,7 @@
 from pydrake.multibody.plant import MultibodyPlant, AddMultibodyPlantSceneGraph
 from pydrake.systems.analysis import Simulator
 from pydrake.systems.framework import DiagramBuilder, PortDataType, Diagram, Context
+from pydrake.systems.primitives import ConstantVectorSource
 
 # Internal Imports
 from brom_drake.all import (
@@ -26,6 +28,8 @@
     FigureNamingConvention
 )
 
+import brom_drake.robots as robots
+
 
 class PortWatcherTest(unittest.TestCase):
     def setUp(self):
@@ -241,14 +245,14 @@ def test_plot_logger_data1(self):
         # Define plant with:
         # + add block model
         # + ground
-        plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-3)
+        plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=time_step)
 
         block_model_idx = Parser(plant=plant).AddModels(
             self.get_brom_drake_dir() + "/examples/watcher/suggested_use1/slider-block.urdf",
         )[0]
         block_body_name = "block"
 
-        p_GroundOrigin = [0, 0.0, 0.0]
+        p_GroundOrigin = [0.0, 0.0, 0.0]
         R_GroundOrigin = RotationMatrix.MakeXRotation(0.0)
         X_GroundOrigin = RigidTransform(R_GroundOrigin, p_GroundOrigin)
         surface_friction = CoulombFriction(
@@ -889,6 +893,99 @@ def test_save_figures7(self):
         if self.delete_test_brom_directory_on_teardown:
             shutil.rmtree(plot_dir)
 
+    def test_system_is_multibody_plant1(self):
+        """
+        Description:
+
+            This test provides a multibody plant to the
+            PortWatcher and verifies that the system_is_multibody_plant
+            method returns True.
+        :return:
+        """
+        # Setup
+        time_step = 0.01
+        builder = DiagramBuilder()
+
+        # Create Diagram
+        plant = builder.AddNamedSystem("my_plant", MultibodyPlant(time_step=time_step))
+        plant.Finalize()
+
+        pw0 = PortWatcher(
+            plant, plant.GetOutputPort("state"), builder,
+        )
+
+        # Test
+        self.assertTrue(pw0.system_is_multibody_plant())
+
+    def test_system_is_multibody_plant2(self):
+        """
+        Description:
+
+            This test provides a non-multibody plant to the
+            PortWatcher and verifies that the system_is_multibody_plant
+            method returns False.
+        :return:
+        """
+        # Setup
+        time_step = 0.01
+        builder = DiagramBuilder()
+
+        # Create Diagram
+        plant = builder.AddNamedSystem("my_plant", MultibodyPlant(time_step=time_step))
+        plant.Finalize()
+
+        constant_vector_source = builder.AddSystem(ConstantVectorSource([1.0, 2.0, 3.0]))
+
+        pw0 = PortWatcher(
+            constant_vector_source,
+            constant_vector_source.get_output_port(),
+            builder,
+        )
+
+        # Test
+        self.assertFalse(pw0.system_is_multibody_plant())
+
+    def test_name_of_data_at_index1(self):
+        """
+        Description:
+
+            This test verifies that the name_of_data_at_index method
+            properly returns the name of the data at the given index
+            when handling a plant with some models in it.
+
+            Note: The new name is only active with specific options.
+        :return:
+        """
+        # Setup
+        time_step = 0.01
+        builder = DiagramBuilder()
+
+        # Add in some models
+        plant =  MultibodyPlant(time_step=time_step)
+        plant = builder.AddSystem(plant)
+        cupboard_sdf_file = str(
+            impresources.files(robots) / "models/cupboard/cupboard.sdf"
+        )
+        Parser(plant).AddModels(cupboard_sdf_file)
+        plant.Finalize()
+
+
+        pw0 = PortWatcher(
+            plant,
+            plant.GetOutputPort("state"),
+            builder,
+            options=PortWatcherOptions(
+                plot_arrangement=PortFigureArrangement.OnePlotPerDim,
+            )
+        )
+
+        # Test
+        name0 = pw0.name_of_data_at_index(0)
+
+        self.assertIn(
+            "cupboard",
+            name0,
+        )
 
 if __name__ == '__main__':
     unittest.main()
diff --git a/tests/example_helpers/block_handler_system_test.py b/tests/example_helpers/block_handler_system_test.py
index 47041c4..39bf850 100644
--- a/tests/example_helpers/block_handler_system_test.py
+++ b/tests/example_helpers/block_handler_system_test.py
@@ -13,8 +13,9 @@
 )
 
 # Internal Imports
-from brom_drake.example_helpers import BlockHandlerSystem, AddGround
+from brom_drake.example_helpers import BlockHandlerSystem
 import brom_drake.example_helpers as eh
+from brom_drake.utils import AddGround
 
 
 class TestBlockHandlerSystem(unittest.TestCase):
diff --git a/tests/file_manipulation/urdf/DrakeReadyURDFConverter_test.py b/tests/file_manipulation/urdf/DrakeReadyURDFConverter_test.py
index d216872..68b70d0 100644
--- a/tests/file_manipulation/urdf/DrakeReadyURDFConverter_test.py
+++ b/tests/file_manipulation/urdf/DrakeReadyURDFConverter_test.py
@@ -6,17 +6,22 @@
 """
 from importlib import resources as impresources
 import loguru
+import numpy as np
 from pathlib import Path
 import unittest
 from xml.etree.ElementTree import ElementTree
 
+from pydrake.systems.analysis import Simulator
 from pydrake.multibody.parsing import Parser
 from pydrake.multibody.plant import MultibodyPlant
 
 # Internal Imports
 import brom_drake
-from brom_drake.file_manipulation.urdf import DrakeReadyURDFConverter
+from brom_drake import robots
+from brom_drake.all import drakeify_my_urdf
+from brom_drake.file_manipulation.urdf import DrakeReadyURDFConverter, MeshReplacementStrategy
 import resources as resources_dir
+from brom_drake.scenes.debug import ShowMeThisModel
 
 
 class DrakeReadyURDFConverterTest(unittest.TestCase):
@@ -122,18 +127,19 @@ def test_convert_tree_element2(self):
             log_file_name="test_convert_tree_element2.log",
             overwrite_old_logs=True,
         )
-        target_mesh_elt = test_tree.find(".//mesh[@filename='./meshes/ur10e/visual/base.dae']")
-        new_root = converter.convert_tree_element(target_mesh_elt)
+        # Find an element that references a .dae file
+        target_visual_elt = test_tree.getroot().find("link/visual")
+        new_visual_elt = converter.convert_tree_element(target_visual_elt)
 
         # Compare the filenames of the mesh and the new obj file
         self.assertNotEqual(
-            target_mesh_elt.attrib["filename"],
-            new_root.attrib["filename"]
+            target_visual_elt.find("geometry/mesh").attrib["filename"],
+            new_visual_elt.find("geometry/mesh").attrib["filename"]
         )
 
         self.assertIn(
             ".obj",
-            new_root.attrib["filename"]
+            new_visual_elt.find("geometry/mesh").attrib["filename"]
         )
 
     def test_create_obj_to_replace_mesh_file1(self):
@@ -439,5 +445,87 @@ def test_convert_urdf6(self):
         added_models = Parser(temp_plant).AddModels(str(new_urdf_path))
         self.assertTrue(True)
 
+    def test_convert_urdf7(self):
+        """
+        Description
+        -----------
+        This test verifies that we can convert a full URDF file
+        with the option to replace collision geometries with visual geometries.
+        """
+        # Setup
+        test_urdf = self.test_urdf1_filename
+
+        # Create pay to the collision base file
+        test_dir = Path(__file__).parent
+
+        # Use converter on urdf6
+        converter = DrakeReadyURDFConverter(
+            test_urdf,
+            output_urdf_file_path="./brom/resources/test_convert_urdf7.urdf",
+            overwrite_old_logs=True,
+            log_file_name="test_convert_urdf7.log",
+            collision_mesh_replacement_strategy=MeshReplacementStrategy.kWithMinimalEnclosingCylinder,
+        )
+
+        # Test
+        new_urdf_path = converter.convert_urdf()
+
+        # Verify that the new file exists
+        self.assertTrue(new_urdf_path.exists())
+
+        # Verify that the new file's collision elements DO NOT contain mesh elements
+        new_tree = ElementTree(file=new_urdf_path)
+        all_mesh_elts = list(new_tree.iter("mesh"))
+        self.assertTrue(
+            len(all_mesh_elts) == 1
+        )
+
+        # Search through all the collision elements and make sure they are cylinders
+        for collision_elt in new_tree.iter("collision"):
+            self.assertEqual(
+                collision_elt.find("geometry").find("cylinder").tag,
+                "cylinder"
+            )
+
+        # Make sure that this can be added to a plant
+        temp_plant = MultibodyPlant(time_step=1e-3)
+        added_models = Parser(temp_plant).AddModels(str(new_urdf_path))
+        self.assertTrue(True)
+
+    def test_vis1(self):
+
+        # Setup
+        urdf_file_path = str(
+            impresources.files(robots) / "models/ur/ur10e.urdf"
+        )
+
+        # Convert the URDF
+        new_urdf_path = drakeify_my_urdf(
+            urdf_file_path,
+            overwrite_old_logs=True,
+            log_file_name="drakeify-my-urdf1.log",
+            collision_mesh_replacement_strategy=MeshReplacementStrategy.kWithMinimalEnclosingCylinder,
+        )
+
+        # Visualize the URDF using the "show-me-this-model" feature
+        time_step = 1e-3
+        scene = ShowMeThisModel(
+            str(new_urdf_path),
+            with_these_joint_positions=[0.0, 0.0, -np.pi/4.0, 0.0, 0.0, 0.0],
+            time_step=time_step,
+        )
+
+        # Build Diagram
+        diagram, diagram_context = scene.cast_scene_and_build()
+
+        # Set up simulation
+        simulator = Simulator(diagram, diagram_context)
+        simulator.set_target_realtime_rate(1.0)
+        simulator.set_publish_every_time_step(False)
+
+        # Run simulation
+        simulator.Initialize()
+        simulator.AdvanceTo(15.0)
+
 if __name__ == '__main__':
     unittest.main()
\ No newline at end of file
diff --git a/tests/file_manipulation/urdf/shapes/cylinder_test.py b/tests/file_manipulation/urdf/shapes/cylinder_test.py
new file mode 100644
index 0000000..339a5b5
--- /dev/null
+++ b/tests/file_manipulation/urdf/shapes/cylinder_test.py
@@ -0,0 +1,76 @@
+"""
+Description:
+    This script tests the cylinder class in the shapes module.
+"""
+import numpy as np
+from pydrake.all import (
+    RigidTransform, RollPitchYaw,
+    Simulator,
+)
+import unittest
+import xml.etree.ElementTree as ET
+
+# Internal Imports
+from brom_drake.directories import DEFAULT_BROM_MODELS_DIR
+from brom_drake.file_manipulation.urdf.simple_writer.urdf_definition import SimpleShapeURDFDefinition
+from brom_drake.file_manipulation.urdf.shapes.cylinder import CylinderDefinition
+from brom_drake.scenes.debug.show_me import ShowMeThisModel
+
+class TestCylinder(unittest.TestCase):
+    """
+    A class to test the Cylinder class.
+    """
+    def test_init1(self):
+        """
+        Test the __init__ method.
+        """
+        cylinder = CylinderDefinition(1, 2)
+        self.assertEqual(cylinder.radius, 1)
+        self.assertEqual(cylinder.length, 2)
+
+    def test_add_geometry_to_element1(self):
+        """
+        Test the add_geometry_to_element method properly creates the cylinder element.
+        """
+        # Setup
+        dummy_element = ET.Element("dummy")
+
+        # Run
+        cylinder = CylinderDefinition(1, 2)
+        element = cylinder.add_geometry_to_element(dummy_element)
+        self.assertEqual(element.tag, "cylinder")
+        self.assertEqual(element.attrib, {"radius": "1", "length": "2"})
+
+    def test_in_urdf1(self):
+        """
+        Test the in_urdf method.
+        """
+        # Setup
+        urdf_location = DEFAULT_BROM_MODELS_DIR + "/test_cylinder.urdf"
+
+        # Create cylinder
+        cylinder = CylinderDefinition(1, 2)
+
+        # Add to URDF Definition
+        defn1 = SimpleShapeURDFDefinition(
+            name="cylinder",
+            shape=cylinder,
+            create_collision=True,
+            pose=RigidTransform(
+                R=RollPitchYaw(np.pi/4, 0, 0).ToRotationMatrix(),
+            ),
+        )
+        defn1.write_to_file(urdf_location)
+
+        # Try to load this into a simple simulation
+        scene = ShowMeThisModel(urdf_location, meshcat_port_number=None)
+        diagram, diagram_context = scene.cast_scene_and_build()
+
+        # Simulate the scene
+        sim = Simulator(diagram, diagram_context)
+        sim.AdvanceTo(1.0)
+
+        self.assertTrue(True)
+
+if __name__ == '__main__':
+    unittest.main()
\ No newline at end of file
diff --git a/tests/file_manipulation/urdf/urdf_definition_test.py b/tests/file_manipulation/urdf/urdf_definition_test.py
new file mode 100644
index 0000000..c7f313d
--- /dev/null
+++ b/tests/file_manipulation/urdf/urdf_definition_test.py
@@ -0,0 +1,43 @@
+import unittest
+import xml.etree.ElementTree as ET
+
+# Internal imports
+from brom_drake.file_manipulation.urdf.shapes.box import BoxDefinition
+from brom_drake.file_manipulation.urdf.simple_writer.urdf_definition import SimpleShapeURDFDefinition
+
+class URDFDefinitionTest(unittest.TestCase):
+    def test_add_visual_elements_to1(self):
+        """
+        Test the add_visual_elements_to() method of the URDFDefinition class.
+        :return:
+        """
+        # Setup
+        shape1 = BoxDefinition(
+            size=(1.0, 1.0, 1.0),
+        )
+
+        # Create the SimpleShapeURDF object
+        defn1 = SimpleShapeURDFDefinition(
+            name="test1",
+            shape=shape1,
+        )
+        root = ET.Element("robot", {"name": f"{defn1.name}_robot"})
+        link = ET.SubElement(root, "link", {"name": defn1.name + "_base_link"})
+
+        self.assertEqual(0, len(link.findall("visual")))
+
+        # Call the method
+        defn1.add_visual_elements_to(link)
+
+        print(ET.tostring(root).decode())
+
+        # Check on the elements on the link; there should be:
+        # - an element with the visual tag
+        # - an element with the box tag exists
+
+        self.assertEqual(1, len(link.findall("visual")))
+        self.assertEqual(1, len(link.findall("visual/geometry/box")))
+
+
+if __name__ == '__main__':
+    unittest.main()
diff --git a/tests/motion_planning/algorithms/motion_planner_test.py b/tests/motion_planning/algorithms/motion_planner_test.py
index 9238ecb..8df0d83 100644
--- a/tests/motion_planning/algorithms/motion_planner_test.py
+++ b/tests/motion_planning/algorithms/motion_planner_test.py
@@ -22,6 +22,8 @@
     drakeify_my_urdf,
 )
 from brom_drake.scenes.debug import ShowMeThisModel
+from brom_drake.file_manipulation.urdf.DrakeReadyURDFConverter import MeshReplacementStrategy
+from brom_drake.utils import AddGround
 
 
 class MotionPlannerTest(unittest.TestCase):
@@ -81,8 +83,9 @@ def check_collision_in_config1(
         # Check for Collisions (and investigate scene) using query object from SceneGraph
         query = scene_graph.get_query_output_port().Eval(scene_graph_context)
         print("Play with the query object here!")
+        print(f"Collision exist in query? {query.HasCollisions()}")
         inspector = scene_graph.model_inspector()
-        print(query.ComputeSignedDistancePairwiseClosestPoints())
+        # print(query.ComputeSignedDistancePairwiseClosestPoints())
         for ii, pair_ii in enumerate(query.ComputeSignedDistancePairwiseClosestPoints()):
             if pair_ii.distance < 0.0:
                 print(pair_ii)
@@ -118,9 +121,9 @@ def test_check_collision_in_config1(self):
             impresources.files(robots) / "models/ur/ur10e.urdf"
         )
         q0 = np.array([
-            0.95*(np.pi/2.0),
-            (-5/4.0)*(np.pi/4.0),
-            +np.pi / 2.0,
+            0.65*(np.pi/2.0), # 0.95*(np.pi/2.0),
+            (-6/4.0)*(np.pi/4.0),
+            -0.25 + np.pi / 2.0,
             1.5*(-np.pi/2.0),
             0.0,
             0.0,
@@ -131,6 +134,7 @@ def test_check_collision_in_config1(self):
             urdf_file_path,
             overwrite_old_logs=True,
             log_file_name="drakeify-my-urdf1.log",
+            collision_mesh_replacement_strategy=MeshReplacementStrategy.kWithMinimalEnclosingCylinder,
         )
 
         # Visualize the URDF using the "show-me-this-model" feature
diff --git a/tests/motion_planning/algorithms/rrt/base_test.py b/tests/motion_planning/algorithms/rrt/base_test.py
index fe005d6..c50ead5 100644
--- a/tests/motion_planning/algorithms/rrt/base_test.py
+++ b/tests/motion_planning/algorithms/rrt/base_test.py
@@ -15,7 +15,7 @@
 from pydrake.systems.framework import DiagramBuilder
 
 # Internal Imports
-from brom_drake.motion_planning.algorithms.rrt.base import BaseRRTPlanner as BaseRRTPlanner
+from brom_drake.motion_planning.algorithms.rrt.base import BaseRRTPlanner, BaseRRTPlannerConfig
 import brom_drake.robots as robots
 from brom_drake.file_manipulation.urdf import drakeify_my_urdf
 
@@ -55,7 +55,7 @@ def test_joint_limits1(self):
         base_rrt = BaseRRTPlanner(model_idcs[0], plant, scene_graph)
         joint_limits = base_rrt.joint_limits
 
-        print("Joint limits:", joint_limits)
+        # print("Joint limits:", joint_limits)
 
         self.assertEqual(
             joint_limits.shape[1], 2,
@@ -131,6 +131,12 @@ def test_plan1(self):
             time_step=1e-3,
         )
 
+        config = BaseRRTPlannerConfig(
+            steering_step_size=0.1,
+            prob_sample_goal=0.05,
+            max_iterations=int(2e3),
+        )
+
         # Add the UR10e
         urdf_file_path = str(
             impresources.files(robots) / "models/ur/ur10e.urdf"
@@ -150,7 +156,12 @@ def test_plan1(self):
         plant.Finalize()
 
         # Create a BaseRRTPlanner instance
-        base_rrt = BaseRRTPlanner(model_idcs[0], plant, scene_graph)
+        base_rrt = BaseRRTPlanner(
+            model_idcs[0],
+            plant,
+            scene_graph,
+            config=config,
+        )
 
         diagram = builder.Build()
         diagram_context = diagram.CreateDefaultContext()
@@ -158,7 +169,7 @@ def test_plan1(self):
         base_rrt.root_context = diagram_context
 
         # Define start and goal configurations
-        start_configuration = np.zeros(base_rrt.dim_q)
+        start_configuration = np.ones(base_rrt.dim_q)*0.1
         goal_configuration = np.ones(base_rrt.dim_q)
 
         # Plan
@@ -169,10 +180,16 @@ def test_plan1(self):
 
         # Check if the distance from the last node to the goal is less than the distance from the start
         last_node = rrt.nodes[len(rrt.nodes)-1]
-        distance_to_goal = np.linalg.norm(last_node['q'] - goal_configuration)
+        min_dist_to_goal = np.inf
+        for node_idx in rrt.nodes:
+            node_ii = rrt.nodes[node_idx]
+            dist_to_goal = np.linalg.norm(node_ii['q'] - goal_configuration)
+            if dist_to_goal < min_dist_to_goal:
+                min_dist_to_goal = dist_to_goal
+
         distance_to_start = np.linalg.norm(start_configuration - goal_configuration)
 
-        self.assertLess(distance_to_goal, distance_to_start, "Last node should be closer to the goal than the start.")
+        self.assertLess(min_dist_to_goal, distance_to_start, "Last node should be closer to the goal than the start.")
 
 if __name__ == "__main__":
     unittest.main()
\ No newline at end of file
diff --git a/tests/motion_planning/algorithms/rrt/connect_test.py b/tests/motion_planning/algorithms/rrt/connect_test.py
new file mode 100644
index 0000000..c82b9a7
--- /dev/null
+++ b/tests/motion_planning/algorithms/rrt/connect_test.py
@@ -0,0 +1,173 @@
+from brom_drake.all import drakeify_my_urdf
+import brom_drake.robots as robots
+import importlib.resources as impresources
+import networkx as nx
+import numpy as np
+from pydrake.all import (
+    AddMultibodyPlantSceneGraph,
+    Parser, DiagramBuilder,
+)
+import unittest
+
+# Internal Imports
+from brom_drake.motion_planning.algorithms.rrt.connect import (
+    RRTConnectPlannerConfig,
+    RRTConnectPlanner,
+)
+
+class TestRRTConnect(unittest.TestCase):
+    def setUp(self) -> None:
+        # Setup
+        self.builder1 = DiagramBuilder()
+
+        # Create a config
+        self.config1 = RRTConnectPlannerConfig(
+            steering_step_size=0.025,
+            prob_sample_goal=0.25,
+            max_iterations=int(1e4),
+            convergence_threshold=1e-3,
+        )
+
+        # Create a plant and scene graph
+        self.plant1, self.scene_graph1 = AddMultibodyPlantSceneGraph(
+            self.builder1,
+            time_step=1e-3,
+        )
+
+        # Collect an arm model 
+        original_arm_urdf_path = str(
+            impresources.files(robots) / "models/ur/ur10e.urdf",
+        )
+        arm_urdf = drakeify_my_urdf(
+            original_arm_urdf_path,
+            overwrite_old_logs=True,
+            log_file_name="drakeify-my-urdf1.log",
+        )
+        self.arm_model_index = Parser(self.plant1).AddModels(str(arm_urdf))[0]
+
+        # Weld arm to world
+        self.plant1.WeldFrames(
+            self.plant1.world_frame(),
+            self.plant1.GetFrameByName("base_link", self.arm_model_index),
+        )
+
+        # Define RRTConnectPlanner # 1
+        self.planner1 = RRTConnectPlanner(
+            self.arm_model_index,
+            self.plant1,
+            self.scene_graph1,
+            config=self.config1,
+        )
+
+    def test_dim_q1(self):
+        # Setup
+
+        # Finalize plant
+        self.plant1.Finalize()
+
+        # Get the dimension of the robot's configuration space
+        self.assertEqual(self.planner1.dim_q, 6)
+
+    def test_find_nearest_node1(self):
+        # Setup
+        q_rand = np.array([0.0, 0.0, 0.2, 0.0, 0.0, 0.0])
+
+        # Define RRTConnectPlanner
+        planner = RRTConnectPlanner(
+            self.arm_model_index,
+            self.plant1,
+            self.scene_graph1,
+            config=self.config1,
+        )
+
+        # Finalize plant
+        self.plant1.Finalize()
+
+        # Create a digraph for the RRT
+        rrt = nx.DiGraph()
+        rrt.add_node(0, q=np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]))
+        rrt.add_node(1, q=np.array([0.0, 0.0, 0.1, 0.0, 0.0, 0.0]))
+
+        # Find the nearest node
+        nearest_node, nearest_node_idx = planner.find_nearest_node(rrt, q_rand)
+
+        # Check
+        self.assertEqual(nearest_node_idx, 1)
+
+    def test_steer1(self):
+        """
+        Test the steering functionality.
+        In this test, we will specifically check that the steering function properly returns
+        a configuration that is closer to the target configuration than the initial one.
+        We will make sure that the steering function DOES not return the goal when it is too far away.
+        """
+        # Setup
+        q_current = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+        q_target = np.array([0.0, 0.0, 0.2, 0.0, 0.0, 0.0])
+
+        # Define RRTConnectPlanner
+        planner = RRTConnectPlanner(
+            self.arm_model_index,
+            self.plant1,
+            self.scene_graph1,
+            config=self.config1,
+        )
+
+        # Finalize plant
+        self.plant1.Finalize()
+
+        # Steer
+        q_new = planner.steer(q_current, q_target)
+
+        # Check that
+        # 1. The new configuration is closer to the target than the current configuration
+        # 2. The new configuration is not the target configuration
+        self.assertLess(np.linalg.norm(q_new - q_target), np.linalg.norm(q_current - q_target))
+        self.assertFalse(np.isclose(q_new, q_target).all())
+
+    def test_plan1(self):
+        """
+        Description
+        -----------
+        Test the planning functionality.
+        We will define a simple planning problem where the start
+        and goal configurations are close AND in an obstacle-free space.
+        Connect should rapidly find a solution.
+        """
+        # Setup
+        start_configuration = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+        goal_configuration = np.array([0.0, 0.0, 0.2, 0.0, 0.0, 0.0])
+
+        config2 = RRTConnectPlannerConfig(
+            steering_step_size=0.025,
+            prob_sample_goal=0.99,
+            max_iterations=int(1e4),
+            convergence_threshold=1e-3,
+        )
+
+        # Define RRTConnectPlanner
+        planner = RRTConnectPlanner(
+            self.arm_model_index,
+            self.plant1,
+            self.scene_graph1,
+            config=self.config1,
+        )
+
+        # Finalize plant + build
+        self.plant1.Finalize()
+        diagram = self.builder1.Build()
+        diagram_context = diagram.CreateDefaultContext()
+
+        planner.root_context = diagram_context
+
+        # Plan
+        rrt, success = planner.plan(start_configuration, goal_configuration)
+
+        # Check
+        # - Path was successfully found
+        # - Path is not empty
+        self.assertTrue(success)
+        self.assertTrue(len(rrt.edges) > 0)
+
+if __name__ == '__main__':
+    unittest.main()
\ No newline at end of file
diff --git a/tests/motion_planning/systems/prototypical_planner_test.py b/tests/motion_planning/systems/prototypical_planner_test.py
index e8d45b5..92452ad 100644
--- a/tests/motion_planning/systems/prototypical_planner_test.py
+++ b/tests/motion_planning/systems/prototypical_planner_test.py
@@ -5,11 +5,13 @@
 system that I've defined in the motion_planning/systems/prototypical_planner.py file.
 """
 from importlib import resources as impresources
-
-from pydrake.multibody.parsing import Parser
-from pydrake.multibody.plant import AddMultibodyPlantSceneGraph
-from pydrake.systems.analysis import Simulator
-from pydrake.systems.framework import DiagramBuilder
+import numpy as np
+from pydrake.all import (
+    Parser, DiagramBuilder,
+    AddMultibodyPlantSceneGraph,
+    RigidTransform, RollPitchYaw,
+    Simulator,
+)
 import unittest
 
 from brom_drake.file_manipulation.urdf import drakeify_my_urdf
@@ -48,6 +50,57 @@ def setUp(self):
         # Add the UR10e to the plant
         self.arm = Parser(self.plant).AddModels(str(new_urdf_path))[0]
 
+    def test_check_collision_in_config1(self):
+        """
+        Description
+        -----------
+        This test checks the prototypical planner's ability to correctly identify when
+        a configuration is in collision.
+        We will place the arm in a known collision configuration with
+        the shelf placed in the way.
+        """
+        # Setup
+        q_collision = np.array([0.0, 0.0, -np.pi/4.0, 0.0, 0.0, 0.0])
+
+        bad_shelf_position = np.array([0.5, 0.0, 0.0])
+        bad_shelf_orientation = RollPitchYaw(np.pi/2.0, 0.0, 0.0)
+        bad_shelf_pose = RigidTransform(bad_shelf_orientation, bad_shelf_position)
+
+        # Create the Scene
+        scene1 = ShelfPlanningScene(
+            meshcat_port_number=None,
+            start_config=q_collision,
+            goal_config=q_collision,
+            shelf_pose=bad_shelf_pose,
+        )
+
+        # Cast all secondary scene members
+        scene1.add_all_secondary_cast_members_to_builder()
+
+        # Create planner with the now finalized arm
+        planner1 = BaseRRTPlanner(
+            scene1.arm,
+            scene1.plant,
+            scene1.scene_graph,
+        )
+
+        system1 = PrototypicalPlannerSystem(
+            scene1.plant,
+            scene1.scene_graph,
+            planner1.plan,
+            robot_model_idx=scene1.arm,
+        )
+
+        # Build scene
+        scene1.fill_role(scene1.suggested_roles()[0], system1)
+        diagram, diagram_context = scene1.build_scene()
+        
+        system1.set_internal_root_context(diagram_context)
+
+        # Check collision
+        in_collision = system1.check_collision_in_config(q_collision)
+        self.assertTrue(in_collision)
+
     def test_init1(self):
         """
         Description
@@ -64,6 +117,7 @@ def test_init1(self):
         prototypical_planner = PrototypicalPlannerSystem(
             self.plant, self.scene_graph,
             planner1,
+            robot_model_idx=self.arm,
         )
 
         self.assertTrue(True)
@@ -77,9 +131,18 @@ def test_use_in_mp_scene1(self):
         :return:
         """
         # Setup
+        q_easy_start = np.array([0.0, 0.0, -np.pi/4.0, 0.0, 0.0, 0.0])
+        q_easy_goal = np.array([0.0, 0.0, -np.pi/8.0, 0.0, 0.0, 0.0])
         scene1 = ShelfPlanningScene(
-            meshcat_port_number=7002,
+            meshcat_port_number=None,
+            start_config=q_easy_start,
+            goal_config=q_easy_goal,
         )
+
+        # Cast all secondary scene members
+        scene1.add_all_secondary_cast_members_to_builder()
+
+        # Create planner with the now finalized arm
         planner2 = BaseRRTPlanner(
             scene1.arm,
             scene1.plant,
@@ -88,21 +151,31 @@ def test_use_in_mp_scene1(self):
 
         # Create the System
         prototypical_planner = PrototypicalPlannerSystem(
-            scene1.plant, scene1.scene_graph,
+            scene1.plant,
+            scene1.scene_graph,
             planner2.plan,
+            robot_model_idx=scene1.arm,
         )
 
         # Add the prototypical planner to the scene
         role1 = scene1.suggested_roles()[0]
-        diagram, diagram_context = scene1.cast_scene_and_build([
-            (role1, prototypical_planner),
-        ])
+        scene1.fill_role(role1, prototypical_planner)
+
+        diagram, diagram_context = scene1.build_scene()
+
+        # Add the connections that we need for the performer
+        prototypical_planner.set_internal_root_context(
+            diagram_context
+        )
 
         # Create simulator
-        simulator = Simulator(diagram, diagram_context)
-        simulator.set_target_realtime_rate(1.0)
-        simulator.Initialize()
-        simulator.AdvanceTo(1.0)
+        # simulator = Simulator(diagram, diagram_context)
+        # simulator.set_target_realtime_rate(1.0)
+        # simulator.Initialize()
+        
+        self.assertTrue(True) # Scene built successfully, which is good.
+
+        #TODO(kwesi): Test that the simulation works for some simple case...
 
 if __name__ == '__main__':
     unittest.main()
\ No newline at end of file
diff --git a/tests/motion_planning/systems/rrt_plan_generator_test.py b/tests/motion_planning/systems/rrt_plan_generator_test.py
index b957744..1d606cc 100644
--- a/tests/motion_planning/systems/rrt_plan_generator_test.py
+++ b/tests/motion_planning/systems/rrt_plan_generator_test.py
@@ -46,16 +46,10 @@ def create_example_scene1() -> Tuple[
         # Setup
         builder = DiagramBuilder()
 
-        # Create start pose
-        start_position = np.array([-0.2, 0.9, 0.3])
-        start_orientation = Quaternion(1, 0, 0, 0)
-        p_WStart = RigidTransform(start_orientation, start_position)
-
-        # Create goal pose
-        goal_position = np.array([+0.2, 1.0, 0.65])
-        goal_orientation = Quaternion(1, 0, 0, 0)
-        p_WGoal = RigidTransform(goal_orientation, goal_position)
-
+        # Create start and goal configurations
+        start_configuration = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+        goal_configuration = np.array([0.0, 0.0, -np.pi/4.0, 0.0, 0.0, 0.0])
+        
         # Create a MultibodyPlant
         plant, scene_graph = AddMultibodyPlantSceneGraph(
             builder,
@@ -81,14 +75,10 @@ def create_example_scene1() -> Tuple[
         plant.Finalize()
 
         # Create start and goal sources
-        start_source = ConstantVectorSource(
-            np.hstack((start_position, start_orientation.wxyz()))
-        )
+        start_source = ConstantVectorSource(start_configuration)
         builder.AddSystem(start_source)
 
-        goal_source = ConstantVectorSource(
-            np.hstack((goal_position, goal_orientation.wxyz()))
-        )
+        goal_source = ConstantVectorSource(goal_configuration)
         builder.AddSystem(goal_source)
 
         # Also create a source which tells the planner what the model index
@@ -128,17 +118,18 @@ def test_plan1(self):
             scene_graph,
             model_idcs[0],
             rrt_config=rrt_config,
+            dim_config=6,
         )
         builder.AddSystem(plan_generator)
 
         # Connect the start and goal sources to the RRTPlanGenerator
         builder.Connect(
             start_source.get_output_port(0),
-            plan_generator.GetInputPort("start_pose"),
+            plan_generator.GetInputPort("start_configuration"),
         )
         builder.Connect(
             goal_source.get_output_port(0),
-            plan_generator.GetInputPort("goal_pose"),
+            plan_generator.GetInputPort("goal_configuration"),
         )
 
         # Connect the robot model index to the plan_generator
@@ -166,8 +157,6 @@ def test_plan1(self):
 
         # Simulate for a short time
         simulator = Simulator(diagram, diagram_context)
-        plant_context = plant.GetMyContextFromRoot(diagram_context)
-
         plan_generator.set_internal_root_context(diagram_context)
 
         simulator.Initialize()
diff --git a/tests/scenes/motion_planning/offline/chem_lab1_test.py b/tests/scenes/motion_planning/offline/chem_lab1_test.py
new file mode 100644
index 0000000..0f7e06b
--- /dev/null
+++ b/tests/scenes/motion_planning/offline/chem_lab1_test.py
@@ -0,0 +1,105 @@
+import unittest
+import numpy as np
+from pydrake.systems.analysis import Simulator
+
+# Internal Imports
+from brom_drake.motion_planning.systems.prototypical_planner import PrototypicalPlannerSystem
+from brom_drake.motion_planning.systems.rrt_plan_generator import RRTPlanGenerator
+from brom_drake.scenes.motion_planning.offline.chem_lab1 import ChemLab1Scene
+from brom_drake.motion_planning.algorithms.rrt.base import BaseRRTPlanner
+
+class ChemLab1Test(unittest.TestCase):
+    def test_add_all_secondary_cast_members_to_builder1(self):
+        # Setup
+        scene = ChemLab1Scene(meshcat_port_number=None)
+
+        # Call the method
+        scene.add_all_secondary_cast_members_to_builder()
+
+        # Build the diagram
+        diagram = scene.builder.Build()
+        diagram_context = diagram.CreateDefaultContext()
+
+        # Simulate the diagram
+        simulator = Simulator(diagram, diagram_context)
+        simulator.set_target_realtime_rate(1.0)
+        simulator.Initialize()
+        simulator.AdvanceTo(0.010)
+
+        self.assertTrue(True)
+
+    def test_cast_scene_and_build1(self):
+        """
+        Description
+        -----------
+        This test checks the prototypical planner's ability to plan a motion
+        from a start pose to a goal pose.
+        :return:
+        """
+        # Setup
+        q_easy_start = np.array([0.0, 0.0, -np.pi/4.0, 0.0, 0.0, 0.0])
+        q_easy_goal = np.array([0.0, 0.0, -np.pi/8.0, 0.0, 0.0, 0.0])
+        scene1 = ChemLab1Scene(
+            meshcat_port_number=None,
+            start_config=q_easy_start,
+            goal_config=q_easy_goal,
+        )
+
+        # Create the Planning System
+        planning_system = RRTPlanGenerator(
+            scene1.plant,
+            scene1.scene_graph,
+            scene1.robot_model_idx_,
+            dim_config=6,
+        )
+
+        # Add the prototypical planner to the scene
+        role1 = scene1.suggested_roles()[0]
+        diagram, diagram_context = scene1.cast_scene_and_build(
+            cast=[(role1, planning_system)],
+        )
+
+        # Create simulator
+        simulator = Simulator(diagram, diagram_context)
+        simulator.set_target_realtime_rate(1.0)
+        simulator.Initialize()
+        
+        self.assertTrue(True) # Scene built successfully, which is good.
+
+    def test_easy_cast_scene_and_build1(self):
+        # Setup
+        easy_start_config = np.array([
+            0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+        ])
+        easy_goal_config = np.array([
+            0.0, 0.0, np.pi/8., 0.0, 0.0, 0.0
+        ])
+        scene = ChemLab1Scene(
+            meshcat_port_number=None,
+            start_configuration=easy_start_config,
+            goal_configuration=easy_goal_config,
+            )
+
+        # Create planner with the now finalized arm
+        planner1 = BaseRRTPlanner(
+            scene.arm,
+            scene.plant,
+            scene.scene_graph,
+        )
+
+        # Call the method
+        diagram, diagram_context = scene.easy_cast_and_build(
+            planner1.plan,
+        )
+
+        # Simulate the diagram
+        simulator = Simulator(diagram, diagram_context)
+        simulator.set_target_realtime_rate(1.0)
+        simulator.Initialize()
+        simulator.AdvanceTo(0.010)
+
+        self.assertTrue(True)
+
+
+if __name__ == '__main__':
+    unittest.main()
\ No newline at end of file
diff --git a/tests/scenes/motion_planning/offline/shelf_test.py b/tests/scenes/motion_planning/offline/shelf_test.py
index f6fa0d0..708b10f 100644
--- a/tests/scenes/motion_planning/offline/shelf_test.py
+++ b/tests/scenes/motion_planning/offline/shelf_test.py
@@ -4,7 +4,7 @@
 
 from brom_drake.scenes import SceneID
 from brom_drake.scenes.motion_planning.offline.shelf import ShelfPlanningScene
-from brom_drake.scenes.roles import kOfflineMotionPlanner
+from brom_drake.scenes.roles import kKinematicMotionPlanner
 
 
 class ShelfTest(unittest.TestCase):
@@ -19,7 +19,7 @@ def test_add_all_secondary_cast_members_to_builder1(self):
 
         # Check that defaults are in place
         self.assertEqual(shelf_planning_scene.id, SceneID.kShelfPlanning1)
-        self.assertEqual(shelf_planning_scene.suggested_roles(), [kOfflineMotionPlanner])
+        self.assertEqual(shelf_planning_scene.suggested_roles(), [kKinematicMotionPlanner])
 
         # Populate scene with builder
         shelf_planning_scene.add_all_secondary_cast_members_to_builder()
diff --git a/tests/scenes/roles/offline_motion_planner_test.py b/tests/scenes/roles/offline_motion_planner_test.py
index 8893aea..f529a80 100644
--- a/tests/scenes/roles/offline_motion_planner_test.py
+++ b/tests/scenes/roles/offline_motion_planner_test.py
@@ -1,6 +1,6 @@
 import unittest
 
-from brom_drake.scenes.roles.offline_motion_planner import kOfflineMotionPlanner
+from brom_drake.scenes.roles.motion_planners.kinematic import kKinematicMotionPlanner
 from brom_drake.scenes.roles.role_port_assignment import PairingType
 
 
@@ -13,10 +13,10 @@ def test_required_input_ports1(self):
         :return:
         """
         # Setup
-        omp_role = kOfflineMotionPlanner
+        omp_role = kKinematicMotionPlanner
         expected_port_names = [
-            "start_pose",
-            "goal_pose",
+            "start_configuration",
+            "goal_configuration",
             "scene_id",
         ]
 
@@ -36,7 +36,7 @@ def test_required_output_ports1(self):
         :return:
         """
         # Check
-        omp_role = kOfflineMotionPlanner
+        omp_role = kKinematicMotionPlanner
         expected_port_names = ["motion_plan", "plan_is_ready"]
 
         # Assertion