diff --git a/kine_ctrl/main.py b/kine_ctrl/main.py index 26bf307..c6144a8 100644 --- a/kine_ctrl/main.py +++ b/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 diff --git a/kine_ctrl/rm75_kine_qp.py b/kine_ctrl/rm75_kine_qp.py index 65a7e6c..f1a0989 100644 --- a/kine_ctrl/rm75_kine_qp.py +++ b/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) diff --git a/kine_ctrl/workspace_comfortable/Comfortable_Region.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/Comfortable_Region.png similarity index 100% rename from kine_ctrl/workspace_comfortable/Comfortable_Region.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/Comfortable_Region.png diff --git a/kine_ctrl/workspace_comfortable/Comfortable_Workspace.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/Comfortable_Workspace.png similarity index 100% rename from kine_ctrl/workspace_comfortable/Comfortable_Workspace.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/Comfortable_Workspace.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.00.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.00.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.00.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.00.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.05.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.05.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.05.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.05.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.10.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.10.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.10.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.10.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.15.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.15.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.15.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.15.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.20.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.20.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.20.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.20.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.25.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.25.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.25.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.25.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.30.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.30.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.30.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.30.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.35.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.35.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.35.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.35.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.40.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.40.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.40.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.40.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.45.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.45.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.45.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.45.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.50.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.50.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.50.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.50.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.55.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.55.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.55.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.55.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.60.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.60.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.60.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.60.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.65.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.65.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.65.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.65.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.70.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.70.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.70.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.70.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.75.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.75.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.75.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.75.png diff --git a/kine_ctrl/workspace_comfortable/ik_contour_z_0.80.png b/kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.80.png similarity index 100% rename from kine_ctrl/workspace_comfortable/ik_contour_z_0.80.png rename to kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.80.png