wip - planner.ik with target and start_state

CHANGELOG.md

@@ -20,6 +20,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Changed
 
+* Calling `forward_kinematics` from the Robot class now uses only the RobotModel to calculate the forward kinematics.
 * Fixed error in `PyBulletForwardKinematics.forward_kinematics` where function would crash if `options` was not passed.
 * Fixed error in `PyBulletInverseKinematics._accurate_inverse_kinematics` where threshold was not squared for comparison.
 * Renamed `PybulletClient.get_cached_robot` to `PybulletClient.get_cached_robot_model` to avoid confusion between the `RobotModel` and `Robot` class.
@@ -39,6 +40,10 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Removed
 
+* Removed `inverse_kinematics`, `forward_kinematics`, `plan_cartesian_motion`, and `plan_motion` methods from ClientInterface, access them using the planner instead.
+* Removed `inverse_kinematics`, `plan_cartesian_motion`, and `plan_motion` methods from Robot, access them using the planner instead.
+* Removed `Robot.ensure_client` method. Client and planner now exist independently.
+* Removed `Robot.client` attribute. Access the planner functions directly using the planner instead.
 * Removed `plan_cartesian_motion_deprecated` and `plan_motion_deprecated` methods from `Robot` class
 * Removed `forward_kinematics_deprecated` and `inverse_kinematics_deprecated` method from `Robot` class
 

docs/examples/03_backends_ros/files/03_inverse_kinematics.py

@@ -1,13 +1,21 @@
 from compas.geometry import Frame
 from compas_fab.backends import RosClient
+from compas_fab.backends import MoveItPlanner
+
+from compas_fab.robots import FrameTarget
+from compas_fab.robots import RobotCellState
 
 with RosClient() as client:
     robot = client.load_robot()
-    assert robot.name == 'ur5_robot'
+    assert robot.name == "ur5_robot"
+    planner = MoveItPlanner(client)
 
     frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
+    target = FrameTarget(frame_WCF)
+
     start_configuration = robot.zero_configuration()
+    start_state = RobotCellState.from_robot_configuration(robot, start_configuration)
 
-    configuration = robot.inverse_kinematics(frame_WCF, start_configuration)
+    configuration = planner.inverse_kinematics(target, start_state)
 
     print("Found configuration", configuration)

docs/examples/03_backends_ros/files/03_iter_inverse_kinematics.py

@@ -1,12 +1,23 @@
 from compas.geometry import Frame
 from compas_fab.backends import RosClient
+from compas_fab.backends import MoveItPlanner
+
+from compas_fab.robots import FrameTarget
+from compas_fab.robots import RobotCellState
 
 with RosClient() as client:
     robot = client.load_robot()
-    assert robot.name == 'ur5_robot'
+    assert robot.name == "ur5_robot"
+    planner = MoveItPlanner(client)
 
     frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
+    target = FrameTarget(frame_WCF)
+
     start_configuration = robot.zero_configuration()
+    start_state = RobotCellState.from_robot_configuration(robot, start_configuration)
 
-    for config in robot.iter_inverse_kinematics(frame_WCF, start_configuration, options=dict(max_results=10)):
+    result_count = 0
+    for config in planner.iter_inverse_kinematics(target, start_state, options=dict(max_results=10)):
         print("Found configuration", config)
+        result_count += 1
+    print("Found %d configurations" % result_count)

docs/examples/05_backends_pybullet/files/02_inverse_kinematics.py

@@ -1,14 +1,22 @@
 import compas_fab
 from compas.geometry import Frame
 from compas_fab.backends import PyBulletClient
+from compas_fab.backends import PyBulletPlanner
+
+from compas_fab.robots import FrameTarget
+from compas_fab.robots import RobotCellState
 
 with PyBulletClient() as client:
-    urdf_filename = compas_fab.get('robot_library/ur5_robot/urdf/robot_description.urdf')
+    urdf_filename = compas_fab.get("robot_library/ur5_robot/urdf/robot_description.urdf")
     robot = client.load_robot(urdf_filename)
+    planner = PyBulletPlanner(client)
 
     frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
+    target = FrameTarget(frame_WCF)
+
     start_configuration = robot.zero_configuration()
