collision detection ik

add

kine_ctrl/main.py

@@ -22,12 +22,12 @@ tools_in_ee = {
 }
 
 # joint limit
-ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi
-lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi
+# ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi
+# lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi
 
 
-# ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
-# lb = -ub
+ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
+lb = -ub
 
 tool_name = "scissor"
 
@@ -48,6 +48,18 @@ def main():
     robot_kine_rm.add_tool_frames(tools_in_ee)
     robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
 
+    ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=[-0.6, -0.6 , 0. ], target_rpy=[1.2022060487764064, -1.0097962261845583, -0.6518417572686532],
+                                                 initial_guess=[0.1] * 7, tool="no_tool")
+
+    print(f'ret_rm = {ret_rm}, q = {q}')
+    pose = robot_kine_rm.forward_kinematics(joint_angles=q, tool="no_tool")
+    print(f'pose = {pose}')
+    print('-'*100)
+
+    time.sleep(5)
+
+
+
 
 
 #   --------------  for comparison   ----------------
@@ -83,27 +95,28 @@ def main():
             if ret_qp == 0:
                 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}')
+                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
 
-                robot_mjk.send_command(q)
-                robot_mjk.wait_until_reached()
-                robot_mjk.print_state()
+                # robot_mjk.send_command(q)
+                # robot_mjk.wait_until_reached()
+                # robot_mjk.print_state()
             else:
                 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'-- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}')
+                print(f'---- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}')
 
             ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name)
             if ret_rm == 0:
                 fk_rm_p2 = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name)
                 d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_rm_p2)
-                print(f'== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_qp}')
+                print(f'==== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_rm}')
                 if d_p_ik < 0.01:
                     result[1][1] += 1
             else:
-                print(f'== fail in the rm ik, ret = {ret_rm}, q = {q}')
+                print(f'==== fail in the rm ik, ret = {ret_rm}, q = {q}')
 
             if ret_qp == 0 or ret_rm == 0:
                 solve_sum += 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/requirements.txt

@@ -0,0 +1,9 @@
+numpy
+pandas
+matplotlib
+tqdm
+scipy
+urdfpy
+pin
+osqp
+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
-            return 0, best_solution.tolist()
+            collision = self.collision_detect(q=best_solution, stop_at_first_collision=True)
+
+            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/rm75_kine_rm.py

@@ -7,7 +7,7 @@ class rm75_kine_api():
     def __init__(self):
         #   ----------      rm75 official algorithm         -----------
         print(f'-------  the realman official kinematic initialising -------')
-        arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E  # RM_65 Robotic arm
+        arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E  # RM_75 Robotic arm
         force_type = rm_force_type_e.RM_MODEL_RM_B_E  # Standard version
         # Initialize the robotic arm model and sensor type in the algorithm
         self.robot_kine_rm = Algo(arm_model, force_type)
@@ -120,14 +120,37 @@ class rm75_kine_api():
             self.work_name = work
             self.cfg_work_frame(work)
 
-        target = target_position + target_rpy
+        target = list(target_position) + list(target_rpy)
 
         if initial_guess is not None:
             q_ref = [ 180/math.pi * ig for ig in  initial_guess ]
         else:
             q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0]
         ret, phi = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
+        # print(f'the arm angle is ret = {ret}, and phi = {phi}')
         params = rm_inverse_kinematics_params_t(q_ref,
                                                 target, 1)
         ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
-        return ret, [ q/180*math.pi for q in q_out]
+        pose_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1)
+        pose_dis = cal_pose_deviation(pose_fk, target)
+
+        # print(f'target pose is {target}, fk pose is {pose_fk}, dis of poses is {pose_dis}')
+        #
+        # print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target}  phi = {phi}, q_out = {q_out}, ret = {ret}\n\n')
+        # print(f'the tool frame is {self.robot_kine_rm.rm_algo_get_curr_toolframe()}')
+        if int(ret) < 0:
+            return ret, [ q/180*math.pi for q in q_out]
+        elif pose_dis < 0.01:
+            return ret, [ q/180*math.pi for q in q_out]
+        else:
+            return -10, [ q/180*math.pi for q in q_out]
+
+def cal_pose_deviation(pose1, pose2):
+    d_fk_p1 = np.array(pose1) - np.array(pose2)
+    for j in [3, 4, 5]:
+        while d_fk_p1[j] > math.pi:
+            d_fk_p1[j] -= 2 * math.pi
+        while d_fk_p1[j] < -math.pi:
+            d_fk_p1[j] += 2 * math.pi
+    d_fk = np.linalg.norm(d_fk_p1)
+    return d_fk

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

