Add translation_tolerance and rotation_tolerance to inverse_kinematics (#636)

- Add translation_tolerance parameter to allow per-axis position tolerance
  from target in target's local coordinate frame
- Add rotation_tolerance parameter to allow per-axis rotation tolerance
  from target orientation
- Support both inverse_kinematics and batch_inverse_kinematics methods
- Add --translation-tolerance and --rotation-tolerance CLI options to
  batch_ik_demo.py and pr2_inverse_kinematics.py examples
- Add unit tests for tolerance parameters
- Fix rotation error calculation in batch IK convergence check

Author: iory

examples/batch_ik_demo.py

@@ -45,6 +45,12 @@ def main():
                         help='Position error threshold in meters. Default: 0.001')
     parser.add_argument('--rthre', type=float, default=1.0,
                         help='Rotation error threshold in degrees. Default: 1.0')
+    parser.add_argument('--translation-tolerance', '--translation_tolerance',
+                        type=float, nargs=3, default=None, metavar=('X', 'Y', 'Z'),
+                        help='Translation tolerance per axis in meters (e.g., 0.05 0.05 0.05)')
+    parser.add_argument('--rotation-tolerance', '--rotation_tolerance',
+                        type=float, nargs=3, default=None, metavar=('R', 'P', 'Y'),
+                        help='Rotation tolerance per axis in degrees (e.g., 10 10 10)')
     parser.add_argument('--no-interactive', action='store_true',
                         help='Disable interactive visualization')
     parser.add_argument('--viewer', type=str,
@@ -59,6 +65,12 @@ def main():
     rotation_axis = parse_axis_constraint(args.rotation_axis)
     translation_axis = parse_axis_constraint(args.translation_axis)
 
+    # Process tolerance arguments
+    translation_tolerance = args.translation_tolerance
+    rotation_tolerance = None
+    if args.rotation_tolerance is not None:
+        rotation_tolerance = [np.deg2rad(v) for v in args.rotation_tolerance]
+
     print("ADVANCED BATCH IK - WITH AXIS CONSTRAINTS")
     print("=" * 55)
     print("Configuration:")
@@ -71,6 +83,10 @@ def main():
     print(f"   Max iterations: {args.stop}")
     print(f"   Position threshold: {args.thre}m")
     print(f"   Rotation threshold: {args.rthre} degrees")
+    if translation_tolerance is not None:
+        print(f"   Translation tolerance: {translation_tolerance}m")
+    if args.rotation_tolerance is not None:
+        print(f"   Rotation tolerance: {args.rotation_tolerance} degrees")
 
     # Initialize robot based on selection
     if args.robot == 'fetch':
@@ -152,6 +168,10 @@ def main():
         'rthre': np.deg2rad(args.rthre),
         'attempts_per_pose': args.attempts_per_pose,
     }
+    if translation_tolerance is not None:
+        ik_kwargs['translation_tolerance'] = translation_tolerance
+    if rotation_tolerance is not None:
+        ik_kwargs['rotation_tolerance'] = rotation_tolerance
 
     # For robots with inverse_kinematics_defaults, use those settings
     if hasattr(arm, 'inverse_kinematics_defaults'):
@@ -362,9 +382,9 @@ def main():
                 print(f"Visualization error: {e}")
 
         else:
-            print("No successful solutions to visualize")
+            print("Interactive visualization skipped (--no-interactive)")
     else:
-        print("Interactive visualization skipped (--no-interactive)")
+        print("No successful solutions to verify")
 
 
 if __name__ == '__main__':

examples/pr2_inverse_kinematics.py

@@ -165,8 +165,24 @@ def main():
         action='store_true',
         help="Skip the revert_if_fail demonstration"
     )