+    start_state = RobotCellState.from_robot_configuration(robot, start_configuration)
 
-    configuration = robot.inverse_kinematics(frame_WCF, start_configuration)
+    configuration = planner.inverse_kinematics(target, start_state)
 
     print("Found configuration", configuration)

docs/examples/05_backends_pybullet/files/02_iter_inverse_kinematics.py

@@ -1,14 +1,25 @@
 import compas_fab
 from compas.geometry import Frame
 from compas_fab.backends import PyBulletClient
+from compas_fab.backends import PyBulletPlanner
 
-with PyBulletClient(connection_type='direct') as client:
-    urdf_filename = compas_fab.get('robot_library/ur5_robot/urdf/robot_description.urdf')
+from compas_fab.robots import FrameTarget
+from compas_fab.robots import RobotCellState
+
+with PyBulletClient(connection_type="direct") as client:
+    urdf_filename = compas_fab.get("robot_library/ur5_robot/urdf/robot_description.urdf")
     robot = client.load_robot(urdf_filename)
+    planner = PyBulletPlanner(client)
 
     frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
+    target = FrameTarget(frame_WCF)
+
     start_configuration = robot.zero_configuration()
+    start_state = RobotCellState.from_robot_configuration(robot, start_configuration)
 
-    options = dict(max_results=20, high_accuracy_threshold=1e-6, high_accuracy_max_iter=20)
-    for config in robot.iter_inverse_kinematics(frame_WCF, start_configuration, options=options):
+    options = dict(max_results=20, high_accuracy_threshold=1e-4, high_accuracy_max_iter=20)
+    result_count = 0
+    for config in planner.iter_inverse_kinematics(target, start_state, options=options):
         print("Found configuration", config)
+        result_count += 1
+    print("Found %d configurations" % result_count)

src/compas_fab/backends/interfaces/backend_features.py

@@ -10,10 +10,15 @@
     if TYPE_CHECKING:
         from typing import Optional  # noqa: F401
         from typing import Dict  # noqa: F401
+        from typing import List  # noqa: F401
+        from typing import Tuple  # noqa: F401
+
         from compas_fab.robots import Robot  # noqa: F401
         from compas_robots import Configuration  # noqa: F401
         from compas.geometry import Frame  # noqa: F401
         from compas_fab.backends.interfaces import ClientInterface  # noqa: F401
+        from compas_fab.robots import RobotCellState  # noqa: F401
+        from compas_fab.robots import FrameTarget  # noqa: F401
 
 
 class BackendFeature(object):
@@ -26,9 +31,11 @@ class BackendFeature(object):
 
     """
 
-    def __init__(self, client):
+    def __init__(self, client=None):
         # All backend features are assumed to be associated with a backend client.
-        self.client = client  # type: ClientInterface
+        if client:
+            self.client = client  # type: ClientInterface
+        super(BackendFeature, self).__init__()
 
 
 #   The code that contains the actual feature implementation is located in the backend's module.
@@ -73,34 +80,119 @@ def forward_kinematics(self, robot, configuration, group=None, options=None):
 class InverseKinematics(BackendFeature):
     """Mix-in interface for implementing a planner's inverse kinematics feature."""
 
-    def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=None, options=None):
+    def __init__(self):
+        # The following fields are used to store the last ik request for iterative calls
+        self._last_ik_request = {"request_hash": None, "solutions": None}
+
+        # Initialize the super class
+        print("InverseKinematics init")
+        super(InverseKinematics, self).__init__()
+
+    def inverse_kinematics(self, target, start_state=None, group=None, options=None):
+        # type: (FrameTarget, Optional[RobotCellState], Optional[str], Optional[Dict]) -> Tuple[List[float], List[str]]
         """Calculate the robot's inverse kinematic for a given frame.
 
-        Note that unlike other backend features, `inverse_kinematics` produces a generator.
+        The default implementation is based on the iter_inverse_kinematics method.
+        Calling this method will return the first solution found by the iterator,
+        subsequent calls will return the next solution from the iterator. Once
+        all solutions have been exhausted, the iterator will be re-initialized.
+
+        The starting state describes the robot cell's state at the moment of the calculation.
+        The robot's configuration is taken as the starting configuration.
+        If a tool is attached to the planning group, the tool's coordinate frame is used.
+
+        If the backend supports collision checking,
 
         Parameters
         ----------
         robot : :class:`compas_fab.robots.Robot`
             The robot instance for which inverse kinematics is being calculated.
-        frame_WCF : :class:`compas.geometry.Frame`
-            The frame to calculate the inverse for.
-        start_configuration : :class:`compas_robots.Configuration`, optional
+        target : :class:`compas_fab.robots.FrameTarget`
+            The frame target to calculate the inverse kinematics for.
+        start_state : :class:`compas_fab.robots.RobotCellState`, optional
+            The starting state to calculate the inverse kinematics for.
+            The robot's configuration in the scene is taken as the starting configuration.
         group : str, optional
             The planning group used for calculation.
         options : dict, optional
             Dictionary containing kwargs for arguments specific to
             the client being queried.
+            - ``"return_full_configuration"``: (:obj:`bool`) If ``True``, the full configuration
+                will be returned. Defaults to ``False``.
+            - ``"max_results"``: (:obj:`int`) Maximum number of results to return.
+                Defaults to ``100``.
 
-        Yields
+        Raises
         ------
+        :class: `compas_fab.backends.exceptions.InverseKinematicsError`
+            If no configuration can be found.
+
+        Returns
+        -------
         :obj:`tuple` of :obj:`list`
             A tuple of 2 elements containing a list of joint positions and a list of matching joint names.
         """
-        pass
+        # This is the default implementation for the inverse kinematics feature to be based on the
+        # iter_inverse_kinematics method. If the planner does not support this feature, it should
+        # override this method and raise a BackendFeatureNotSupportedError.
 
-        # The implementation code is located in the backend's module:
+        # The planner-specific implementation code is located in the backend's module:
         # "src/compas_fab/backends/<backend_name>/backend_features/<planner_name>_inverse_kinematics"
 
+        # Pseudo-memoized sequential calls will re-use iterator if not exhausted
+        request_hash = (target.sha256(), start_state.sha256(), str(group), str(options))
+
+        if self._last_ik_request["request_hash"] == request_hash and self._last_ik_request["solutions"] is not None:
+            solution = next(self._last_ik_request["solutions"], None)
+            if solution is not None:
+                return solution
+
+        solutions = self.iter_inverse_kinematics(target, start_state, group, options)
+        self._last_ik_request["request_hash"] = request_hash
+        self._last_ik_request["solutions"] = solutions
+
+        return next(solutions)
+
+    def iter_inverse_kinematics(self, target, start_state=None, group=None, options=None):
+        # type: (FrameTarget, Optional[RobotCellState], Optional[str], Optional[Dict]) -> Tuple[List[float], List[str]]
+        """Calculate the robot's inverse kinematic for a given frame.
+
+        This function returns a generator that yields possible solutions for the
+        inverse kinematics problem. The generator will be exhausted after all
+        possible solutions have been returned or when the maximum number of
+        results has been reached.
+
+        Parameters
+        ----------
+        frame_WCF: :class:`compas.geometry.Frame`
+            The frame to calculate the inverse for.
+        start_configuration: :class:`compas_fab.robots.Configuration`, optional
+            If passed, the inverse will be calculated such that the calculated
+            joint positions differ the least from the start_configuration.
+            Defaults to the zero configuration.
+        group: str, optional
+            The planning group used for calculation. Defaults to the robot's
+            main planning group.
+        options: dict, optional
+            Dictionary containing planner-specific arguments.
+            See the planner's documentation for supported options.
+            One of the supported options related to the iterator is:
+
+            - ``"max_results"``: (:obj:`int`) Maximum number of results to return.
+              Defaults to ``100``.
+
+        Raises
+        ------
+        compas_fab.backends.exceptions.InverseKinematicsError
+            If no configuration can be found.
+
+        Yields
+        ------
+        :obj:`tuple` of :obj:`list`
+            A tuple of 2 elements containing a list of joint positions and a list of matching joint names.
+        """
+        pass
+
 
 class PlanMotion(BackendFeature):
     """Mix-in interface for implementing a planner's plan motion feature."""