update teh contour plot method

kine_ctrl/workspace_comfortable/Contour_plot.py

@@ -8,31 +8,40 @@ import pandas as pd
 # --------------------------------------------------
 # 1. Load the data
 # --------------------------------------------------
-csv_path = Path("workspace_nocollisiondetection.csv")
+file_name = "rm75b_comfort_workspace_collision_minisci.csv"
+csv_path = Path(file_name)
 
 # The file has no column names, so header=None is important.
-df = pd.read_csv(
+df_csv = pd.read_csv(
     csv_path,
-    header=None,
-    names=["x", "y", "z", "ik_rate"],
 )
+rate_res = df_csv.iloc[:, :4]
+rate_res_sort = rate_res.sort_values('z').reset_index(drop=True)
 
-print(df.head())
+DECIMALS = 4
-print("z planes:", np.sort(df["z"].unique()))
 
+x_unique = np.round(rate_res_sort['x'], DECIMALS).unique()
+y_unique = np.round(rate_res_sort['y'], DECIMALS).unique()
+z_unique = np.round(rate_res_sort['z'], DECIMALS).unique()
+
+nx, ny, nz = len(x_unique), len(y_unique), len(z_unique)
+
+
+ik_rates = rate_res_sort.to_numpy()
 
 # --------------------------------------------------
 # 2. Create an output directory
 # --------------------------------------------------
-output_dir = Path("contour_plots")
+output_dir = Path(file_name.split(".")[0])
 output_dir.mkdir(exist_ok=True)
 
 
 # --------------------------------------------------
 # 3. Use the same colour scale for every z-plane
 # --------------------------------------------------
-value_min = df["ik_rate"].min()
+df = rate_res_sort
-value_max = df["ik_rate"].max()
+value_min = df["ik_success_rate"].min()
+value_max = df["ik_success_rate"].max()
 
 # More levels give a smoother-looking contour plot.
 levels = np.linspace(value_min, value_max, 51)
@@ -44,7 +53,7 @@ levels = np.linspace(value_min, value_max, 51)
 for z_value, plane in df.groupby("z", sort=True):
 
     # Rows become y-coordinates, columns become x-coordinates.
-    grid = plane.pivot(index="y", columns="x", values="ik_rate")
+    grid = plane.pivot(index="y", columns="x", values="ik_success_rate")
 
     x = grid.columns.to_numpy()
     y = grid.index.to_numpy()
@@ -63,6 +72,15 @@ for z_value, plane in df.groupby("z", sort=True):
         extend="both",
     )
 
+    highlight_levels = [0.6, 0.7]
+    # Only plot if the levels are within the data range (optional)
+    if value_min <= 0.6 <= value_max or value_min <= 0.7 <= value_max:
+        lines = ax.contour(X, Y, ik_grid, levels=highlight_levels,
+                           colors='red', linewidths=2, linestyles='solid')
+        # Optionally label the lines
+        ax.clabel(lines, inline=True, fontsize=10, fmt='%1.1f')
+
+
     colorbar = fig.colorbar(contour, ax=ax)
     colorbar.set_label("IK rate")