From a41bd10179bd492564b651c74e459c10e2611933 Mon Sep 17 00:00:00 2001
From: Yijiang Huang <yijiangh@mit.edu>
Date: Mon, 21 Mar 2022 16:45:51 -0400
Subject: [PATCH] Add target_quat in IK with null_space

collision_fn diagnosis message show joint name
---
 CHANGELOG.rst                                      |  1 +
 .../interfaces/kinematics/ik_utils.py              | 14 +++++++++++---
 .../interfaces/robots/collision.py                 |  9 +++++----
 3 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index 7660447..3dd4380 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -16,6 +16,7 @@ Unreleased
 
 **Fixed**
 * Fixed `clone_body` bug when input links contains `BASE_LINK`
+* Fixed `inverse_kinematics_helper` to use `target_quat` when `null_space` is used
 
 **Deprecated**
 
diff --git a/src/pybullet_planning/interfaces/kinematics/ik_utils.py b/src/pybullet_planning/interfaces/kinematics/ik_utils.py
index 6fc558f..489005d 100644
--- a/src/pybullet_planning/interfaces/kinematics/ik_utils.py
+++ b/src/pybullet_planning/interfaces/kinematics/ik_utils.py
@@ -12,9 +12,12 @@ def inverse_kinematics_helper(robot, link, target_pose, null_space=None):
             assert target_quat is not None
             lower, upper, ranges, rest = null_space
 
-            kinematic_conf = p.calculateInverseKinematics(robot, link, target_point,
+            kinematic_conf = p.calculateInverseKinematics(robot, link, target_point, target_quat,
                                                           lowerLimits=lower, upperLimits=upper, jointRanges=ranges, restPoses=rest,
-                                                          physicsClientId=CLIENT)
+                                                          physicsClientId=CLIENT,
+                                                        #   maxNumIterations=1000,
+                                                        #   residualThreshold=1e-12,
+                                                          )
         elif target_quat is None:
             #ikSolver = p.IK_DLS or p.IK_SDLS
             kinematic_conf = p.calculateInverseKinematics(robot, link, target_point,
@@ -22,7 +25,12 @@ def inverse_kinematics_helper(robot, link, target_pose, null_space=None):
                                                           # solver=ikSolver, maxNumIterations=-1, residualThreshold=-1,
                                                           physicsClientId=CLIENT)
         else:
-            kinematic_conf = p.calculateInverseKinematics(robot, link, target_point, target_quat, physicsClientId=CLIENT)
+            # ! normal case
+            kinematic_conf = p.calculateInverseKinematics(robot, link, target_point, target_quat,
+                                                          physicsClientId=CLIENT,
+                                                        #   maxNumIterations=1000,
+                                                        #   residualThreshold=1e-12,
+                                                          )
     except p.error as e:
         kinematic_conf = None
     if (kinematic_conf is None) or any(map(math.isnan, kinematic_conf)):
diff --git a/src/pybullet_planning/interfaces/robots/collision.py b/src/pybullet_planning/interfaces/robots/collision.py
index ceb3f0b..23bc50d 100644
--- a/src/pybullet_planning/interfaces/robots/collision.py
+++ b/src/pybullet_planning/interfaces/robots/collision.py
@@ -383,9 +383,10 @@ def get_collision_fn(body, joints, obstacles=[],
         which link is colliding to which.
     """
     from pybullet_planning.interfaces.env_manager.pose_transformation import all_between
-    from pybullet_planning.interfaces.robots.joint import set_joint_positions, get_custom_limits
+    from pybullet_planning.interfaces.robots.joint import set_joint_positions, get_custom_limits, get_joint_name
     from pybullet_planning.interfaces.robots.link import get_self_link_pairs, get_moving_links
     from pybullet_planning.interfaces.debug_utils.debug_utils import draw_collision_diagnosis
+
     moving_links = frozenset(get_moving_links(body, joints))
     attached_bodies = [attachment.child for attachment in attachments]
     moving_bodies = [(body, moving_links)] + attached_bodies
@@ -430,13 +431,13 @@ def collision_fn(q, diagnosis=False):
         if not all_between(lower_limits, q, upper_limits):
             if diagnosis:
                 # warnings.warn('joint limit violation!', UserWarning)
-                cr = np.less_equal(q, lower_limits), np.less_equal(upper_limits, q)
+                cr = np.less(q, lower_limits), np.less(upper_limits, q)
                 LOGGER.warning('joint limit violation : {} / {}'.format(cr[0], cr[1]))
                 for i, (cr_l, cr_u) in enumerate(zip(cr[0], cr[1])):
                     if cr_l:
-                        LOGGER.warning('J{}: {} < lower limit {}'.format(i, q[i], lower_limits[i]))
+                        LOGGER.warning('J{} ({}): {} < lower limit {}'.format(i, get_joint_name(body, joints[i]), q[i], lower_limits[i]))
                     if cr_u:
-                        LOGGER.warning('J{}: {} > upper limit {}'.format(i, q[i], upper_limits[i]))
+                        LOGGER.warning('J{} ({}): {} > upper limit {}'.format(i, get_joint_name(body, joints[i]), q[i], upper_limits[i]))
             return True
         # * set body & attachment positions
         set_joint_positions(body, joints, q)