collision detection ik

add

kine_ctrl/main.py

@@ -71,7 +71,7 @@ def main():
 
         solve_sum = 0
 
-        for i in range(1000):
+        for i in range(10):
             print(f'\n-------------- in i = {i} ----------------')
             joint_rand = np.random.uniform(ub, lb)
             print(f'the predefined joints are {joint_rand}')
@@ -96,6 +96,7 @@ def main():
                 fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
                 d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
                 print(f'---- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}')
+                # robot_kine_qp.collision_detect(q,stop_at_first_collision=True, verbose=True)
                 if d_p_ik < 0.01:
                     result[0][1] += 1
 

kine_ctrl/req.txt

@@ -0,0 +1,3 @@
+conda install -c conda-forge osqp scipy tqdm matplotlib pandas "numpy<1.24" pinocchio -y
+pip install urdfpy mujoco "networkx>=2.8.4"
+pip install Robotic_Arm

kine_ctrl/rm75_kine_qp.py

@@ -20,8 +20,12 @@ class KinematicsSolver():
         unit: m, rad
         """
         print(f' ------------  the qp based kinematic initialising   -----------')
-        self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
+        self.model, self.collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
 
+        self.geom_model = pin.buildGeomFromUrdf(self.model, urdf_path, pin.GeometryType.COLLISION, mesh_dir)
+        self.geom_model.addAllCollisionPairs()
+        self.remove_adjacent_collision_pairs(verbose=True)
+        self.geom_data = pin.GeometryData(self.geom_model)
 
 
         self.cfg_j_limit()
@@ -335,12 +339,100 @@ class KinematicsSolver():
                 iter_count += 1
 
         if best_solution is not None:
-            # return best_solution, True, best_error, iter_count
+            collision = self.collision_detect(q=best_solution, stop_at_first_collision=True)
-            return 0, best_solution.tolist()
+
+            if collision is False:
+                # return best_solution, True, best_error, iter_count
+                return 0, best_solution.tolist()
+            else:
+                return -2, q[:7].copy().tolist()
         else:
             # return q[:7].copy(), False, error_norm, iter_count
             return -1, q[:7].copy().tolist()
 
+    def collision_detect(self, q ,stop_at_first_collision=True, verbose=False ):
+        q = np.asarray(q, dtype=np.float64).reshape(-1)
+
+        if q.shape[0] != self.model.nq:
+            raise ValueError(f"q size mismatch: expected {self.model.nq}, got {q.shape[0]}")
+
+        # Update robot kinematics
+        pin.forwardKinematics(self.model, self.data, q)
+        pin.updateGeometryPlacements(
+            self.model,
+            self.data,
+            self.geom_model,
+            self.geom_data,
+            q
+        )
+
+        # Now compute collisions on the updated geometry model
+        collision = pin.computeCollisions(
+            self.geom_model,
+            self.geom_data,
+            stop_at_first_collision
+        )
+
+        if verbose:
+            print(f"the collision is {collision}\n")
+
+            for k, cr in enumerate(self.geom_data.collisionResults):
+                if cr.isCollision():
+                    cp = self.geom_model.collisionPairs[k]
+                    geom1 = self.geom_model.geometryObjects[cp.first]
+                    geom2 = self.geom_model.geometryObjects[cp.second]
+
+                    print(
+                        f"collision pair {k}: "
+                        f"{geom1.name} <--> {geom2.name}"
+                    )
+
+        return bool(collision)
+
+    def remove_adjacent_collision_pairs(self, verbose=True):
+        """
+        Remove collision pairs between same/adjacent parent joints.
+
+        This avoids false positives such as:
+            base_link_0 <--> link_1_0
+        """
+
+        pairs_to_remove = []
+
+        for pair_id, pair in enumerate(self.geom_model.collisionPairs):
+            geom1 = self.geom_model.geometryObjects[pair.first]
+            geom2 = self.geom_model.geometryObjects[pair.second]
+
+            j1 = geom1.parentJoint
+            j2 = geom2.parentJoint
+
+            # Same body or directly connected bodies
+            if j1 == j2 or abs(j1 - j2) <= 1:
+                pairs_to_remove.append(pair_id)
+
+                if verbose:
+                    print(
+                        "Removing adjacent pair:",
+                        pair_id,
+                        geom1.name,
+                        "<-->",
+                        geom2.name,
+                        "parentJoint:",
+                        j1,
+                        j2,
+                    )
+
+        for pair_id in reversed(pairs_to_remove):
+            self.geom_model.removeCollisionPair(
+                self.geom_model.collisionPairs[pair_id]
+            )
+
+        # Important: recreate geometry data after modifying pairs
+        self.geom_data = pin.GeometryData(self.geom_model)
+
+        if verbose:
+            print("Remaining collision pairs:", len(self.geom_model.collisionPairs))
+
     def quaternion_to_euler(self, q):
         """
         Convert quaternion to Euler angles (roll, pitch, yaw)

