From ace5dea9a2d26b808884afd96fb7c2953b8a909d Mon Sep 17 00:00:00 2001 From: LiuzhengSJ Date: Wed, 29 Jul 2026 11:30:32 +0100 Subject: [PATCH] add collision detection in workspace cal --- kine_ctrl/rm75_kine_qp.py | 159 -------------------------------------- 1 file changed, 159 deletions(-) diff --git a/kine_ctrl/rm75_kine_qp.py b/kine_ctrl/rm75_kine_qp.py index d42fd0d..03c11c8 100644 --- a/kine_ctrl/rm75_kine_qp.py +++ b/kine_ctrl/rm75_kine_qp.py @@ -81,11 +81,6 @@ class KinematicsSolver(): except: print(f'tcp_id of {tcp} not found') - - - - - def add_frame(self,frame_name, position, rotationXYZ): ''' :param frame_name: str @@ -126,15 +121,9 @@ class KinematicsSolver(): def forward_kinematics(self, joint_angles, tool="omnipic"): """ Compute forward kinematics. - Args: joint_angles: List or array of 7 joint angles (radians) tool: Name of frame to compute - - Returns: - dict: Position, rotation, rpy, quaternion - unit: position: m - rpy: rad """ if len(joint_angles) != 7: raise ValueError(f"RM75 has 7 joints, got {len(joint_angles)}") @@ -175,7 +164,6 @@ class KinematicsSolver(): max_iter=500, tolerance=5e-3, debug=False, tool="ee"): """ Compute inverse kinematics using differential IK with multiple strategies. - Args: target_position: [x, y, z] target position (meters) target_rpy: [roll, pitch, yaw] target orientation (radians) @@ -185,7 +173,6 @@ class KinematicsSolver(): tolerance: Error tolerance debug: Print debug information tool: the frame name ('scissor', 'camera', 'ee') - Returns: tuple: (joint_angles, success, error) """ @@ -412,11 +399,9 @@ class KinematicsSolver(): 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): @@ -511,146 +496,6 @@ class KinematicsSolver(): # # target_placement = pin.SE3(target_rotation, np.array(target_position)) # - # # Try multiple initial guesses - # initial_guesses = [] - # - # if initial_guess is not None: - # initial_guesses.append(initial_guess) - # else: - # # Try different initial configurations - # initial_guesses.append([0.1] * 7) # Zero config - # initial_guesses.append([radians(30), radians(45), radians(30), - # radians(-45), radians(30), radians(-30), 0]) - # initial_guesses.append([radians(-30), radians(45), radians(-30), - # radians(45), radians(30), radians(30), 0]) - # - # best_solution = None - # best_error = float('inf') - # - # for guess_idx, guess in enumerate(initial_guesses): - # q = pin.neutral(self.model) - # for i, angle in enumerate(guess): - # if i < len(q): - # q[i] = np.clip(angle, self.model.lowerPositionLimit[i], - # self.model.upperPositionLimit[i]) - # - # # Differential IK with adaptive damping - # damping = 0.01 - # damping_reduction = 0.95 - # iter_count = 0 - # prev_error = float('inf') - # - # ee_frame_id = self.tool_frames[tool] - # - # J = pin.computeFrameJacobian( - # self.model, - # self.data, - # q, - # ee_frame_id, - # pin.ReferenceFrame.LOCAL_WORLD_ALIGNED - # ) - # - # while iter_count < max_iter: - # # Compute forward kinematics - # - # pin.computeJointJacobians(self.model, self.data, q) - # pin.framesForwardKinematics(self.model, self.data, q) - # - # # Get current end-effector placement - # - # current_placement = self.data.oMf[ee_frame_id] - # - # # Compute error - # error_SE3 = current_placement.actInv(target_placement) - # error_vec = pin.log(error_SE3).vector - # error_norm = np.linalg.norm(error_vec) - # - # if error_norm < tolerance: - # joint_angles = q[:7].copy() - # fk_result = self.forward_kinematics(joint_angles, tool=tool) - # position_error = np.linalg.norm(fk_result['position'] - np.array(target_position)) - # - # if position_error < best_error: - # best_error = position_error - # best_solution = joint_angles - # break - # - # # Check if error is increasing (diverging) - # if error_norm > prev_error * 1.1 and iter_count > 10: - # damping = min(1.0, damping * 1.5) - # else: - # damping = max(0.01, damping * damping_reduction) - # - # J = pin.getFrameJacobian( - # self.model, - # self.data, - # ee_frame_id, - # pin.ReferenceFrame.LOCAL_WORLD_ALIGNED - # ) - # - # # ========================= - # # QP-based IK - # # ========================= - # - # H = J.T @ self.W @ J - # H += damping * damping * np.eye(7) - # - # H_triu = sparse.triu(H).tocsc() - # - # g = -J.T @ self.W @ error_vec - # - # # ------------------------- - # # Joint velocity constraints - # # ------------------------- - # - # dq_limit = 0.05 # rad per iteration - # - # lb = -dq_limit * np.ones(7) - # ub = dq_limit * np.ones(7) - # - # # ------------------------- - # # Joint position constraints - # # ------------------------- - # - # q_min_step = self.model.lowerPositionLimit[:7] - q[:7] - # q_max_step = self.model.upperPositionLimit[:7] - q[:7] - # - # lb = np.maximum(lb, q_min_step) - # ub = np.minimum(ub, q_max_step) - # - # # ------------------------- - # # Solve QP - # # ------------------------ - # # Update solver - # self.osqp_solver.update( - # Px=H_triu.data, - # q=g, - # l=lb, - # u=ub - # ) - # - # # Solve - # result = self.osqp_solver.solve() - # - # if result.info.status != 'solved': - # break - # - # dq = result.x - # - # if dq is None: - # break - # - # # Apply joint limits with scaling - # alpha = 0.5 - # q = pin.integrate(self.model, q, alpha * dq) - # - # prev_error = error_norm - # iter_count += 1 - # - # if best_solution is not None: - # return best_solution, True, best_error - # else: - # return None, False, None def compute_jacobian(self, joint_angles, tool="ee"): """Compute geometric Jacobian (6x7)""" @@ -930,7 +775,3 @@ def main(): if __name__ == "__main__": rm75, trajectory = main() - - print("\n" + "=" * 60) - print("All tests completed!") - print("=" * 60) \ No newline at end of file