diff --git a/irsim/env/env_base.py b/irsim/env/env_base.py
index ecade671..9ab4f069 100644
--- a/irsim/env/env_base.py
+++ b/irsim/env/env_base.py
@@ -3,12 +3,11 @@
 Author: Ruihua Han (hanrh@connect.hku.hk)
 '''
 
-import yaml
 import matplotlib
 
 matplotlib.use("TkAgg")
 from irsim.env.env_config import EnvConfig
-from irsim.world import world
+from irsim.world import World
 from .env_plot import EnvPlot
 from irsim.global_param import world_param, env_param
 from irsim.world.object_factory import ObjectFactory
@@ -58,9 +57,9 @@ def __init__(
 
         self.env_config = EnvConfig(world_name)
         object_factory = ObjectFactory()
-
         # init objects (world, obstacle, robot)
-        self._world = world(world_name, **self.env_config.parse["world"])
+        
+        self._world = World(world_name, **self.env_config.parse["world"])
 
         self._robot_collection = object_factory.create_from_parse(
             self.env_config.parse["robot"], "robot"
@@ -71,9 +70,6 @@ def __init__(
         self._map_collection = object_factory.create_from_map(
             self._world.obstacle_positions, self._world.buffer_reso
         )
-        self._object_collection = (
-            self._robot_collection + self._obstacle_collection + self._map_collection
-        )
 
         # env parameters
         self._env_plot = EnvPlot(
@@ -83,6 +79,7 @@ def __init__(
             self._world.y_range,
             **self.env_config.parse["plot"],
         )
+
         env_param.objects = self.objects
 
         if world_param.control_mode == "keyboard":
@@ -609,7 +606,7 @@ def obstacle_list(self):
 
     @property
     def objects(self):
-        return self._object_collection
+        return self._robot_collection + self._obstacle_collection + self._map_collection
 
     @property
     def step_time(self):
diff --git a/irsim/env/env_base3d.py b/irsim/env/env_base3d.py
index 4d8ccab3..7303cdb8 100644
--- a/irsim/env/env_base3d.py
+++ b/irsim/env/env_base3d.py
@@ -1,19 +1,51 @@
 from irsim.env import EnvBase
 from .env_plot3d import EnvPlot3D
+from irsim.world.object_factory3d import ObjectFactory3D
+from irsim.world.world3d import World3D
+from irsim.global_param import world_param, env_param
+from irsim.world.object_base import ObjectBase
+import itertools
 
 class EnvBase3D(EnvBase):
 
     def __init__(self, world_name, **kwargs):
+
         super().__init__(world_name, **kwargs)
 
+        object_factory = ObjectFactory3D()
+
+        self._world = World3D(world_name, **self.env_config.parse["world"])
+
+        ObjectBase.id_iter = itertools.count()
+
+        self._robot_collection = object_factory.create_from_parse(
+            self.env_config.parse["robot"], "robot"
+        )
+        self._obstacle_collection = object_factory.create_from_parse(
+            self.env_config.parse["obstacle"], "obstacle"
+        )
+        self._map_collection = object_factory.create_from_map(
+            self._world.obstacle_positions, self._world.buffer_reso
+        )
+
+        self._env_plot.close()
+
         self._env_plot = EnvPlot3D(
             self._world.grid_map,
             self.objects,
             self._world.x_range,
             self._world.y_range,
+            self._world.z_range,
             **self.env_config.parse["plot"],
         )
 
+        env_param.objects = self.objects
+
+
+
+
+
+
 
 
     
diff --git a/irsim/env/env_plot.py b/irsim/env/env_plot.py
index 23a179c3..87eee37b 100644
--- a/irsim/env/env_plot.py
+++ b/irsim/env/env_plot.py
@@ -318,6 +318,12 @@ def show(self):
         """
         plt.show()
 
+    def close(self):
+        """
+        Close the plot.
+        """
+        plt.close()
+
 
 def linewidth_from_data_units(linewidth, axis, reference='y'):
     """
@@ -350,3 +356,40 @@ def linewidth_from_data_units(linewidth, axis, reference='y'):
     length *= 72
     # Scale linewidth to value range
     return linewidth * (length / value_range)
+
+
+
+def linewidth_from_data_units_3d(linewidth, axis, reference='z'):
+    """
+    Convert a linewidth in data units to linewidth in points.
+
+    Parameters
+    ----------
+    linewidth: float
+        Linewidth in data units of the respective reference-axis
+    axis: matplotlib axis
+        The axis which is used to extract the relevant transformation
+        data (data limits and size must not change afterwards)
+    reference: string
+        The axis that is taken as a reference for the data width.
+        Possible values: 'x' and 'y'. Defaults to 'y'.
+
+    Returns
+    -------
+    linewidth: float
+        Linewidth in points
+    """
+    fig = axis.get_figure()
+    if reference == 'x':
+        length = fig.bbox_inches.width * axis.get_position().width
+        value_range = np.diff(axis.get_xlim())
+    elif reference == 'y':
+        length = fig.bbox_inches.height * axis.get_position().height
+        value_range = np.diff(axis.get_ylim())
+    elif reference == 'z':
+        length = fig.bbox_inches.height * axis.get_position().height
+        value_range = np.diff(axis.get_zlim())
+    # Convert length to points
+    length *= 72
+    # Scale linewidth to value range
+    return linewidth * (length / value_range)
diff --git a/irsim/env/env_plot3d.py b/irsim/env/env_plot3d.py
index 07e7d601..04388dbd 100644
--- a/irsim/env/env_plot3d.py
+++ b/irsim/env/env_plot3d.py
@@ -1,35 +1,30 @@
 from .env_plot import EnvPlot
-import matplotlib.pyplot as plt
 
 
 class EnvPlot3D(EnvPlot):
 
     def __init__(
-        self,
-        grid_map=None,
+        self,grid_map=None,
         objects=[],
         x_range=[0, 10],
         y_range=[0, 10],
+        z_range=[0, 10],
         saved_figure=dict(),
         saved_ani=dict(),
         dpi: int = 100,
-        figure_pixels: list = [1920, 1080],
+        figure_pixels: list =[1920, 1080],
         **kwargs,
     ):
-        super().__init__(
-            grid_map,
-            objects,
-            x_range,
-            y_range,
-            saved_figure,
-            saved_ani,
-            dpi,
-            figure_pixels,
-            **kwargs,
-        )
-
-        self.fig, self.ax = plt.subplots(
-            figsize=(figure_pixels[0] / dpi, figure_pixels[1] / dpi),
-            dpi=dpi,
-            subplot_kw={"projection": "3d"},
-        )
+        super().__init__(grid_map, objects, x_range, y_range, saved_figure, saved_ani, dpi, figure_pixels, **kwargs)
+        
+        self.clear_components()
+        self.ax = self.fig.add_subplot(111, projection='3d')
+        
+        self.init_plot(grid_map, objects, **kwargs)
+        self.ax.set_zlim(z_range)
+
+        
+
+
+
+
diff --git a/irsim/usage/04collision_world/collision_world.yaml b/irsim/usage/04collision_world/collision_world.yaml
index db816d06..b86ba781 100644
--- a/irsim/usage/04collision_world/collision_world.yaml
+++ b/irsim/usage/04collision_world/collision_world.yaml
@@ -20,9 +20,8 @@ obstacle:
   - shape: {name: 'circle', radius: 1.0}  # radius
     state: [5, 5, 0]  
   
-  
-  - shape: {name: 'rectangle', length: 1.5, width: 1.2}  # radius
-    state: [6, 5, 1] 
+  # - shape: {name: 'rectangle', length: 1.5, width: 1.2}  # radius
+  #   state: [6, 5, 1] 
 
   # - shape: {name: 'circle', radius: 0.4}  # radius
   #   state: [1, 1, 0] 
\ No newline at end of file
diff --git a/irsim/usage/05lidar_world/lidar_world.py b/irsim/usage/05lidar_world/lidar_world.py
index 70d298a9..9af7d07c 100644
--- a/irsim/usage/05lidar_world/lidar_world.py
+++ b/irsim/usage/05lidar_world/lidar_world.py
@@ -1,7 +1,7 @@
 import irsim
 
 # env = irsim.make('lidar_world.yaml')
-env = irsim.make('lidar_world_noise.yaml')
+env = irsim.make('lidar_world_noise.yaml', projection='3d')
 
 for i in range(300):
 
diff --git a/irsim/usage/14world_3d/car_world.yaml b/irsim/usage/14world_3d/car_world.yaml
new file mode 100644
index 00000000..3795655d
--- /dev/null
+++ b/irsim/usage/14world_3d/car_world.yaml
@@ -0,0 +1,27 @@
+world:
+  height: 50  # the height of the world
+  width: 50   # the height of the world
+  step_time: 0.1  # 10Hz calculate each step
+  sample_time: 0.1  # 10 Hz for render and data extraction 
+  offset: [0, 0] # the offset of the world on x and y 
+
+robot:  
+  - kinematics: {name: 'acker'}  # omni, diff, acker
+    shape: {name: 'rectangle', length: 4.6, width: 1.6, wheelbase: 3}
+    state: [1, 1, 0, 0]
+    goal: [40, 40, 0]
+    vel_max: [4, 1]
+    behavior: {name: 'dash'} # move toward to the goal directly
+    plot:
+      show_trajectory: True
+      
+  
+    
+  
+    
+    
+
+
+
+  
+
diff --git a/irsim/usage/14world_3d/robot_omni_world.yaml b/irsim/usage/14world_3d/robot_omni_world.yaml
new file mode 100644
index 00000000..f4684c30
--- /dev/null
+++ b/irsim/usage/14world_3d/robot_omni_world.yaml
@@ -0,0 +1,29 @@
+world:
+  height: 10  # the height of the world
+  width: 10   # the height of the world
+  step_time: 0.1  # 10Hz calculate each step
+  sample_time: 0.1  # 10 Hz for render and data extraction 
+  offset: [0, 0] # the offset of the world on x and y 
+
+robot:
+  - kinematics: {name: 'omni'}  # omni, diff, acker
+    # shape: {name: 'circle', radius: 0.2}  # radius
+    shape: {name: 'rectangle', length: 0.3, width: 1.0} 
+    state: [1, 1, 0]  
+    goal: [9, 9, 0] 
+    # acce: [3, .inf]   # acce of [linear, angular]  or [v_x, v_y] or [linear, steer]
+    behavior: {name: 'dash'} # move toward to the goal directly 
+    color: 'g'
+    state_dim: 3
+    plot:
+      show_trajectory: True
+    # description: 'diff_robot0.png'
+    
+  
+    
+    
+
+
+
+  
+
diff --git a/irsim/usage/14world_3d/robot_polygon_world.yaml b/irsim/usage/14world_3d/robot_polygon_world.yaml
new file mode 100644
index 00000000..12a94237
--- /dev/null
+++ b/irsim/usage/14world_3d/robot_polygon_world.yaml
@@ -0,0 +1,26 @@
+world:
+  height: 10  # the height of the world
+  width: 10   # the height of the world
+  step_time: 0.1  # 10Hz calculate each step
+  sample_time: 0.1  # 10 Hz for render and data extraction 
+  offset: [0, 0] # the offset of the world on x and y 
+
+robot:
+  kinematics: {name: 'diff'}  # omni, diff, acker
+  shape: {name: 'polygon', vertices: [[0.1, 0.2], [-0.1, 0.3], [0, -0.3], [0.3, -0.2]]}  # radius
+  behavior: {name: 'dash'} # move toward to the goal directly 
+  state: [1, 1, 0]  
+  goal: [9, 9, 0] 
+  # acce: [3, .inf]   # acce of [linear, angular]  or [v_x, v_y] or [linear, steer]
+  color: 'g'
+  plot:
+    show_trajectory: True
+    
+  
+    
+    
+
+
+
+  
+
diff --git a/irsim/usage/14world_3d/robot_world_3d.py b/irsim/usage/14world_3d/robot_world_3d.py
index b397406d..37083e9a 100644
--- a/irsim/usage/14world_3d/robot_world_3d.py
+++ b/irsim/usage/14world_3d/robot_world_3d.py
@@ -1,6 +1,6 @@
 import irsim
 
-env = irsim.make(projection='3d')
+env = irsim.make('car_world.yaml', projection='3d')
 
 for i in range(1000):
 
@@ -10,4 +10,6 @@
     if env.done():
         break
 
+
+env.show()
 env.end()
diff --git a/irsim/usage/14world_3d/robot_world_3d.yaml b/irsim/usage/14world_3d/robot_world_3d.yaml
index 75b7b652..af2dd527 100644
--- a/irsim/usage/14world_3d/robot_world_3d.yaml
+++ b/irsim/usage/14world_3d/robot_world_3d.yaml
@@ -16,6 +16,7 @@ robot:
   color: 'g'
   plot:
     show_trajectory: True
+    show_trail: True
   # description: 'diff_robot0.png'
     
   
diff --git a/irsim/world/__init__.py b/irsim/world/__init__.py
index cd42fdb2..c01adc30 100644
--- a/irsim/world/__init__.py
+++ b/irsim/world/__init__.py
@@ -1,16 +1,29 @@
-from irsim.world.world import world
+from irsim.world.world import World
 from irsim.world.sensors.sensor_factory import SensorFactory
 
+
 from irsim.world.object_base import ObjectBase
 from irsim.world.robots.robot_diff import RobotDiff
 from irsim.world.robots.robot_acker import RobotAcker
 from irsim.world.robots.robot_omni import RobotOmni
 
+from irsim.world.object_base_3d import ObjectBase3D
+from irsim.world.robots.robot_diff_3d import RobotDiff3D
+from irsim.world.robots.robot_acker_3d import RobotAcker3D
+from irsim.world.robots.robot_omni_3d import RobotOmni3D
+
 from irsim.world.obstacles.obstacle_diff import ObstacleDiff
 from irsim.world.obstacles.obstacle_omni import ObstacleOmni
 from irsim.world.obstacles.obstacle_acker import ObstacleAcker
 
+from irsim.world.obstacles.obstacle_diff_3d import ObstacleDiff3D
+from irsim.world.obstacles.obstacle_omni_3d import ObstacleOmni3D
+from irsim.world.obstacles.obstacle_acker_3d import ObstacleAcker3D
+
 from irsim.world.map.obstacle_map import ObstacleMap
 
 from irsim.world.obstacles.obstacle_static import ObjectStatic
+from irsim.world.obstacles.obstacle_static_3d import ObjectStatic3D
+
+from irsim.world.object_factory import ObjectFactory
 
diff --git a/irsim/world/object_base.py b/irsim/world/object_base.py
index 4cfc3a7a..49b76780 100644
--- a/irsim/world/object_base.py
+++ b/irsim/world/object_base.py
@@ -15,6 +15,7 @@
 from matplotlib import image
 from typing import Optional
 
+
 from irsim.util.util import (
     WrapToRegion,
     relative_position,
@@ -254,6 +255,7 @@ def __init__(
 
         self.collision_obj = []
 
+
     def __eq__(self, o: object) -> bool:
         return self._id == o._id
 
@@ -584,6 +586,7 @@ def plot(self, ax, **kwargs):
         """
         self.state_re = self._state
         self.goal_re = self._goal
+        self.plot_patch_list
 
         self.plot_kwargs.update(kwargs)
 
@@ -912,6 +915,10 @@ def name(self):
     def shape(self):
         return self.gf.name
 
+    @property
+    def z(self):
+        return self._state[2, 0] if self.state_dim >= 6 else 0
+
     @property
     def kinematics(self):
         return self.kf.name if self.kf is not None else None
diff --git a/irsim/world/object_base3d.py b/irsim/world/object_base3d.py
deleted file mode 100644
index 9c7e9e86..00000000
--- a/irsim/world/object_base3d.py
+++ /dev/null
@@ -1 +0,0 @@
-from 
\ No newline at end of file
diff --git a/irsim/world/object_base_3d.py b/irsim/world/object_base_3d.py
new file mode 100644
index 00000000..1285299f
--- /dev/null
+++ b/irsim/world/object_base_3d.py
@@ -0,0 +1,205 @@
+from irsim.world import ObjectBase
+import matplotlib as mpl
+from matplotlib import transforms as mtransforms
+from matplotlib import image
+from irsim.global_param.path_param import path_manager
+from math import pi, atan2, cos, sin
+import mpl_toolkits.mplot3d.art3d as art3d
+import numpy as np
+
+class ObjectBase3D(ObjectBase):
+        
+        def __init__(self, **kwargs):
+            super().__init__(**kwargs)
+
+
+        def plot_object(self, ax, **kwargs):
+            """
+            Plot the object itself.
+
+            Args:
+                ax: Matplotlib axis.
+                **kwargs: Additional plotting options.
+            """
+            if self.description is None:
+                x = self.state_re[0, 0]
+                y = self.state_re[1, 0]
+
+                if self.shape == "circle":
+                    object_patch = mpl.patches.Circle(
+                        xy=(x, y), radius=self.radius, color=self.color
+                    )
+                    object_patch.set_zorder(3)
+                    ax.add_patch(object_patch)
+
+                    art3d.patch_2d_to_3d(object_patch, z=self.z)
+
+                elif self.shape == "polygon" or self.shape == "rectangle":
+                    object_patch = mpl.patches.Polygon(xy=self.vertices.T, color=self.color)
+                    object_patch.set_zorder(3)
+                    ax.add_patch(object_patch)
+
+                    art3d.patch_2d_to_3d(object_patch, z=self.z)
+
+                elif self.shape == "linestring":
+                    object_patch = mpl.lines.Line2D(
+                        self.vertices[0, :], self.vertices[1, :], color=self.color
+                    )
+                    object_patch.set_zorder(3)
+                    ax.add_line(object_patch)
+
+                elif self.shape == "map":
+                    return
+
+                self.plot_patch_list.append(object_patch)
+
+            else:
+                self.plot_object_image(ax, self.description, **kwargs)
+
+
+        def plot_goal(self, ax, goal_color="r"):
+            """
+            Plot the goal position of the object.
+
+            Args:
+                ax: Matplotlib axis.
+                goal_color (str): Color of the goal marker.
+            """
+            goal_x = self.goal_re[0, 0]
+            goal_y = self.goal_re[1, 0]
+
+            goal_circle = mpl.patches.Circle(
+                xy=(goal_x, goal_y), radius=self.radius, color=goal_color, alpha=0.5
+            )
+            goal_circle.set_zorder(1)
+
+            ax.add_patch(goal_circle)
+
+            # 3D plot
+            art3d.patch_2d_to_3d(goal_circle, z=self.z)
+
+            self.plot_patch_list.append(goal_circle)
+
+        def plot_arrow(self, ax, arrow_length=0.4, arrow_width=0.6, **kwargs):
+            """
+            Plot an arrow indicating the velocity orientation of the object.
+
+            Args:
+                ax: Matplotlib axis.
+                arrow_length (float): Length of the arrow.
+                arrow_width (float): Width of the arrow.
+                **kwargs: Additional plotting options.
+            """
+            x = self.state_re[0][0]
+            y = self.state_re[1][0]
+            theta = atan2(self.velocity_xy[1, 0], self.velocity_xy[0, 0])
+            arrow_color = kwargs.get("arrow_color", "gold")
+
+            arrow = mpl.patches.Arrow(
+                x,
+                y,
+                arrow_length * cos(theta),
+                arrow_length * sin(theta),
+                width=arrow_width,
+                color=arrow_color,
+            )
+            arrow.set_zorder(3)
+            ax.add_patch(arrow)
+
+            # 3D plot
+            art3d.patch_2d_to_3d(arrow, z=self.z)
+
+            self.plot_patch_list.append(arrow)
+
+        def plot_trail(self, ax, **kwargs):
+            """
+            Plot the trail of the object.
+
+            Args:
+                ax: Matplotlib axis.
+                **kwargs: Additional plotting options.
+            """
+            trail_type = kwargs.get("trail_type", self.shape)
+            trail_edgecolor = kwargs.get("edgecolor", self.color)
+            trail_linewidth = kwargs.get("linewidth", 0.8)
+            trail_alpha = kwargs.get("trail_alpha", 0.7)
+            trail_fill = kwargs.get("trail_fill", False)
+            trail_color = kwargs.get("trail_color", self.color)
+
+            r_phi_ang = 180 * self._state[2, 0] / pi
+
+            if trail_type == "rectangle" or trail_type == "polygon":
+                start_x = self.vertices[0, 0]
+                start_y = self.vertices[1, 0]
+
+                car_rect = mpl.patches.Rectangle(
+                    xy=(start_x, start_y),
+                    width=self.length,
+                    height=self.width,
+                    angle=r_phi_ang,
+                    edgecolor=trail_edgecolor,
+                    fill=False,
+                    alpha=trail_alpha,
+                    linewidth=trail_linewidth,
+                    facecolor=trail_color,
+                )
+                ax.add_patch(car_rect)
+
+                # 3D plot
+                art3d.patch_2d_to_3d(car_rect, z=self.z)
+
+            elif trail_type == "circle":
+                car_circle = mpl.patches.Circle(
+                    xy=self.centroid,
+                    radius=self.radius,
+                    edgecolor=trail_edgecolor,
+                    fill=trail_fill,
+                    alpha=trail_alpha,
+                    facecolor=trail_color,
+                )
+                ax.add_patch(car_circle)
+
+                # 3D plot
+                art3d.patch_2d_to_3d(car_circle, z=self.z)
+
+
+        def plot_trajectory(self, ax, keep_length=0, **kwargs):
+
+            """
+            Plot the trajectory of the object.
+
+            Args:
+                ax: Matplotlib axis.
+                keep_length (int): Number of steps to keep in the plot.
+                **kwargs: Additional plotting options.
+            """
+            traj_color = kwargs.get("traj_color", self.color)
+            traj_style = kwargs.get("traj_style", "-")
+            traj_width = kwargs.get("traj_width", self.width)
+            traj_alpha = kwargs.get("traj_alpha", 0.5)
+
+            x_list = [t[0, 0] for t in self.trajectory[-keep_length:]]
+            y_list = [t[1, 0] for t in self.trajectory[-keep_length:]]
+
+            # linewidth = linewidth_from_data_units_3d(traj_width, ax, "z") 
+            linewidth = traj_width * 10
+            solid_capstyle = "round" if self.shape == "circle" else "butt"
+
+            self.plot_line_list.append(
+                ax.plot(
+                    x_list,
+                    y_list,
+                    color=traj_color,
+                    linestyle=traj_style,
+                    linewidth=linewidth,
+                    solid_joinstyle="round",
+                    solid_capstyle=solid_capstyle,
+                    alpha=traj_alpha,
+                    zs=0, zdir='z',
+                )
+                
+            )
+
+        
+            
+            
\ No newline at end of file
diff --git a/irsim/world/object_factory3d.py b/irsim/world/object_factory3d.py
new file mode 100644
index 00000000..adfe55ac
--- /dev/null
+++ b/irsim/world/object_factory3d.py
@@ -0,0 +1,65 @@
+from irsim.world import ObjectFactory
+from irsim.world import (
+    RobotAcker3D,
+    RobotDiff3D,
+    RobotOmni3D,
+    ObstacleAcker3D,
+    ObstacleDiff3D,
+    ObstacleOmni3D,
+    ObjectStatic3D,
+)
+
+class ObjectFactory3D(ObjectFactory):
+    
+        
+    def create_robot(self, kinematics=dict(), **kwargs):
+        """
+        Create a robot based on kinematics.
+
+        Args:
+            kinematics (dict): Kinematics configuration.
+            **kwargs: Additional parameters for robot creation.
+
+        Returns:
+            Robot: An instance of a robot.
+        """
+        kinematics_name = kinematics.get("name", None)
+
+        if kinematics_name == "diff":
+            return RobotDiff3D(kinematics=kinematics, **kwargs)
+        elif kinematics_name == "acker":
+            return RobotAcker3D(kinematics=kinematics, **kwargs)
+        elif kinematics_name == "omni":
+            return RobotOmni3D(kinematics=kinematics, **kwargs)
+        elif kinematics_name == "static" or kinematics_name is None:
+            return ObjectStatic3D(kinematics=kinematics, role="robot", **kwargs)
+        else:
+            raise NotImplementedError(
+                f"Robot kinematics {kinematics_name} not implemented"
+            )
+
+    def create_obstacle(self, kinematics=dict(), **kwargs):
+        """
+        Create a obstacle based on kinematics.
+
+        Args:
+            kinematics (dict): Kinematics configuration.
+            **kwargs: Additional parameters for robot creation.
+
+        Returns:
+            Obstacle: An instance of an obstacle.
+        """
+        kinematics_name = kinematics.get("name", None)
+
+        if kinematics_name == "diff":
+            return ObstacleDiff3D(kinematics=kinematics, **kwargs)
+        elif kinematics_name == "acker":
+            return ObstacleAcker3D(kinematics=kinematics, **kwargs)
+        elif kinematics_name == "omni":
+            return ObstacleOmni3D(kinematics=kinematics, **kwargs)
+        elif kinematics_name == "static" or kinematics_name is None:
+            return ObjectStatic3D(kinematics=kinematics, role="obstacle", **kwargs)
+        else:
+            raise NotImplementedError(
+                f"Robot kinematics {kinematics_name} not implemented"
+            )
\ No newline at end of file
diff --git a/irsim/world/obstacles/obstacle_acker_3d.py b/irsim/world/obstacles/obstacle_acker_3d.py
new file mode 100644
index 00000000..939d3385
--- /dev/null
+++ b/irsim/world/obstacles/obstacle_acker_3d.py
@@ -0,0 +1,8 @@
+from irsim.world import ObjectBase3D
+
+
+class ObstacleAcker3D(ObjectBase3D):
+    def __init__(self, kinematics={"name": "acker"}, color="k", **kwargs):
+        super(ObstacleAcker3D, self).__init__(
+            kinematics=kinematics, color=color, role="obstacle", **kwargs
+        )
diff --git a/irsim/world/obstacles/obstacle_diff_3d.py b/irsim/world/obstacles/obstacle_diff_3d.py
new file mode 100644
index 00000000..a34621c7
--- /dev/null
+++ b/irsim/world/obstacles/obstacle_diff_3d.py
@@ -0,0 +1,7 @@
+from irsim.world import ObjectBase3D
+
+class ObstacleDiff3D(ObjectBase3D):
+    def __init__(self, kinematics={"name": "diff"}, color="k", **kwargs):
+        super(ObstacleDiff3D, self).__init__(
+            kinematics=kinematics, color=color, role="obstacle", **kwargs
+        )
diff --git a/irsim/world/obstacles/obstacle_omni_3d.py b/irsim/world/obstacles/obstacle_omni_3d.py
new file mode 100644
index 00000000..09ab8cdb
--- /dev/null
+++ b/irsim/world/obstacles/obstacle_omni_3d.py
@@ -0,0 +1,10 @@
+from irsim.world import ObjectBase3D
+
+
+class ObstacleOmni3D(ObjectBase3D):
+    def __init__(
+        self, kinematics={"name": "omni"}, color="k", **kwargs
+    ):
+        super(ObstacleOmni3D, self).__init__(
+            kinematics=kinematics, color=color, role="obstacle", **kwargs
+        )
diff --git a/irsim/world/obstacles/obstacle_static_3d.py b/irsim/world/obstacles/obstacle_static_3d.py
new file mode 100644
index 00000000..5fcd5b7a
--- /dev/null
+++ b/irsim/world/obstacles/obstacle_static_3d.py
@@ -0,0 +1,11 @@
+from irsim.world import ObjectBase3D
+
+
+class ObjectStatic3D(ObjectBase3D):
+    def __init__(
+        self, kinematics=None, color="k", role="obstacle", **kwargs
+    ):
+        super(ObjectStatic3D, self).__init__(
+            kinematics=kinematics, color=color, static=True, role=role, **kwargs
+        )
+        
diff --git a/irsim/world/robots/robot_acker_3d.py b/irsim/world/robots/robot_acker_3d.py
new file mode 100644
index 00000000..c81e47c3
--- /dev/null
+++ b/irsim/world/robots/robot_acker_3d.py
@@ -0,0 +1,19 @@
+from irsim.world import ObjectBase3D
+
+
+class RobotAcker3D(ObjectBase3D):
+    def __init__(
+        self, color="y", state_dim=4, description=None, **kwargs
+    ):
+        super(RobotAcker3D, self).__init__(
+            role="robot",
+            color=color,
+            state_dim=state_dim,
+            description=description,
+            **kwargs,
+        )
+
+        assert (
+            state_dim >= 4
+        ), "for ackermann robot, the state dimension should be greater than 4"
+
diff --git a/irsim/world/robots/robot_diff_3d.py b/irsim/world/robots/robot_diff_3d.py
new file mode 100644
index 00000000..bf157cda
--- /dev/null
+++ b/irsim/world/robots/robot_diff_3d.py
@@ -0,0 +1,18 @@
+from irsim.world import ObjectBase3D
+
+
+class RobotDiff3D(ObjectBase3D):
+    def __init__(
+        self, kinematics={'name': "diff"}, color="g", state_dim=3, **kwargs
+    ):
+        super(RobotDiff3D, self).__init__(
+            kinematics=kinematics,
+            role="robot",
+            color=color,
+            state_dim=state_dim,
+            **kwargs,
+        )
+
+        assert (
+            state_dim >= 3
+        ), "for differential robot, the state dimension should be greater than 3"
diff --git a/irsim/world/robots/robot_omni_3d.py b/irsim/world/robots/robot_omni_3d.py
new file mode 100644
index 00000000..0aabad76
--- /dev/null
+++ b/irsim/world/robots/robot_omni_3d.py
@@ -0,0 +1,20 @@
+from irsim.world import ObjectBase3D
+
+
+class RobotOmni3D(ObjectBase3D):
+    def __init__(
+        self, color="g", state_dim=2, **kwargs
+    ):
+        super(RobotOmni3D, self).__init__(
+            role="robot",
+            color=color,
+            state_dim=state_dim,
+            **kwargs,
+        )
+
+        assert (
+            state_dim >= 2
+        ), "for omni robot, the state dimension should be greater than 2"
+
+    
+
diff --git a/irsim/world/world.py b/irsim/world/world.py
index 19bb3075..f947106f 100644
--- a/irsim/world/world.py
+++ b/irsim/world/world.py
@@ -6,7 +6,7 @@
 from typing import Optional
 
 
-class world:
+class World:
     """
     Represents the main simulation environment, managing objects and maps.
 
diff --git a/irsim/world/world3d.py b/irsim/world/world3d.py
new file mode 100644
index 00000000..43c4f921
--- /dev/null
+++ b/irsim/world/world3d.py
@@ -0,0 +1,13 @@
+from irsim.world import World
+
+
+class World3D(World):
+    
+    def __init__(self, depth = 0, offset=[0, 0, 0], **kwargs):
+        super().__init__(**kwargs)
+
+        self.depth = depth
+
+        self.offset = offset if len(offset) == 3 else [offset[0], offset[1], 0]
+        
+        self.z_range = [self.offset[2], self.offset[2] + self.width]
\ No newline at end of file