Voronoi Terrain Resolution

pit/dynamics/dynamic_bicycle.py

@@ -73,6 +73,26 @@ def forward(self, states, control_inputs):
 
         diff = torch.zeros_like(states)
         tire_forces = self.calculate_tire_forces(states, control_inputs)
+        if torch.isnan(tire_forces).any():
+            # Handle the case when tire forces return NaN
+            # Check for specific parameters causing the blow-up
+            if torch.isnan(states).any() or torch.isnan(control_inputs).any():
+               print("NaN encountered in states or control_inputs.")
+               print("states:", states)
+               print("control_inputs:", control_inputs)
+            if torch.isnan(self.Cm).any() or torch.isnan(self.Cr0).any() or torch.isnan(self.Cr2).any():
+                raise ValueError("NaN encountered in Cm, Cr0, or Cr2.")
+            if torch.isnan(self.Df).any() or torch.isnan(self.Cf).any() or torch.isnan(self.Bf).any():
+                raise ValueError("NaN encountered in Df, Cf, or Bf.")
+            if torch.isnan(self.Dr).any() or torch.isnan(self.Cr).any() or torch.isnan(self.Br).any():
+                raise ValueError("NaN encountered in Dr, Cr, or Br.")
+            if torch.isnan(self.lf).any() or torch.isnan(self.lr).any() or torch.isnan(self.mass).any():
+                raise ValueError("NaN encountered in lf, lr, or mass.")
+            if torch.isnan(self.Iz).any():
+                raise ValueError("NaN encountered in Iz.")
+
+            # Add your desired handling logic here
+
         if batch_mode:
             diff[:, X] = states[:, VX] * torch.cos(states[:, YAW]) - states[:, VY] * torch.sin(states[:, YAW])
             diff[:, Y] = states[:, VX] * torch.sin(states[:, YAW]) - states[:, VY] * torch.cos(states[:, YAW])

pit/integration/euler.py

@@ -9,6 +9,10 @@ def __init__(self, dynamics, timestep=0.10, include_initial_state=False) -> None
         self.timestep = timestep
         self.include_initial_state = include_initial_state
 
+    def check_for_nans(self, tensor, name):
+        if torch.isnan(tensor).any():
+            print(f"NaNs detected in {name}")
+
     def forward(self, initial_state, control_inputs):
         """
         We integrate the specified dynamics
@@ -40,31 +44,41 @@ def forward(self, initial_state, control_inputs):
 
         if batch_mode:
             diff = self.dynamics(initial_state, control_inputs[:,0])
+            self.check_for_nans(diff, "diff (initial batch)")
             #state = torch.zeros((B, state_dims))
             state = initial_state + diff * self.timestep
+            self.check_for_nans(state, "state (initial batch)")
             integrated_states.append(state)
 
             for i in range(1, control_inputs.shape[1]):
                 #state = torch.zeros((B, state_dims))
                 diff = self.dynamics(integrated_states[-1], control_inputs[:,i])
+                self.check_for_nans(diff, f"diff (batch step {i})")
                 state = integrated_states[-1] + diff * self.timestep
+                self.check_for_nans(state, f"state (batch step {i})")
                 integrated_states.append(state)
             integrated_states = torch.stack(integrated_states, dim=1)
+            self.check_for_nans(integrated_states, "integrated_states (batch)")
             #assert(list(integrated_states.shape) == [control_inputs.shape[0], control_inputs.shape[1], state_dims])
 
         else:
             diff = self.dynamics(initial_state, control_inputs[0])
+            self.check_for_nans(diff, "diff (initial)")
             #state = torch.zeros((state_dims))
             state = initial_state + diff * self.timestep
+            self.check_for_nans(state, "state (initial)")
             integrated_states.append(state)
 
             for i in range(1, control_inputs.shape[0]):
                 diff = self.dynamics(integrated_states[-1], control_inputs[i])
                 #state = torch.zeros((state_dims))
+                self.check_for_nans(diff, f"diff (step {i})")
                 state = integrated_states[-1] + diff * self.timestep
+                self.check_for_nans(state, f"state (step {i})")
                 integrated_states.append(state)
 
             integrated_states = torch.stack(integrated_states, dim=0)
+            self.check_for_nans(integrated_states, "integrated_states")
             #assert(list(integrated_states.shape) == [control_inputs.shape[0], state_dims])
 
 

pit/utilities.py

@@ -0,0 +1,100 @@
+import math
+
+from pit.dynamics.dynamic_bicycle import DynamicBicycle
+
+class VoronoiClassifier:
+    def __init__(self, points):
+        """
+        Initialize the VoronoiClassifier with a set of labeled points.
+
+        :param points: List of tuples (x, y, l) where x and y are coordinates, and l is the label.
+        """
+        self.points = points
+
+    def classify(self, x, y):
+        """
+        Classify a point (x, y) based on the closest labeled point.
+
+        :param x: X coordinate of the point to classify.
+        :param y: Y coordinate of the point to classify.
+        :return: Label of the closest point.
+        """
+        closest_point = None
+        min_distance = float('inf')
+
+        for px, py, label in self.points:
+            distance = math.sqrt((px - x) ** 2 + (py - y) ** 2)
+            if distance < min_distance:
+                min_distance = distance
+                closest_point = label
+
+        return closest_point
+
+
+
+
+class DynamicsWrapper:
+    def __init__(self, parameters, dynamics_model_class):
+        """
+        Initialize the DynamicsWrapper with sets of parameters and a dynamics model class.
+
+        :param parameters: Dictionary where keys are labels and values are parameter sets.
+        :param dynamics_model_class: The class of the dynamics model to use.
+        """
+        self.parameters = parameters
+        self.dynamics_model_class = dynamics_model_class
+        self.current_model = None
+        self.current_params = None
+
+    def select_parameters(self, label):
+        """
+        Select a set of parameters based on the label and initialize the dynamics model.
+
+        :param label: The label for the parameter set to use.
+        """
+        if label in self.parameters:
+            self.current_params = self.parameters[label]
+            self.current_model = self.dynamics_model_class(**self.current_params)
+        else:
+            raise ValueError(f"No parameter set found for label {label}")
+
+    def get_current_model(self):
+        """
+        Get the current dynamics model.
+
+        :return: The current dynamics model.
+        """
+        return self.current_model
+
+    def hot_swap_parameters(self, new_label):
+        """
+        Hot-swap the parameters and reinitialize the dynamics model.
+
+        :param new_label: The new label for the parameter set to switch to.
+        """
+        self.select_parameters(new_label)
+
+# Example usage:
+if __name__ == "__main__":
+    points = [(1, 2, 0), (3, 4, 1), (5, 6, 2)]
+    classifier = VoronoiClassifier(points)
+
+    print(classifier.classify(2, 3))  # Output will be the label of the closest point
+
+    # Define parameter sets for different labels
+    parameters = {
+        0: {"param1": 5, "param2": 6},
+        1: {"param1": 7, "param2": 8},
+        # Add more parameter sets as needed
+    }
+
+    # Initialize the wrapper with the parameter sets and the dynamics model class
+    wrapper = DynamicsWrapper(parameters, DynamicBicycle)
+
+    # Select parameters based on a label
+    wrapper.select_parameters(0)
+    current_model = wrapper.get_current_model()
+
+    # Hot-swap parameters to a new set
+    wrapper.hot_swap_parameters(1)
+    new_model = wrapper.get_current_model()