From 745cf6e2051603dd30cbd1a64dfc29abb62d8e93 Mon Sep 17 00:00:00 2001
From: zhangshixiang <@zhangshixiang>
Date: Tue, 10 Jun 2025 01:36:04 +0800
Subject: [PATCH] fix ik error

---
 unilabos/devices/ros_dev/moveit_interface.py | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/unilabos/devices/ros_dev/moveit_interface.py b/unilabos/devices/ros_dev/moveit_interface.py
index 82b9564..d065403 100644
--- a/unilabos/devices/ros_dev/moveit_interface.py
+++ b/unilabos/devices/ros_dev/moveit_interface.py
@@ -292,9 +292,17 @@ class MoveitInterface:
                         )
                         end_pose = deep_pose
 
-                    retval_ik = self.moveit2[cmd_dict["move_group"]].compute_ik(
-                        position=end_pose, quat_xyzw=quaternion, constraints=Constraints(joint_constraints=constraints)
-                    )
+                    retval_ik = None
+                    retry = config.get("retry", 10)
+                    while retval_ik is None and retry > 0:
+                        retval_ik = self.moveit2[cmd_dict["move_group"]].compute_ik(
+                            position=end_pose, quat_xyzw=quaternion, constraints=Constraints(joint_constraints=constraints)
+                        )
+                        time.sleep(0.1)
+                        retry -= 1
+                    if retval_ik is None:
+                        result.success = False
+                        return result
                     position_ = [
                         retval_ik.position[retval_ik.name.index(i)]
                         for i in self.moveit2[cmd_dict["move_group"]].joint_names