+    parser.add_argument(
+        '--translation-tolerance', '--translation_tolerance',
+        type=float, nargs=3, default=None, metavar=('X', 'Y', 'Z'),
+        help='Translation tolerance per axis in meters (e.g., 0.05 0.05 0.05)'
+    )
+    parser.add_argument(
+        '--rotation-tolerance', '--rotation_tolerance',
+        type=float, nargs=3, default=None, metavar=('R', 'P', 'Y'),
+        help='Rotation tolerance per axis in degrees (e.g., 10 10 10)'
+    )
     args = parser.parse_args()
 
+    # Process tolerance arguments
+    translation_tolerance = args.translation_tolerance
+    rotation_tolerance = None
+    if args.rotation_tolerance is not None:
+        rotation_tolerance = [np.deg2rad(v) for v in args.rotation_tolerance]
+
     # Create robot model
     robot_model = skrobot.models.PR2()
     robot_model.reset_pose()
@@ -196,6 +212,10 @@ def main():
     # Define target position
     target_pos = [0.7, -0.2, 0.8]
     print("Solving inverse kinematics for target: {}".format(target_pos))
+    if translation_tolerance is not None:
+        print("Translation tolerance: {}m".format(translation_tolerance))
+    if args.rotation_tolerance is not None:
+        print("Rotation tolerance: {} degrees".format(args.rotation_tolerance))
 
     # Create target coordinates
     target_coords = skrobot.coordinates.Coordinates(target_pos, [0, 0, 0])
@@ -211,22 +231,23 @@ def main():
     viewer.redraw()
     print("Target position visualized with coordinate frame")
 
+    # Build IK kwargs
+    ik_kwargs = {
+        'link_list': link_list,
+        'move_target': robot_model.rarm_end_coords,
+        'rotation_axis': True,
+    }
+    if translation_tolerance is not None:
+        ik_kwargs['translation_tolerance'] = translation_tolerance
+    if rotation_tolerance is not None:
+        ik_kwargs['rotation_tolerance'] = rotation_tolerance
+
     # Solve inverse kinematics with optional visualization
     if not args.no_ik_visualization:
         with ik_visualization(viewer, sleep_time=0.5):
-            result = robot_model.inverse_kinematics(
-                target_coords,
-                link_list=link_list,
-                move_target=robot_model.rarm_end_coords,
-                rotation_axis=True
-            )
+            result = robot_model.inverse_kinematics(target_coords, **ik_kwargs)
     else:
-        result = robot_model.inverse_kinematics(
-            target_coords,
-            link_list=link_list,
-            move_target=robot_model.rarm_end_coords,
-            rotation_axis=True
-        )
+        result = robot_model.inverse_kinematics(target_coords, **ik_kwargs)
 
     # Check if result is successful (could be False or an array)
     success = result is not False and result is not None

skrobot/model/robot_model.py

@@ -980,7 +980,44 @@ def inverse_kinematics(
             check_collision=None,
             additional_jacobi=None,
             additional_vel=None,
+            translation_tolerance=None,
+            rotation_tolerance=None,
             **kwargs):
+        """Solve inverse kinematics to reach target coordinates.
+
+        Parameters
+        ----------
+        target_coords : skrobot.coordinates.Coordinates or list
+            Target coordinate(s) for the end-effector(s).
+        stop : int
+            Maximum number of iterations.
+        link_list : list
+            List of links to use for IK.
+        move_target : skrobot.coordinates.Coordinates or list
+            End-effector coordinate frame(s).
+        revert_if_fail : bool
+            If True, revert to initial joint angles on failure.
+        rotation_axis : bool, str, or list
+            Which rotation axes to constrain. True=all, False=none,
+            'x'/'y'/'z'=single axis, 'xy'/'yz'/'zx'=two axes.
+        translation_axis : bool, str, or list
+            Which translation axes to constrain. Same format as rotation_axis.
+        translation_tolerance : list or None
+            Per-axis position tolerance from target as [x_tol, y_tol, z_tol]
+            in meters. If error on an axis is within tolerance, it's treated
+            as reached. Use None for no tolerance on an axis.
+        rotation_tolerance : list or None
+            Per-axis rotation tolerance from target as
+            [roll_tol, pitch_tol, yaw_tol] in radians. If error on an axis
+            is within tolerance, it's treated as reached.
+        **kwargs
+            Additional keyword arguments passed to inverse_kinematics_loop.
+
+        Returns
+        -------
+        numpy.ndarray or False
+            Joint angle vector if successful, False otherwise.
+        """
         additional_jacobi = additional_jacobi or []
         additional_vel = additional_vel or []
         target_coords = listify(target_coords)
