Add target_quat in IK with null_space

collision_fn diagnosis message show joint name
yijiangh · Mar 21, 2022 · a41bd10 · a41bd10
1 parent 2ae3826
commit a41bd10
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 7 deletions.
diff --git 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
@@ -12,17 +12,25 @@ 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,
                                                           #lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp, jointDamping=jd,
                                                           # 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
@@ -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)