Add G h cone_type

irsim/lib/handler/geometry_handler.py

@@ -67,16 +67,22 @@ def get_init_Gh(self):
         if self.name == "circle":
             G = np.array([[1, 0], [0, 1], [0, 0]])
             h = np.array([[0], [0], [-self.radius]])
+            cone_type = "norm2"
+            convex_flag = True
 
         elif self.name == "polygon" or self.name == "rectangle":
 
             convex_flag, _ = is_convex_and_ordered(self.init_vertices)
 
-            assert convex_flag, "Object Vertices are not convex"
+            # assert convex_flag, "Polygon Objects are not convex"
 
             G, h = gen_inequal_from_vertex(self.init_vertices)
 
-        return G, h
+            cone_type = "Rpositive"
+        else:
+            G, h, cone_type, convex_flag = None, None, None, None
+
+        return G, h, cone_type, convex_flag
 
 
     def cal_length_width(self, geometry):

irsim/lib/handler/kinematics_handler.py

@@ -76,14 +76,14 @@ def step(self, state: np.ndarray, velocity: np.ndarray, step_time: float) -> np.
         )
         return next_state
 
-class Rigid3DKinematics(KinematicsHandler):
+# class Rigid3DKinematics(KinematicsHandler):
 
-    def __init__(self, name, noise, alpha):
-        super().__init__(name, noise, alpha)
+#     def __init__(self, name, noise, alpha):
+#         super().__init__(name, noise, alpha)
 
-    def step(self, state: np.ndarray, velocity: np.ndarray, step_time: float) -> np.ndarray:
-        next_state = rigid3d_kinematics(state, velocity, step_time, self.noise, self.alpha)
-        return next_state
+#     def step(self, state: np.ndarray, velocity: np.ndarray, step_time: float) -> np.ndarray:
+#         next_state = rigid3d_kinematics(state, velocity, step_time, self.noise, self.alpha)
+#         return next_state
 
 class KinematicsFactory:
     """

irsim/world/object_base.py

@@ -46,6 +46,7 @@ class ObjectInfo:
     G: np.ndarray
     h: np.ndarray
     cone_type: str
+    convex_flag: bool
 
     def add_property(self, key, value):
         setattr(self, key, value)
@@ -60,6 +61,7 @@ class ObstacleInfo:
     G: np.ndarray
     h: np.ndarray
     cone_type: str
+    convex_flag: bool
 
     def add_property(self, key, value):
         setattr(self, key, value)
@@ -204,6 +206,8 @@ def __init__(
             else None
         )
 
+        self.G, self.h, self.cone_type, self.convex_flag = self.gf.get_init_Gh()
+
         self.state_dim = state_dim if state_dim is not None else self.state_shape[0]
         self.state_shape = (
             (self.state_dim, 1) if state_dim is not None else self.state_shape
@@ -251,6 +255,10 @@ def __init__(
             np.c_[angle_range],
             goal_threshold,
             self.wheelbase,
+            self.G,
+            self.h,
+            self.cone_type,
+            self.convex_flag,
         )
 
         self.obstacle_info = None
@@ -568,30 +576,6 @@ def set_init_geometry(self, geometry):
         """
         self._init_geometry = geometry
 
-    def generate_Gh(self, shape, shape_tuple):
-        """
-        Generate G and h for convex object.
-
-        Args:
-            shape (str): The shape of the object.
-            shape_tuple: Tuple to initialize the geometry.
-
-        Returns:
-            tuple: G matrix, h vector, and cone type.
-        """
-        if shape == "circle":
-            radius = shape_tuple[2]
-            G = np.array([[1, 0], [0, 1], [0, 0]])
-            h = np.array([[0], [0], [-radius]])
-            cone_type = "norm2"
-
-        else:
-            init_vertex = np.array(shape_tuple).T
-            G, h = gen_inequal_from_vertex(init_vertex)
-            cone_type = "Rpositive"
-
-        return G, h, cone_type
-
     def geometry_state_transition(self):
         pass
 
@@ -996,6 +980,7 @@ def get_obstacle_info(self):
             self.G,
             self.h,
             self.cone_type,
+            self.convex_flag,
         )
 
     def get_init_Gh(self):
@@ -1172,6 +1157,3 @@ def beh_config(self):
         # behavior config dictory
         return self.obj_behavior.behavior_dict
 
-    @property
-    def cone_type(self):
-        return self.gf.cone_type