kine_ctrl/workspace_comfortable/Contour_plot.py

@@ -0,0 +1,80 @@
+from pathlib import Path
+
+import matplotlib.pyplot as plt
+import numpy as np
+import pandas as pd
+
+
+# --------------------------------------------------
+# 1. Load the data
+# --------------------------------------------------
+csv_path = Path("workspace_nocollisiondetection.csv")
+
+# The file has no column names, so header=None is important.
+df = pd.read_csv(
+    csv_path,
+    header=None,
+    names=["x", "y", "z", "ik_rate"],
+)
+
+print(df.head())
+print("z planes:", np.sort(df["z"].unique()))
+
+
+# --------------------------------------------------
+# 2. Create an output directory
+# --------------------------------------------------
+output_dir = Path("contour_plots")
+output_dir.mkdir(exist_ok=True)
+
+
+# --------------------------------------------------
+# 3. Use the same colour scale for every z-plane
+# --------------------------------------------------
+value_min = df["ik_rate"].min()
+value_max = df["ik_rate"].max()
+
+# More levels give a smoother-looking contour plot.
+levels = np.linspace(value_min, value_max, 51)
+
+
+# --------------------------------------------------
+# 4. Draw one contour plot for each z-plane
+# --------------------------------------------------
+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")
+
+    x = grid.columns.to_numpy()
+    y = grid.index.to_numpy()
+    ik_grid = grid.to_numpy()
+
+    X, Y = np.meshgrid(x, y)
+
+    fig, ax = plt.subplots(figsize=(7, 6))
+
+    contour = ax.contourf(
+        X,
+        Y,
+        ik_grid,
+        levels=levels,
+        cmap="viridis",
+        extend="both",
+    )
+
+    colorbar = fig.colorbar(contour, ax=ax)
+    colorbar.set_label("IK rate")
+
+    ax.set_title(f"IK rate at z = {z_value:.2f}")
+    ax.set_xlabel("x")
+    ax.set_ylabel("y")
+    ax.set_aspect("equal")
+
+    fig.tight_layout()
+
+    output_path = output_dir / f"ik_contour_z_{z_value:.2f}.png"
+    fig.savefig(output_path, dpi=200, bbox_inches="tight")
+    plt.close(fig)
+
+print(f"Plots saved to: {output_dir.resolve()}")

kine_ctrl/workspace_comfortable/workspace_cal.py

@@ -53,7 +53,9 @@ X_RANGE = (-0.6, 0.6)
 Y_RANGE = (-0.6, 0.6)
 Z_RANGE = (0.0, 0.8)
 
-GRID_RESOLUTION = 0.3  # 5 cm. Use 0.02 for finer but slower.
+GRID_RESOLUTION = 0.05  # 5 cm. Use 0.02 for finer but slower.
+
+num_orientations =  120 
 
 # Comfort thresholds
 MIN_JOINT_MARGIN = 0.05       # 15% away from joint limits
@@ -133,7 +135,7 @@ JACOBIAN_EPS = 1e-5
 # 2. TASK ORIENTATION SAMPLING
 # ============================================================
 
-def make_task_orientations(num_orientations=200, seed=1):
+def make_task_orientations(num_orientations=num_orientations, seed=1):
     """
     Random orientation sampling using RM's Euler convention:
 
@@ -556,8 +558,7 @@ def evaluate_workspace():
         for rpy in orientations:
             attempted += 1
 
-            # print(f"\n - target point: {point}, target orientation: {rpy}")
+
-            rpy = [1.2022060487764064, -1.0097962261845583, -0.6518417572686532]
             ik_result = solve_ik_user(point, rpy)
 
             # print(f'\n point is {point}, rpy is {rpy}, and ik result q: {ik_result}')