@@ -47,6 +47,29 @@ from Robotic_Arm.rm_robot_interface import *
 import time
 from math import radians, degrees, pi, cos, sin
 
+# Cartesian workspace grid, in meters.
+# Adjust according to your robot placement and task.
+X_RANGE = (-0.6, 0.6)
+Y_RANGE = (-0.6, 0.6)
+Z_RANGE = (0.0, 0.8)
+
+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
+MAX_CONDITION_NUMBER = 150.0
+MIN_MANIPULABILITY_RATIO = 0.10
+
+# Scoring weights
+WEIGHT_IK_SUCCESS = 0.70
+WEIGHT_JOINT_LIMIT = 0.10
+WEIGHT_MANIPULABILITY = 0.1
+WEIGHT_SINGULARITY = 0.1
+
+
+
 
 #   pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
 #   load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
@@ -67,8 +90,11 @@ lb = -ub
 
 tool_name = "no_tool"
 
+URDF_PATH = str(parent_dir) + '/urdf_rm75/RM75-B.urdf'
+MESH_DIR = str(Path(URDF_PATH).parent)
+
 #  -----------      rm75 qp based kine          ------------
-robot_kine_qp = kine_qp(urdf_path='/home/zl/Downloads/urdf_rm75/RM75-B.urdf', mesh_dir='/home/zl/Downloads/urdf_rm75')
+robot_kine_qp = kine_qp(urdf_path=URDF_PATH, mesh_dir=MESH_DIR)
 robot_kine_qp.add_tool_frames(tools_in_ee)
 robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
 
@@ -83,7 +109,7 @@ robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
 # 1. USER SETTINGS
 # ============================================================
 
-URDF_PATH = '/home/zl/Downloads/urdf_rm75/RM75-B.urdf'
+
 
 BASE_LINK = "base_link"
 TCP_LINK = "link_7"
@@ -98,24 +124,7 @@ JOINT_NAMES = [
     "joint_7",
 ]
 
-# Cartesian workspace grid, in meters.
-# Adjust according to your robot placement and task.
-X_RANGE = (-0.25, 0.6)
-Y_RANGE = (-0.25, 0.6)
-Z_RANGE = (0.1, 0.6)
 
-GRID_RESOLUTION = 0.1  # 5 cm. Use 0.02 for finer but slower.
-
-# Comfort thresholds
-MIN_JOINT_MARGIN = 0.05       # 15% away from joint limits
-MAX_CONDITION_NUMBER = 150.0
-MIN_MANIPULABILITY_RATIO = 0.10
-
-# Scoring weights
-WEIGHT_IK_SUCCESS = 0.70
-WEIGHT_JOINT_LIMIT = 0.10
-WEIGHT_MANIPULABILITY = 0.1
-WEIGHT_SINGULARITY = 0.1
 
 # Numerical Jacobian settings
 JACOBIAN_EPS = 1e-5
@@ -126,7 +135,7 @@ JACOBIAN_EPS = 1e-5
 # 2. TASK ORIENTATION SAMPLING
 # ============================================================
 
-def make_task_orientations(num_orientations=60, seed=1):
+def make_task_orientations(num_orientations=num_orientations, seed=1):
     """
     Random orientation sampling using RM's Euler convention:
 
@@ -229,6 +238,8 @@ def solve_ik_user(target_position, target_rotation):
     ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=target_position, target_rpy=target_rotation, initial_guess=initial_guess, tool=tool_name)
     # print(f'==== with rm ik, ret_rm: {ret_rm}, q = {q}')
     if ret_rm == 0:
+        pose_rm = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name)
+        # print(f'target position = {target_position}\ntarget_rpy = {target_rotation} \npose_rm = {pose_rm}')
         return q
 
 
@@ -547,20 +558,20 @@ def evaluate_workspace():
         for rpy in orientations:
             attempted += 1
 
-            # print(f"\n - target point: {point}, target orientation: {rpy}")
 
             ik_result = solve_ik_user(point, rpy)
+
+            # print(f'\n point is {point}, rpy is {rpy}, and ik result q: {ik_result}')
             candidate_solutions = normalize_ik_solutions(ik_result)
 
-
-
-
             if len(candidate_solutions) == 0:
                 continue
 
             evaluated_solutions = []
 
             for q in candidate_solutions:
+                # pose = robot_kine_qp.forward_kinematics(joint_angles=q, tool=tool_name)
+                # print(f'the fk of q is {pose}\n')
                 metrics = evaluate_single_solution(robot, q, lower, upper)
                 # print(f'matrics: {metrics}, q = {q}, lower = {lower}, upper = {upper}')
                 if metrics is not None: