diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py index 9d3ae78bc..0d60296b5 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py @@ -30,7 +30,9 @@ from compas_robots.model import Joint from compas.geometry import Frame +from compas.geometry import Quaternion from compas.geometry import is_parallel_vector_vector +from compas.geometry import axis_angle_from_quaternion from compas_fab.backends.exceptions import InverseKinematicsError from compas_fab.backends.exceptions import CollisionCheckError from compas_fab.backends.interfaces import InverseKinematics @@ -353,7 +355,7 @@ def set_random_config(): # Calling the IK function using the helper function that repeatedly # calls the pybullet IK solver until convergence. - joint_positions, close_enough = self._accurate_inverse_kinematics( + joint_positions = self._accurate_inverse_kinematics( joint_ids_sorted=joint_ids_sorted, tolerance_position=target.tolerance_position, tolerance_orientation=target.tolerance_orientation, @@ -362,12 +364,7 @@ def set_random_config(): **ik_options ) - # NOTE: In principle, this accurate iter IK should work out of the - # pybullet box, but the results seem to be way off target. For now, - # I'm leaving the legacy iterative accurate ik in python as per - # older examples of pybullet, until we figure out why the builtin - # one is not cooperating. - if not close_enough: + if not joint_positions: # If the solution is not close enough, we retry with a new randomized joint values set_random_config() continue @@ -681,7 +678,7 @@ def set_random_config(): ) def _accurate_inverse_kinematics( - self, joint_ids_sorted, tolerance_position, tolerance_orientation, max_iter, verbose=False, **kwargs + self, joint_ids_sorted, max_iter, tolerance_position=None, tolerance_orientation=None, verbose=False, **kwargs ): """Iterative inverse kinematics solver with a threshold for the distance to the target. @@ -694,13 +691,27 @@ def _accurate_inverse_kinematics( tuple of list of float, bool A tuple containing the joint positions and a boolean indicating if the solution is close enough to the target. """ + # NOTE: In principle, this accurate iter IK should work out of the + # pybullet box, but the results seem to be way off target. For now, + # I'm leaving the legacy iterative accurate ik in python as per + # older examples of pybullet, until we figure out why the builtin + # one is not cooperating. + # Based on these examples # https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_kinematics_husky_kuka.py#L81 - close_enough = False - iter = 0 body_id = kwargs["bodyUniqueId"] link_id = kwargs["endEffectorLinkIndex"] + + # Target position (Point) and orientation (convert to Quaternion) target_position = kwargs["targetPosition"] + target_orientation_xyzw = kwargs.get("targetOrientation", None) + if target_orientation_xyzw: + _x, _y, _z, _w = target_orientation_xyzw + target_quaternion = Quaternion(_w, _x, _y, _z) + target_orientation_frame = Frame.from_quaternion(target_quaternion) + else: + target_quaternion = None + tolerance_position = tolerance_position or self.DEFAULT_TARGET_TOLERANCE_POSITION tolerance_orientation = tolerance_orientation or self.DEFAULT_TARGET_TOLERANCE_ORIENTATION # Note: the following two options that was present in the PyBullet IK Example does not seem to do anything @@ -708,33 +719,47 @@ def _accurate_inverse_kinematics( # kwargs["maxNumIterations"] = max_iter # kwargs["residualThreshold"] = threshold - while not close_enough and iter < max_iter: + for i in range(max_iter): joint_poses = pybullet.calculateInverseKinematics(**kwargs) for i in range(len(joint_ids_sorted)): pybullet.resetJointState(body_id, joint_ids_sorted[i], joint_poses[i]) - + # Retrieve the last link state that contains the last link's position (index 4) and orientation (index 5) link_state = pybullet.getLinkState(body_id, link_id) - new_pose = link_state[4] + # Check tolerance_position + new_position = link_state[4] diff = [ - target_position[0] - new_pose[0], - target_position[1] - new_pose[1], - target_position[2] - new_pose[2], + target_position[0] - new_position[0], + target_position[1] - new_position[1], + target_position[2] - new_position[2], ] - # The distance is squared to avoid a sqrt operation - distance_squared = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2] # Therefor, the threshold is squared as well - # print("Iter: %d, Distance: %s" % (iter, distance_squared)) - close_enough = distance_squared < tolerance_position * tolerance_position - - # TODO: Check orientation as well + distance_squared = diff[0] ** 2 + diff[1] ** 2 + diff[2] ** 2 + tolerance_position_ok = distance_squared < tolerance_position**2 + # print("Iter: %d, Distance: %s" % (i, distance_squared)) + + # Check tolerance_orientation + if target_quaternion: + _x, _y, _z, _w = link_state[5] + new_quaternion = Quaternion(_w, _x, _y, _z) + new_frame = Frame.from_quaternion(new_quaternion) + delta_frame = target_orientation_frame.to_local_coordinates(new_frame) + _, angle = axis_angle_from_quaternion(Quaternion.from_frame(delta_frame)) + + tolerance_orientation_ok = angle < tolerance_orientation + # print("Iter: %d, Angle: %s" % (i, angle_squared)) + else: + tolerance_orientation_ok = True + if tolerance_position_ok and tolerance_orientation_ok: + if verbose: + print("Iterative IK took %d iterations" % isinstance) + return joint_poses kwargs["restPoses"] = joint_poses - iter += 1 - if verbose: - print("Iterative IK took %d iterations" % iter) - return joint_poses, close_enough + i += 1 + + return None def _check_configuration_match_group(self, start_configuration, configuration, group): # type: (Configuration, Configuration, str) -> None diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py index 06d164c6e..baebbd18d 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py @@ -1064,7 +1064,7 @@ def total_distance(self): @property def total_angle(self): # type: () -> float - """Tthe total angle between the start and end frames. + """Tte total angle between the start and end frames. Returns ------- diff --git a/src/compas_fab/backends/pybullet/conversions.py b/src/compas_fab/backends/pybullet/conversions.py index bca87d135..4958909a6 100644 --- a/src/compas_fab/backends/pybullet/conversions.py +++ b/src/compas_fab/backends/pybullet/conversions.py @@ -8,6 +8,7 @@ def pose_from_frame(frame): + # type: (Frame) -> tuple """Returns a PyBullet pose from a frame. Parameters diff --git a/tests/backends/pybullet/test_pybullet_planner.py b/tests/backends/pybullet/test_pybullet_planner_forward_kinematics.py similarity index 100% rename from tests/backends/pybullet/test_pybullet_planner.py rename to tests/backends/pybullet/test_pybullet_planner_forward_kinematics.py diff --git a/tests/backends/pybullet/test_pybullet_planner_inverse_kinematics.py b/tests/backends/pybullet/test_pybullet_planner_inverse_kinematics.py index 317df7f51..7aba73bc9 100644 --- a/tests/backends/pybullet/test_pybullet_planner_inverse_kinematics.py +++ b/tests/backends/pybullet/test_pybullet_planner_inverse_kinematics.py @@ -19,6 +19,9 @@ from compas_robots import Configuration from compas.geometry import Frame +from compas.geometry import Vector +from compas.geometry import matrix_from_frame +from compas.geometry import axis_and_angle_from_matrix @pytest.fixture @@ -43,6 +46,60 @@ def planner_with_test_cell(pybullet_client): return planner, robot_cell, robot_cell_state +def compare_frames(frame1, frame2, tolerance_position, tolerance_orientation): + # type: (Frame, Frame, float, float) -> bool + delta_frame = frame1.to_local_coordinates(frame2) # type: Frame + if Vector(*delta_frame.point).length > tolerance_position: + return False + axis, angle = axis_and_angle_from_matrix(matrix_from_frame(delta_frame)) + if angle > tolerance_orientation: + return False + return True + + +def test_frame_target_tolerance(planner_with_test_cell): + planner, robot_cell, robot_cell_state = planner_with_test_cell + group = robot_cell.robot.main_group_name + link_name = robot_cell.robot.get_end_effector_link_name(group) + result_cell_state = robot_cell_state.copy() # type: RobotCellState + + target_frame = Frame([0.5, 0.5, 0.5], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]) + options = { + "check_collision": False, + } + + def test_planning_tolerance(tolerance_position, tolerance_orientation): + # Test with a frame target with a different tolerance and we also check that the result does not over achieve + target = FrameTarget( + target_frame, + TargetMode.ROBOT, + tolerance_position=tolerance_position, + tolerance_orientation=tolerance_orientation, + ) + result = planner.inverse_kinematics(target, robot_cell_state, options=options) + result_cell_state.robot_configuration = result + result_frame = planner.forward_kinematics(result_cell_state, group, options={"link": link_name}) + assert isinstance(result, Configuration) + assert compare_frames(target_frame, result_frame, tolerance_position, tolerance_orientation) + # The result should not be 2 orders of magnitude better than the target tolerance + assert not compare_frames(target_frame, result_frame, tolerance_position * 1e-2, tolerance_orientation * 1e-2) + + # Test with unspecified tolerance (default position tolerance is 1e-3, equivalent to 1 mm) + test_planning_tolerance( + PyBulletPlanner.DEFAULT_TARGET_TOLERANCE_POSITION, + PyBulletPlanner.DEFAULT_TARGET_TOLERANCE_ORIENTATION, + ) + + # Test with a frame target with a different tolerance and we also check that the result does not over achieve + test_planning_tolerance(1e-2, 1e-2) + + # Test with higher tolerance + test_planning_tolerance(1e-4, 1e-4) + + # Test with even higher tolerance (1e-6 is equivalent to 1 micron) + test_planning_tolerance(1e-6, 1e-6) + + def test_inverse_kinematics_frame_target_target_modes(planner_with_test_cell): planner, robot_cell, robot_cell_state = planner_with_test_cell