Temp develop for 3D env

irsim/env/env_base.py

@@ -3,12 +3,11 @@
 Author: Ruihua Han ([email protected])
 '''
 
-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):

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
+
+
+
+
+
+
 
 
 

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)

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)
+
+
+
+
+
+

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] 

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):
 

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
+
+
+
+
+
+
+
+
+
+
+

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'
+
+
+
+
+
+
+
+
+

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
+
+
+
+
+
+
+
+
+

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()

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'
 
 

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