@@ -1079,11 +1116,37 @@ def inverse_kinematics(
             dif_pos = list(map(lambda mv, tc, trans_axis:
                                mv.difference_position(
                                    tc, translation_axis=trans_axis),
-                               move_target, target_coords, translation_axis))
+                               move_target, target_coords,
+                               translation_axis))
             dif_rot = list(map(lambda mv, tc, rot_axis:
                                mv.difference_rotation(
                                    tc, rotation_axis=rot_axis),
-                               move_target, target_coords, rotation_axis))
+                               move_target, target_coords,
+                               rotation_axis))
+
+            # Apply translation tolerance in target_coords' local frame
+            # Note: tol > 0 check ensures 0.0 means "no special tolerance"
+            if translation_tolerance is not None:
+                for i, (mt, tc) in enumerate(zip(move_target, target_coords)):
+                    # Calculate error in target_coords' local frame
+                    world_error = tc.worldpos() - mt.worldpos()
+                    local_error = tc.worldrot().T @ world_error
+                    for axis_idx, tol in enumerate(translation_tolerance):
+                        if tol is not None and tol > 0:
+                            if abs(local_error[axis_idx]) <= tol:
+                                # Zero out the corresponding dif_pos component
+                                # dif_pos is in move_target's frame, so transform
+                                target_axis_in_mt = mt.worldrot().T @ tc.worldrot()[:, axis_idx]
+                                dif_pos[i] -= (dif_pos[i] @ target_axis_in_mt) * target_axis_in_mt
+
+            # Apply rotation tolerance: if error is within tolerance,
+            # treat as reached (set dif to 0)
+            if rotation_tolerance is not None:
+                for i, dr in enumerate(dif_rot):
+                    for axis_idx, tol in enumerate(rotation_tolerance):
+                        if tol is not None and tol > 0 and abs(dr[axis_idx]) <= tol:
+                            dif_rot[i][axis_idx] = 0.0
+
             if loop == 1 and self.ik_convergence_check(
                     dif_pos, dif_rot, rotation_axis, translation_axis,
                     thre, rthre, centroid_thre, target_centroid_pos,
@@ -2434,6 +2497,8 @@ def batch_inverse_kinematics(
             alpha=1.0,
             attempts_per_pose=1,
             random_initial_range=0.7,
+            translation_tolerance=None,
+            rotation_tolerance=None,
             **kwargs):
         """Solve batch inverse kinematics for multiple target poses.
 
@@ -2474,6 +2539,14 @@ def batch_inverse_kinematics(
             Number of attempts with different random initial poses per target (default: 1)
         random_initial_range : float
             Range for random initial poses as fraction of joint limits (0.0-1.0, default: 0.7)
+        translation_tolerance : list or None
+            Per-axis position tolerance from target as [x_tol, y_tol, z_tol]
+            in meters. If error on an axis is within tolerance, it's treated
+            as reached. Use None for no tolerance on an axis.
+        rotation_tolerance : list or None
+            Per-axis rotation tolerance from target as
+            [roll_tol, pitch_tol, yaw_tol] in radians. If error on an axis
+            is within tolerance, it's treated as reached.
         **kwargs : dict
             Additional keyword arguments
 
@@ -2506,12 +2579,14 @@ def batch_inverse_kinematics(
         return self._batch_inverse_kinematics_impl(
             target_coords, move_target, link_list,
             rotation_axis, translation_axis, stop, thre, rthre,
-            initial_angles, alpha, attempts_per_pose, random_initial_range, **kwargs)
+            initial_angles, alpha, attempts_per_pose, random_initial_range,
+            translation_tolerance, rotation_tolerance, **kwargs)
 
     def _batch_inverse_kinematics_impl(
             self, target_coords, move_target, link_list,
             rotation_axis, translation_axis, stop, thre, rthre,
-            initial_angles, alpha, attempts_per_pose, random_initial_range, **kwargs):
+            initial_angles, alpha, attempts_per_pose, random_initial_range,
+            translation_tolerance, rotation_tolerance, **kwargs):
         """Internal implementation of batch inverse kinematics."""
         # Auto-adjust initial_angles based on attempts_per_pose
         if isinstance(initial_angles, str) and initial_angles == "current" and attempts_per_pose > 1:
@@ -2635,7 +2710,8 @@ def _batch_inverse_kinematics_impl(
             stop, thre, rthre, alpha, base_to_source,
             rotation_axis, translation_axis, actual_joint_list,
             attempts_per_pose, random_initial_range,
-            move_target_offset_pos, move_target_offset_rot
+            move_target_offset_pos, move_target_offset_rot,
+            translation_tolerance, rotation_tolerance
         )
 
         full_solutions = []
@@ -2655,7 +2731,8 @@ def _solve_batch_ik(
             stop, thre, rthre, alpha, base_to_source,
             rotation_axis, translation_axis, joint_list_without_fixed,
             attempts_per_pose, random_initial_range,
-            move_target_offset_pos=None, move_target_offset_rot=None):
+            move_target_offset_pos=None, move_target_offset_rot=None,
+            translation_tolerance=None, rotation_tolerance=None):
         """Batch IK solver using batch expansion for multiple attempts."""
 
         n_poses = target_poses_xyz_qwxyz.shape[0]
@@ -2704,7 +2781,8 @@ def _solve_batch_ik(
             stop, thre, rthre, alpha, base_to_source,
             expanded_rotation_axis, expanded_translation_axis,
             joint_list_without_fixed, min_angles, max_angles,
-            move_target_offset_pos, move_target_offset_rot
+            move_target_offset_pos, move_target_offset_rot,
+            translation_tolerance, rotation_tolerance
         )
 
         solutions = []
@@ -2740,7 +2818,8 @@ def _solve_batch_ik_internal(
             stop, thre, rthre, alpha, base_to_source,
             rotation_axis, translation_axis,
             joint_list_without_fixed, min_angles, max_angles,
-            move_target_offset_pos=None, move_target_offset_rot=None):
+            move_target_offset_pos=None, move_target_offset_rot=None,
+            translation_tolerance=None, rotation_tolerance=None):
         """Internal batch IK solver."""
         # Use integrated batch IK functions
 
@@ -2781,7 +2860,9 @@ def _solve_batch_ik_internal(
                 joint_list_without_fixed, min_angles, max_angles,
                 joint_list_for_fk=None,  # Will be derived from link_list in the function
                 move_target_offset_pos=move_target_offset_pos,
-                move_target_offset_rot=move_target_offset_rot
+                move_target_offset_rot=move_target_offset_rot,
+                translation_tolerance=translation_tolerance,
+                rotation_tolerance=rotation_tolerance
             )
 
             # Update current angles first
@@ -2805,8 +2886,12 @@ def _solve_batch_ik_internal(
             pose_errors = np.zeros((len(unsolved_indices), 6))
             translation_errors = target_poses_unsolved[:, :3] - current_poses_updated[:, :3]
             pose_errors[:, :3] = translation_errors
-            # Rotation errors (simplified - assume no rotation constraint for now)
-            pose_errors[:, 3:] = 0
+            # Calculate rotation errors using quaternion difference
+            current_quat_inv = quaternion_inverse(current_poses_updated[:, 3:])
+            rotation_error_quat = quaternion_multiply(
+                target_poses_unsolved[:, 3:], current_quat_inv)
+            rotation_error_rpy = quaternion2rpy(rotation_error_quat)[0][:, ::-1]
+            pose_errors[:, 3:] = rotation_error_rpy
 
             # Check convergence with respect to constraints
             converged = np.zeros(len(unsolved_indices), dtype=bool)
@@ -2857,6 +2942,31 @@ def _solve_batch_ik_internal(
                         if 'z' not in rot_axis.lower():
                             constrained_pose_error[5] = 0
 
+                # Apply translation tolerance in target's local frame
+                # Note: tol > 0 check ensures 0.0 means "no special tolerance"
+                if translation_tolerance is not None:
+                    # Get target rotation matrix
+                    target_rot = quaternion2matrix(target_poses_unsolved[i, 3:])
+                    # Calculate error in target's local frame
+                    world_error = target_poses_unsolved[i, :3] - current_poses_updated[i, :3]
+                    local_error = target_rot.T @ world_error
+                    for axis_idx, tol in enumerate(translation_tolerance):
+                        if tol is not None and tol > 0:
+                            if abs(local_error[axis_idx]) <= tol:
+                                # Zero out this axis in constrained_pose_error
+                                # by projecting out the target axis component
+                                target_axis = target_rot[:, axis_idx]
+                                proj = (constrained_pose_error[:3] @ target_axis)
+                                constrained_pose_error[:3] -= proj * target_axis
+
+                # Apply rotation tolerance: if error is within tolerance,
+                # treat as reached (set to 0)
+                if rotation_tolerance is not None:
+                    for axis_idx, tol in enumerate(rotation_tolerance):
+                        if tol is not None and tol > 0:
+                            if abs(constrained_pose_error[3 + axis_idx]) <= tol:
+                                constrained_pose_error[3 + axis_idx] = 0.0
+
                 # Check convergence only for active constraints
                 # Note: pose_errors is in [translation, rotation] order after reordering
                 position_error = np.linalg.norm(constrained_pose_error[:3])
@@ -2998,7 +3108,8 @@ def _batch_ik_step_with_constraints(
             rotation_axis_list, translation_axis_list,
             actual_joint_list, min_angles, max_angles,
             joint_list_for_fk=None,
-            move_target_offset_pos=None, move_target_offset_rot=None):
+            move_target_offset_pos=None, move_target_offset_rot=None,
+            translation_tolerance=None, rotation_tolerance=None):
         """Batch IK step with per-problem axis constraints.
 
         Args:
@@ -3140,6 +3251,32 @@ def _batch_ik_step_with_constraints(
         pose_errors[:, 3:] = rotation_error_rpy
         pose_errors = pose_errors[:, [3, 4, 5, 0, 1, 2]]
 
+        # Apply tolerance in target's local frame
+        # pose_errors is now in [rot, rot, rot, trans, trans, trans] order
+        # Note: tol > 0 check ensures 0.0 means "no special tolerance"
+        if translation_tolerance is not None:
+            for b in range(batch_size):
+                # Get target rotation matrix
+                target_rot = quaternion2matrix(target_poses[b, 3:])
+                # Calculate error in target's local frame
+                world_error = target_poses[b, :3] - current_poses[b, :3]
+                local_error = target_rot.T @ world_error
+                for axis_idx, tol in enumerate(translation_tolerance):
+                    if tol is not None and tol > 0:
+                        if abs(local_error[axis_idx]) <= tol:
+                            # Zero out this axis by projecting out the target axis
+                            target_axis = target_rot[:, axis_idx]
+                            # Translation is at indices 3, 4, 5 after reordering
+                            proj = pose_errors[b, 3:6] @ target_axis
+                            pose_errors[b, 3:6] -= proj * target_axis
+
+        if rotation_tolerance is not None:
+            for axis_idx, tol in enumerate(rotation_tolerance):
+                if tol is not None and tol > 0:
+                    # Rotation is at indices 0, 1, 2 after reordering
+                    mask = np.abs(pose_errors[:, axis_idx]) <= tol
+                    pose_errors[mask, axis_idx] = 0.0
+
         # Apply constraints and calculate delta for each problem
         joint_angle_deltas = np.zeros((batch_size, ndof))