add collision detection in workspace cal

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)