diff --git a/CHANGELOG.md b/CHANGELOG.md index a9664794b..ff0bc03ac 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/docs/examples/03_backends_ros/files/03_inverse_kinematics.py b/docs/examples/03_backends_ros/files/03_inverse_kinematics.py index b67e39952..055cebabb 100644 --- a/docs/examples/03_backends_ros/files/03_inverse_kinematics.py +++ b/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) diff --git a/docs/examples/03_backends_ros/files/03_iter_inverse_kinematics.py b/docs/examples/03_backends_ros/files/03_iter_inverse_kinematics.py index e3f298b25..e922ddc49 100644 --- a/docs/examples/03_backends_ros/files/03_iter_inverse_kinematics.py +++ b/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) diff --git a/docs/examples/05_backends_pybullet/files/02_inverse_kinematics.py b/docs/examples/05_backends_pybullet/files/02_inverse_kinematics.py index ca2cc15b5..d0aaf4cbe 100644 --- a/docs/examples/05_backends_pybullet/files/02_inverse_kinematics.py +++ b/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) diff --git a/docs/examples/05_backends_pybullet/files/02_iter_inverse_kinematics.py b/docs/examples/05_backends_pybullet/files/02_iter_inverse_kinematics.py index f28e47125..e696756d3 100644 --- a/docs/examples/05_backends_pybullet/files/02_iter_inverse_kinematics.py +++ b/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) diff --git a/src/compas_fab/backends/interfaces/backend_features.py b/src/compas_fab/backends/interfaces/backend_features.py index 0e69a94df..595027f5c 100644 --- a/src/compas_fab/backends/interfaces/backend_features.py +++ b/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_features/_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.""" diff --git a/src/compas_fab/backends/interfaces/client.py b/src/compas_fab/backends/interfaces/client.py index ecde96520..e8a050386 100644 --- a/src/compas_fab/backends/interfaces/client.py +++ b/src/compas_fab/backends/interfaces/client.py @@ -1,113 +1,78 @@ from __future__ import absolute_import from __future__ import division from __future__ import print_function -from compas_fab.backends.exceptions import BackendFeatureNotSupportedError +import compas + +from compas_fab.backends.exceptions import BackendFeatureNotSupportedError -def forward_docstring(backend_feature): - def dec(obj): - obj.__doc__ = backend_feature.__dict__[obj.__name__].__doc__ - return obj +if compas.IPY: + from typing import TYPE_CHECKING - return dec + if TYPE_CHECKING: + from compas_fab.robots import Robot + from compas_fab.robots import RobotCell class ClientInterface(object): """Interface for all backend clients. Forwards all planning services and planning scene management to the planner. + + Attributes + ---------- + robot : :class:`compas_fab.robots.Robot`, read-only + The robot instance associated with the client. + Typically this is set by the backend client after it is initialized, using + `client.load_robot()`. It cannot be changed after it is set. + Consult the chosen backend client for how to set this attribute. """ def __init__(self): - self.planner = PlannerInterface(self) - # self.control = ControlInterface() - - # ========================================================================== - # planning services - # ========================================================================== - - def inverse_kinematics(self, *args, **kwargs): - """Forwards call to appropriate method in the planner.""" - return self.planner.inverse_kinematics(*args, **kwargs) - - def forward_kinematics(self, *args, **kwargs): - """Forwards call to appropriate method in the planner.""" - return self.planner.forward_kinematics(*args, **kwargs) - - def plan_cartesian_motion(self, *args, **kwargs): - """Forwards call to appropriate method in the planner.""" - return self.planner.plan_cartesian_motion(*args, **kwargs) - - def plan_motion(self, *args, **kwargs): - """Forwards call to appropriate method in the planner.""" - return self.planner.plan_motion(*args, **kwargs) - - # ========================================================================== - # collision objects and planning scene - # ========================================================================== + self._robot = None # type: Robot + self._robot_cell = None # type: RobotCell + print("ClientInterface init") - def get_planning_scene(self, *args, **kwargs): - """Forwards call to appropriate method in the planner.""" - return self.planner.get_planning_scene(*args, **kwargs) + @property + def robot(self): + # type: () -> Robot + return self._robot - def reset_planning_scene(self, *args, **kwargs): - """Forwards call to appropriate method in the planner.""" - return self.planner.reset_planning_scene(*args, **kwargs) - - def add_collision_mesh(self, *args, **kwargs): - """Forwards call to appropriate method in the planner.""" - return self.planner.add_collision_mesh(*args, **kwargs) - - def remove_collision_mesh(self, *args, **kwargs): - """Forwards call to appropriate method in the planner.""" - return self.planner.remove_collision_mesh(*args, **kwargs) - - def append_collision_mesh(self, *args, **kwargs): - """Forwards call to appropriate method in the planner.""" - return self.planner.append_collision_mesh(*args, **kwargs) - - def add_attached_collision_mesh(self, *args, **kwargs): - """Forwards call to appropriate method in the planner.""" - return self.planner.add_attached_collision_mesh(*args, **kwargs) - - def remove_attached_collision_mesh(self, *args, **kwargs): - """Forwards call to appropriate method in the planner.""" - return self.planner.remove_attached_collision_mesh(*args, **kwargs) - - -# # ========================================================================== -# # executing -# # ========================================================================== -# -# def execute_joint_trajectory(self, *args, **kwargs): -# self.control.execute_joint_trajectory(*args, **kwargs) -# -# def follow_joint_trajectory(self, *args, **kwargs): -# self.control.follow_joint_trajectory(*args, **kwargs) -# -# def get_configuration(self, *args, **kwargs): -# self.control.get_configuration(*args, **kwargs) -# -# -# class ControlInterface(object): -# def execute_joint_trajectory(self, *args, **kwargs): -# raise NotImplementedError('Assigned control does not have this feature.') -# -# def follow_joint_trajectory(self, *args, **kwargs): -# raise NotImplementedError('Assigned control does not have this feature.') -# -# def get_configuration(self, *args, **kwargs): -# raise NotImplementedError('Assigned control does not have this feature.') + @property + def robot_cell(self): + # type: () -> RobotCell + return self._robot_cell class PlannerInterface(object): - """Interface for all planners associated with a backend client. Provides default - behavior for all planning services and planning scene management methods. To be - use in conjunction with backend feature interfaces. + """Interface for all backend planners. + Planner is connected to a ClientInterface when initiated, and can access + the client's robot instance. + It is responsible for all planning services for the robot. + + Parameters + ---------- + client : :class:`compas_fab.backends.interfaces.ClientInterface`, optional + The client instance associated with the planner. + Typically this is set by the backend client during initialization, + using `planner = PlannerInterface(client)`. + Consult the chosen backend client for how to set this attribute. + + Attributes + ---------- + client : :class:`compas_fab.backends.interfaces.ClientInterface`, read-only + The client instance associated with the planner. + It cannot be changed after initialization. """ - def __init__(self, client): - # super(PlannerInterface, self).__init__() - self.client = client + def __init__(self, client=None): + if client: + self._client = client + + super(PlannerInterface, self).__init__() + + @property + def client(self): + return self._client # ========================================================================== # planning services 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 eee4e1f2e..e18ea9b21 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 @@ -2,6 +2,24 @@ from __future__ import division from __future__ import print_function +import compas + +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from compas_robots import Configuration # noqa: F401 + from compas.geometry import Frame # noqa: F401 + from typing import Optional # noqa: F401 + from typing import Iterator # noqa: F401 + from typing import List # noqa: F401 + from typing import Tuple # noqa: F401 + from typing import Dict # noqa: F401 + from compas_fab.robots import RobotCellState # noqa: F401 + from compas_fab.robots import FrameTarget # noqa: F401 + from compas_fab.robots import AttachedCollisionMesh # noqa: F401 + + import math import random @@ -11,6 +29,7 @@ from compas_fab.backends.interfaces import InverseKinematics from compas_fab.backends.pybullet.conversions import pose_from_frame from compas_fab.utilities import LazyLoader +from compas_fab.utilities import from_tcf_to_t0cf pybullet = LazyLoader("pybullet", globals(), "pybullet") @@ -23,19 +42,16 @@ class PyBulletInverseKinematics(InverseKinematics): """Callable to calculate the robot's inverse kinematics for a given frame.""" - def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=None, options=None): + def iter_inverse_kinematics(self, target, start_state=None, group=None, options=None): + # type: (FrameTarget, Optional[RobotCellState], Optional[str], Optional[Dict]) -> Iterator[Tuple[List[float], List[str]]] """Calculate the robot's inverse kinematic for a given frame. 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_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. + target: :class:`compas.geometry.FrameTarget` + The Frame Target to calculate the inverse for. + start_state : :class:`compas_fab.robots.RobotCellState`, optional + The starting state to calculate the inverse kinematics for. group: str, optional The planning group used for determining the end effector and labeling the ``start_configuration``. Defaults to the robot's main planning group. @@ -46,7 +62,7 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N to compute the inverse kinematics. Defaults to the given group's end effector. - ``"semi-constrained"``: (:obj:`bool`, optional) When ``True``, only the - position and not the orientation of ``frame_WCF`` will not be considered + position of the target is considered. The orientation of frame will not be considered in the calculation. Defaults to ``False``. - ``"enforce_joint_limits"``: (:obj:`bool`, optional) When ``False``, the robot's joint limits will be ignored in the calculation. Defaults to @@ -55,7 +71,7 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N solver will iteratively try to approach minimum deviation from the requested target frame. Defaults to ``True``. - ``"high_accuracy_threshold"``: (:obj:`float`, optional) Defines the maximum - acceptable distance threshold for the high accuracy solver. Defaults to ``1e-6``. + acceptable distance threshold for the high accuracy solver. Defaults to ``1e-4``. - ``"high_accuracy_max_iter"``: (:obj:`float`, optional) Defines the maximum number of iterations to use for the high accuracy solver. Defaults to ``20``. - ``"max_results"``: (:obj:`int`) Maximum number of results to return. @@ -71,13 +87,27 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N :class:`compas_fab.backends.InverseKinematicsError` """ options = options or {} + robot = self.client.robot + high_accuracy = options.get("high_accuracy", True) max_results = options.get("max_results", 100) link_name = options.get("link_name") or robot.get_end_effector_link_name(group) + cached_robot_model = self.client.get_cached_robot_model(robot) body_id = self.client.get_uid(cached_robot_model) link_id = self.client._get_link_id_by_name(link_name, cached_robot_model) - point, orientation = pose_from_frame(frame_WCF) + + # Target frame and Tool Coordinate Frame + target_frame = target.target_frame + if robot.need_scaling: + target_frame = target_frame.scaled(1.0 / robot.scale_factor) + # Now target_frame is back in meter scale + attached_tool = self.client.robot_cell.get_attached_tool(start_state, group) + if attached_tool: + target_frame = from_tcf_to_t0cf(target_frame, attached_tool.frame) + # Attached tool frames does not need scaling because Tools are modelled in meter scale + + point, orientation = pose_from_frame(target_frame) joints = cached_robot_model.get_configurable_joints() joints.sort(key=lambda j: j.attr["pybullet"]["id"]) @@ -86,62 +116,67 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N lower_limits = [joint.limit.lower if joint.type != Joint.CONTINUOUS else 0 for joint in joints] upper_limits = [joint.limit.upper if joint.type != Joint.CONTINUOUS else 2 * math.pi for joint in joints] - start_configuration = start_configuration or robot.zero_configuration(group) + start_configuration = start_state.robot_configuration or robot.zero_configuration(group) start_configuration = self.client.set_robot_configuration(robot, start_configuration, group) + # Rest pose is PyBullet's way of defining the initial guess for the IK solver + # The order of the values needs to match with pybullet's joint id order rest_poses = self._get_rest_poses(joint_names, start_configuration) - for _ in range(max_results): - ik_options = dict( - bodyUniqueId=body_id, - endEffectorLinkIndex=link_id, - targetPosition=point, - physicsClientId=self.client.client_id, - ) - - if options.get("enforce_joint_limits", True): - # I don't know what jointRanges needs to be. Erwin Coumans knows, but he isn't telling. - # https://stackoverflow.com/questions/49674179/understanding-inverse-kinematics-pybullet - # https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/preview?pru=AAABc7276PI*zazLer2rlZ8tAUI8lF98Kw#heading=h.9i02ojf4k3ve - joint_ranges = [u - l for u, l in zip(upper_limits, lower_limits)] - - if options.get("semi-constrained"): - ik_options.update( - dict( - lowerLimits=lower_limits, - upperLimits=upper_limits, - jointRanges=joint_ranges, - restPoses=rest_poses, - ) - ) - else: - ik_options.update( - dict( - targetPosition=point, - targetOrientation=orientation, - lowerLimits=lower_limits, - upperLimits=upper_limits, - jointRanges=joint_ranges, - restPoses=rest_poses, - ) + # Prepare Parameters for calling pybullet.calculateInverseKinematics + ik_options = dict( + bodyUniqueId=body_id, + endEffectorLinkIndex=link_id, + targetPosition=point, + physicsClientId=self.client.client_id, + ) + + if options.get("enforce_joint_limits", True): + # I don't know what jointRanges needs to be. Erwin Coumans knows, but he isn't telling. + # https://stackoverflow.com/questions/49674179/understanding-inverse-kinematics-pybullet + # https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/preview?pru=AAABc7276PI*zazLer2rlZ8tAUI8lF98Kw#heading=h.9i02ojf4k3ve + joint_ranges = [u - l for u, l in zip(upper_limits, lower_limits)] + + if options.get("semi-constrained"): + ik_options.update( + dict( + lowerLimits=lower_limits, + upperLimits=upper_limits, + jointRanges=joint_ranges, + restPoses=rest_poses, ) + ) else: - if not options.get("semi-constrained"): - ik_options.update( - dict( - targetOrientation=orientation, - ) + ik_options.update( + dict( + targetPosition=point, + targetOrientation=orientation, + lowerLimits=lower_limits, + upperLimits=upper_limits, + jointRanges=joint_ranges, + restPoses=rest_poses, ) - - # Ready to call IK - if high_accuracy: + ) + else: + if not options.get("semi-constrained"): ik_options.update( dict( - joints=joints, - threshold=options.get("high_accuracy_threshold", 1e-4), - max_iter=options.get("high_accuracy_max_iter", 20), + targetOrientation=orientation, ) ) + if high_accuracy: + ik_options.update( + dict( + joints=joints, + threshold=options.get("high_accuracy_threshold", 1e-4), + max_iter=options.get("high_accuracy_max_iter", 20), + ) + ) + # Loop to get multiple results + for _ in range(max_results): + + # Ready to call IK + if high_accuracy: joint_positions, close_enough = self._accurate_inverse_kinematics(**ik_options) # NOTE: In principle, this accurate iter IK should work out of the @@ -159,8 +194,14 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N joint_positions = pybullet.calculateInverseKinematics(**ik_options) close_enough = True # yeah, let's say we trust it - if not joint_positions: - raise InverseKinematicsError() + # if not joint_positions: + # raise InverseKinematicsError() + + # Normally, no results ends the iteration, but if we have no solution + # because the accurate IK solving says it's not close enough, we can retry + # with a new randomized seed + if close_enough: + yield joint_positions, joint_names # Randomize joints to get a different solution on the next iter self.client._set_joint_positions( @@ -169,15 +210,6 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N body_id, ) - # Normally, no results ends the iteration, but if we have no solution - # because the accurate IK solving says it's not close enough, we can retry - # with a new randomized seed - if not close_enough and high_accuracy: - continue - - rest_poses = joint_positions - yield joint_positions, joint_names - def _accurate_inverse_kinematics(self, joints, threshold, max_iter, **kwargs): # Based on these examples # https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_kinematics_husky_kuka.py#L81 @@ -204,6 +236,7 @@ def _accurate_inverse_kinematics(self, joints, threshold, max_iter, **kwargs): # 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 < threshold * threshold kwargs["restPoses"] = joint_poses iter += 1 diff --git a/src/compas_fab/backends/pybullet/client.py b/src/compas_fab/backends/pybullet/client.py index 6d28cd92f..ba4c2379f 100644 --- a/src/compas_fab/backends/pybullet/client.py +++ b/src/compas_fab/backends/pybullet/client.py @@ -21,6 +21,7 @@ from compas_fab.robots import Robot from compas_fab.robots import RobotLibrary from compas_fab.robots import RobotSemantics +from compas_fab.robots import RobotCell from compas_fab.utilities import LazyLoader from . import const @@ -351,6 +352,7 @@ def load_semantics(self, robot, srdf_filename): self.disabled_collisions = robot.semantics.disabled_collisions def _load_robot_to_pybullet(self, urdf_file, robot): + """Load a robot to PyBullet via a temp URDF file.""" cached_robot_model = self.get_cached_robot_model(robot) with redirect_stdout(enabled=not self.verbose): pybullet_uid = pybullet.loadURDF( @@ -364,6 +366,9 @@ def _load_robot_to_pybullet(self, urdf_file, robot): self._add_ids_to_robot_joints(cached_robot_model) self._add_ids_to_robot_links(cached_robot_model) + self._robot = robot + self._robot_cell = RobotCell(robot.model) + def reload_from_cache(self, robot): """Reloads the PyBullet server with the robot's cached model. diff --git a/src/compas_fab/backends/pybullet/planner.py b/src/compas_fab/backends/pybullet/planner.py index 8acd89ec2..304d5b443 100644 --- a/src/compas_fab/backends/pybullet/planner.py +++ b/src/compas_fab/backends/pybullet/planner.py @@ -34,4 +34,7 @@ class PyBulletPlanner( """Implement the planner backend interface for PyBullet.""" def __init__(self, client): - self.client = client + self._client = client + + # Initialize all mixins + super(PyBulletPlanner, self).__init__() diff --git a/src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py b/src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py index e865fa8d8..595078cb1 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py @@ -2,7 +2,27 @@ from __future__ import division from __future__ import print_function +import compas + +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from compas_robots import Configuration # noqa: F401 + from compas.geometry import Frame # noqa: F401 + from typing import Optional # noqa: F401 + from typing import Iterator # noqa: F401 + from typing import List # noqa: F401 + from typing import Tuple # noqa: F401 + from typing import Dict # noqa: F401 + from compas_fab.robots import RobotCellState # noqa: F401 + from compas_fab.robots import FrameTarget # noqa: F401 + from compas_fab.robots import AttachedCollisionMesh # noqa: F401 + + from compas.utilities import await_callback +from compas_fab.utilities import from_tcf_to_t0cf + from compas_fab.backends.interfaces import InverseKinematics from compas_fab.backends.ros.backend_features.helpers import convert_constraints_to_rosmsg @@ -20,6 +40,8 @@ from compas_fab.backends.ros.messages import RosDistro from compas_fab.backends.ros.service_description import ServiceDescription +from compas.tolerance import TOL + __all__ = [ "MoveItInverseKinematics", ] @@ -28,23 +50,67 @@ class MoveItInverseKinematics(InverseKinematics): """Callable to calculate the robot's inverse kinematics for a given frame.""" + def __init__(self): + # The following fields are used to store the last ik request for iterative calls + self._last_ik_request = {"request_id": None, "solutions": None} + # Initialize the super class + print("MoveItInverseKinematics init") + super(MoveItInverseKinematics, self).__init__() + GET_POSITION_IK = ServiceDescription( "/compute_ik", "GetPositionIK", GetPositionIKRequest, GetPositionIKResponse, validate_response ) - def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=None, options=None): + # def inverse_kinematics(self, frame_WCF, start_configuration=None, group=None, options=None): + # """ + # Inverse kinematics calculation using MoveIt. + + # """ + # options = options or {} + # kwargs = {} + # kwargs["options"] = options + # kwargs["frame_WCF"] = frame_WCF + # kwargs["group"] = group or self.client.robot.main_group_name + # kwargs["start_configuration"] = start_configuration or self.client.robot.zero_configuration() + # kwargs["errback_name"] = "errback" + + # # Use base_link or fallback to model's root link + # options["base_link"] = options.get("base_link", self.client.robot.model.root.name) + + # return await_callback(self.inverse_kinematics_async, **kwargs) + # # return_full_configuration = options.get("return_full_configuration", False) + # # use_attached_tool_frame = options.get("use_attached_tool_frame", False) + + # # # Pseudo-memoized sequential calls will re-use iterator if not exhausted + # # request_id = "{}-{}-{}-{}-{}".format( + # # str(frame_WCF), str(start_configuration), str(group), str(return_full_configuration), str(options) + # # ) + + # # # Re-use last ik request generator if the request is the same + # # if self._last_ik_request["request_id"] == request_id and self._last_ik_request["solutions"] is not None: + # # solution = next(self._last_ik_request["solutions"], None) + # # if solution is not None: + # # return solution + + # # # Create new ik generator object + # # solutions = self.iter_inverse_kinematics( + # # frame_WCF, start_configuration, group, return_full_configuration, use_attached_tool_frame, options + # # ) + # # self._last_ik_request["request_id"] = request_id + # # 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]) -> Iterator[Tuple[List[float], List[str]]] """Calculate the robot's inverse kinematic for a given frame. 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_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. + target: :class:`compas.geometry.FrameTarget` + The Frame Target to calculate the inverse for. + start_state : :class:`compas_fab.robots.RobotCellState`, optional + The starting state to calculate the inverse kinematics for. group: str, optional The planning group used for calculation. Defaults to the robot's main planning group. @@ -74,24 +140,74 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N Yields ------ - :obj:`tuple` of :obj:`list` + :obj:`tuple` of (:obj:`list` of :obj:`float`, :obj:`list` of :obj:`str`) A tuple of 2 elements containing a list of joint positions and a list of matching joint names. """ options = options or {} + robot = self.client.robot + + # Group + group = group or robot.main_group_name + + # Set scene + # TODO: Implement start_state + self.client.robot_cell.assert_cell_state_match(start_state) + start_state = start_state or RobotCellState.from_robot_configuration(robot) + + # Start configuration + start_configuration = start_state.robot_configuration + # Merge with zero configuration ensure all joints are present + start_configuration = robot.zero_configuration(group).merged(start_configuration) + + # Target frame and Tool Coordinate Frame + target_frame = target.target_frame + if robot.need_scaling: + target_frame = target_frame.scaled(1.0 / robot.scale_factor) + # Now target_frame is back in meter scale + attached_tool = self.client.robot_cell.get_attached_tool(start_state, group) + if attached_tool: + target_frame = from_tcf_to_t0cf(target_frame, attached_tool.frame) + # Attached tool frames does not need scaling because Tools are modelled in meter scale + + # Scale Attached Collision Meshes + if robot.need_scaling: + acms = options.get("attached_collision_meshes", []) # type: List[AttachedCollisionMesh] + for acm in acms: + acm.collision_mesh.scale(1.0 / robot.scale_factor) + + # Compose the kwargs for the async function kwargs = {} kwargs["options"] = options - kwargs["frame_WCF"] = frame_WCF + kwargs["frame_WCF"] = target_frame kwargs["group"] = group - kwargs["start_configuration"] = start_configuration + kwargs["start_configuration"] = start_configuration or robot.zero_configuration() kwargs["errback_name"] = "errback" max_results = options.get("max_results", 100) # Use base_link or fallback to model's root link - options["base_link"] = options.get("base_link", robot.model.root.name) + options["base_link"] = options.get("base_link", self.client.robot.model.root.name) + all_yielded_joint_positions = [] + # Yield up to max_results configurations for _ in range(max_results): - yield await_callback(self.inverse_kinematics_async, **kwargs) + # First iteration uses the provided start_configuration + joint_positions, joint_names = await_callback(self.inverse_kinematics_async, **kwargs) + + # Check if the result is the same as any of the previous results + result_is_unique = True + for j in all_yielded_joint_positions: + if TOL.is_allclose(joint_positions, j, rtol=TOL.APPROXIMATION): + result_is_unique = False + + # If result is unique, we yield it back to the user + if result_is_unique: + yield joint_positions, joint_names + all_yielded_joint_positions.append(joint_positions) + + # Generate random start configuration for next iteration + random_configuration = self.client.robot.random_configuration(kwargs["group"]) + kwargs["start_configuration"] = random_configuration def inverse_kinematics_async( self, callback, errback, frame_WCF, start_configuration=None, group=None, options=None diff --git a/src/compas_fab/backends/ros/client.py b/src/compas_fab/backends/ros/client.py index fe98a4305..acf2a762e 100644 --- a/src/compas_fab/backends/ros/client.py +++ b/src/compas_fab/backends/ros/client.py @@ -28,6 +28,7 @@ from compas_fab.backends.tasks import CancellableFutureResult from compas_fab.robots import Robot from compas_fab.robots import RobotSemantics +from compas_fab.robots import RobotCell __all__ = [ "RosClient", @@ -135,8 +136,8 @@ class RosClient(Ros, ClientInterface): def __init__(self, host="localhost", port=9090, is_secure=False, planner_backend="moveit"): super(RosClient, self).__init__(host, port, is_secure) - planner_backend_type = PLANNER_BACKENDS[planner_backend] - self.planner = planner_backend_type(self) + # planner_backend_type = PLANNER_BACKENDS[planner_backend] + # self.planner = planner_backend_type(self) self._ros_distro = None def __enter__(self): @@ -210,7 +211,9 @@ def load_robot( if load_geometry: model.load_geometry(loader, precision=precision) - return Robot(model, semantics=semantics, client=self) + self._robot = Robot(model, semantics=semantics, client=self) + self._robot_cell = RobotCell(self.robot.model) + return self._robot # ========================================================================== # executing diff --git a/src/compas_fab/backends/ros/planner.py b/src/compas_fab/backends/ros/planner.py index b1fbbce0e..063e93076 100644 --- a/src/compas_fab/backends/ros/planner.py +++ b/src/compas_fab/backends/ros/planner.py @@ -45,4 +45,7 @@ class MoveItPlanner( """Implement the planner backend interface based on MoveIt!""" def __init__(self, client): - self.client = client + self._client = client + + # Initialize all mixins + super(MoveItPlanner, self).__init__() diff --git a/src/compas_fab/robots/__init__.py b/src/compas_fab/robots/__init__.py index 192ac21d7..4eaae02f7 100644 --- a/src/compas_fab/robots/__init__.py +++ b/src/compas_fab/robots/__init__.py @@ -123,6 +123,14 @@ from .robot import ( Robot, ) + +from .robot_cell import ( + RobotCell, + RobotCellState, + RigidBody, + RigidBodyState, + RobotCellState, +) from .robot_library import ( RobotLibrary, ) @@ -189,4 +197,10 @@ "Wrench", "to_degrees", "to_radians", + # New + "RobotCell", + "RobotCellState", + "RigidBody", + "RigidBodyState", + "RobotCellState", ] diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index 7c7776868..7ccc444ea 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -16,6 +16,7 @@ from compas_robots.resources import LocalPackageMeshLoader from compas_fab.robots.constraints import Constraint +from compas_fab.utilities import from_t0cf_to_tcf if not compas.IPY: from typing import TYPE_CHECKING @@ -106,7 +107,6 @@ def __init__(self, model=None, scene_object=None, semantics=None, client=None): self.model = model self.scene_object = scene_object self.semantics = semantics - self.client = client self.attributes = {} @property @@ -1121,18 +1121,6 @@ def detach_tool(self, group=None): # checks # ========================================================================== - def ensure_client(self): - # type: () -> None - """Check if the client is set. - - Raises - ------ - :exc:`Exception` - If :attr:`client` is not set - """ - if not self.client: - raise Exception("This method is only callable once a client is assigned.") - def ensure_semantics(self): # type: () -> None """Check if semantics is set. @@ -1302,7 +1290,6 @@ def iter_inverse_kinematics( options = options or {} attached_collision_meshes = options.get("attached_collision_meshes") or [] - self.ensure_client() group = group or self.main_group_name if self.semantics else None start_configuration, start_configuration_scaled = self._check_full_configuration_and_scale(start_configuration) @@ -1344,7 +1331,7 @@ def _build_configuration(self, joint_positions, joint_names, group, return_full_ return configuration.scaled(self.scale_factor) - def forward_kinematics(self, configuration, group=None, use_attached_tool_frame=True, options=None): + def forward_kinematics(self, configuration, group=None, tool_coordinate_frame=None, options=None): # type: (Configuration, Optional[str], Optional[bool], Optional[Dict[str, Any]]) -> Frame """Calculate the robot's forward kinematic. @@ -1357,17 +1344,14 @@ def forward_kinematics(self, configuration, group=None, use_attached_tool_frame= group: obj:`str`, optional The planning group used for the calculation. Defaults to the robot's main planning group. - use_attached_tool_frame : :obj:`bool`, optional - If ``True`` and there is a tool attached to the planning group, FK will return - the TCF of the attached tool instead of the T0CF. Defaults to ``True``. + tool_coordinate_frame : :class:`compas.geometry.Frame`, optional + The tool tip coordinate frame relative to the flange of the robot. + Units must be in meters. + If set, forward kinematics will return the TCF of the attached tool + instead of the T0CF. Defaults to ``None``. options: obj:`dict`, optional Dictionary containing the following key-value pairs: - - ``"solver"``: (:obj:`str`, optional) If ``None`` calculates FK - with the client if it exists or with the robot model. - If ``'model'`` use the robot model to calculate FK. - Other values depend on specific backend implementation, some backends might - allow selecting different FK solvers dynamically. - ``"link"``: (:obj:`str`, optional) The name of the link to calculate the forward kinematics for. Defaults to the group's end effector link. @@ -1402,38 +1386,27 @@ def forward_kinematics(self, configuration, group=None, use_attached_tool_frame= True """ options = options or {} - solver = options.get("solver") group = group or self.main_group_name if self.semantics else None full_configuration = self.merge_group_with_full_configuration(configuration, self.zero_configuration(), group) full_configuration, full_configuration_scaled = self._check_full_configuration_and_scale(full_configuration) - # If there's no client, we default to `model` solver if there is no other assigned. - if not self.client: - solver = solver or "model" + # Calling forward_kinematics from Robot class will use the RobotModel's function to calculate FK - # Solve with the model - if solver == "model": - link = options.get("link", options.get("tool0")) - link = link or self.get_end_effector_link_name(group) - if link not in self.get_link_names(group): - raise ValueError("Link name {} does not exist in planning group".format(link)) + link = options.get("link", options.get("tool0")) + link = link or self.get_end_effector_link_name(group) + if link not in self.get_link_names(group): + raise ValueError("Link name {} does not exist in planning group".format(link)) - frame_WCF = self.model.forward_kinematics(full_configuration, link) + frame_WCF = self.model.forward_kinematics(full_configuration, link) - # Otherwise, pass everything down to the client - else: - self.ensure_client() - frame_WCF = self.client.forward_kinematics(self, full_configuration_scaled, group, options) + if tool_coordinate_frame: + frame_WCF = from_t0cf_to_tcf(frame_WCF, tool_coordinate_frame) # Scale and return frame_WCF.point *= self.scale_factor - attached_tool = self.attached_tools.get(group) - if use_attached_tool_frame and attached_tool: - frame_WCF = self.from_t0cf_to_tcf([frame_WCF], group)[0] - return frame_WCF def plan_cartesian_motion(self, waypoints, start_configuration=None, group=None, options=None): @@ -1505,7 +1478,6 @@ def plan_cartesian_motion(self, waypoints, start_configuration=None, group=None, options = options or {} - self.ensure_client() if not group: group = self.main_group_name # ensure semantics @@ -1659,7 +1631,6 @@ def plan_motion(self, target, start_configuration=None, group=None, options=None # TODO: add workspace_parameters - self.ensure_client() if not group: group = self.main_group_name @@ -1832,6 +1803,12 @@ def scale_factor(self): else: return self._scale_factor + @property + def need_scaling(self): + # type: () -> bool + """Check if the robot's geometry needs scaling.""" + return not TOL.is_close(self.scale_factor, 1.0, rtol=1e-8) + def info(self): # type: () -> None """Print information about the robot.""" diff --git a/src/compas_fab/robots/robot_cell.py b/src/compas_fab/robots/robot_cell.py new file mode 100644 index 000000000..dab033c06 --- /dev/null +++ b/src/compas_fab/robots/robot_cell.py @@ -0,0 +1,235 @@ +import compas + +from compas.data import Data + +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import Dict # noqa: F401 + from typing import List # noqa: F401 + from typing import Optional # noqa: F401 + from compas.datastructures import Mesh # noqa: F401 + from compas.geometry import Frame # noqa: F401 + from compas_robots import Configuration # noqa: F401 + from compas_robots import RobotModel # noqa: F401 + from compas_robots import ToolModel # noqa: F401 + from compas.geometry import Transformation # noqa: F401 + from compas_fab.robots import Robot # noqa: F401 + + +class RobotCell(Data): + """Represents objects in a robot cell. This include a single robot, a list of tools, and rigid bodies. + + Tools include those that are attached to the robot, and those that are not attached but placed in the environment. + + Rigid bodies are objects can be used to represent: + - Workpieces that are attached to the tip of a tool + - Workpieces that are placed in the environment and are not attached + - Robotic backpacks or other accessories that are attached to links of the robot + - Static obstacles in the environment + """ + + def __init__(self, robot_model, tool_models={}, rigid_body_models={}, static_environment_models={}): + super(RobotCell, self).__init__() + self.robot_model = robot_model # type: RobotModel + self.tool_models = tool_models # type: Dict[str, ToolModel] + self.rigid_body_models = rigid_body_models # type: Dict[str, RigidBody] + + @property + def tool_ids(self): + # type: () -> List[str] + return self.tool_models.keys() + + @property + def rigid_body_ids(self): + # type: () -> List[str] + return self.rigid_body_models.keys() + + @property + def __data__(self): + return { + "robot_model": self.robot_model, + "tool_models": self.tool_models, + "rigid_body_models": self.rigid_body_models, + } + + def assert_cell_state_match(self, cell_state): + # type: (RobotCellState) -> None + """Assert that the number of tools and rigid bodies in the cell state match the number of tools and workpieces in the robot cell.""" + symmetric_difference = set(cell_state.tool_ids) ^ set(self.tool_ids) + if symmetric_difference != set(): + raise ValueError( + "The tools in the cell state do not match the tools in the robot cell. Mismatch: %s" + % symmetric_difference + ) + symmetric_difference = set(cell_state.rigid_body_ids) ^ set(self.rigid_body_ids) + if symmetric_difference != set(): + raise ValueError( + "The workpieces in the cell state do not match the workpieces in the robot cell. Mismatch: %s" + % symmetric_difference + ) + + def get_attached_tool(self, robot_cell_state, group): + # type: (RobotCellState, str) -> Optional[ToolModel] + """Returns the tool attached to the group in the robot cell state.""" + tool_id = robot_cell_state.get_attached_tool_id(group) + if tool_id: + return self.tool_models[tool_id] + + +class RigidBody(Data): + """Represents a rigid body.""" + + def __init__(self, name, visual_meshes=[], collision_meshes=[]): + super(RigidBody, self).__init__() + self.name = name # type: str + self.visual_meshes = visual_meshes # type: List[Mesh] + self.collision_meshes = collision_meshes # type: List[Mesh] + + @property + def __data__(self): + return { + "name": self.name, + "visual_meshes": self.visual_meshes, + "collision_meshes": self.collision_meshes, + } + + +class RobotCellState(Data): + """Represents the state of a robot cell. + + This class should be used to represent the complete state of a robot cell, + not a partial state. The list of tool_states and rigid_body_states should + match the list of tools and workpieces in the RobotCell. + + The only optional attribute is the robot configuration, + which is not known before the motions are planned. + + """ + + def __init__(self, robot_flange_frame, robot_configuration=None, tool_states={}, rigid_body_states={}): + super(RobotCellState, self).__init__() + self.robot_flange_frame = robot_flange_frame # type: Frame + self.robot_configuration = robot_configuration # type: Optional[Configuration] + self.tool_states = tool_states # type: Dict[str, ToolState] + self.rigid_body_states = rigid_body_states # type: Dict[str, RigidBodyState] + + @property + def tool_ids(self): + # type: () -> List[str] + return self.tool_states.keys() + + @property + def rigid_body_ids(self): + # type: () -> List[str] + return self.rigid_body_states.keys() + + @property + def __data__(self): + return { + "robot_configuration": self.robot_configuration, + "tool_states": self.tool_states, + "rigid_body_states": self.rigid_body_states, + } + + @classmethod + def from_robot_configuration(cls, robot, configuration=None, group=None): + # type: (Robot, Optional[Configuration], Optional[str]) -> RobotCellState + """Creates a `RobotCellState` from a robot and a configuration. + + Parameters + ---------- + robot : :class:`~compas_robots.Robot` + The robot. + configuration : :class:`~compas_fab.Configuration`, optional + The configuration of the robot. If the configuration is not provided, the robot's zero configuration will be used. + group : str, optional + The planning group used for calculation. + """ + + configuration = configuration or robot.zero_configuration(group) + flange_frame = robot.forward_kinematics(configuration, group) + return cls(flange_frame, configuration) + + def get_attached_tool_id(self, group): + # type: (str) -> Optional[str] + """Returns the id of the tool attached to the planning group.""" + for tool_id, tool_state in self.tool_states.items(): + if tool_state.attached_to_group == group: + return tool_id + + +class ToolState(Data): + """Represents the state of a tool in a RobotCell. + + When representing a tool that is attached to a robot, the `attached_to_group` + attributes should be set to the planning group name. Otherwise, it should be `None`. + Note that the tool's base frame is attached without any offset to the end of + that planning group. + + When representing a tool that is kinematic (a ToolModel with movable joints), the + `configuration` attribute should be set. Otherwise, if left at `None`, the tool's + configuration will be assumed to be at its zero configuration. Note that grasp is + relative to the base of the tool, and thus is not affected by the configuration of the tool. + + + Attributes + ---------- + frame : :class:`compas.geometry.Frame` + The base frame of the tool relative to the world coordinate frame. + attached_to_group : :obj:`str`, optional + The name of the robot planning group to which the tool is attached. Defaults to ``None``. + configuration : :class:`compas_robots.Configuration`, optional + The configuration of the tool if the tool is kinematic. Defaults to ``None``. + is_hidden : :obj:`bool`, optional + Whether the tool is hidden in the scene. Collision checking will be turned off + for hidden objects. Defaults to ``False``. + """ + + def __init__(self, frame, attached_to_group=None, configuration=None, is_hidden=False): + super(ToolState, self).__init__() + self.frame = frame # type: Frame + self.attached_to_group = attached_to_group # type: Optional[str] + self.configuration = configuration # type: Optional[Configuration] + self.is_hidden = is_hidden # type: bool + + @property + def __data__(self): + return { + "frame": self.frame, + "attached_to_group": self.attached_to_group, + "configuration": self.configuration, + "is_hidden": self.is_hidden, + } + + +class RigidBodyState(Data): + """Represents the state of a workpiece in a RobotCell. + + When representing a workpiece that is attached to a tool, the `attached_to_tool` attribute should be set. + When representing a collision geometry (such as robotic backpacks) that is attached to a link of the robot, + the `attached_to_link` attribute should be set. Otherwise, they should be `None`. + + When representing a workpiece that is currently not in the scene, the `is_hidden` attribute should be set to True. + This will turn off collision checking for the workpiece. + + Attributes + ---------- + frame : :class:`compas.geometry.Frame` + The base frame of the workpiece relative to the world coordinate frame. + attached_to_link : :obj:`str`, optional + The name of the robot link to which the workpiece is attached. Defaults to ``None``. + attached_to_tool : :obj:`str`, optional + The id of the tool to which the workpiece is attached. Defaults to ``None``. + grasp : :class:`compas.geometry.Frame` | :class:`compas.geometry.Transformation`, optional + The grasp frame of the workpiece relative to (the base frame of) the attached link or + (the tool tip frame of) the tool. Defaults to ``None``. + is_hidden : :obj:`bool`, optional + Whether the workpiece is hidden in the scene. Collision checking will be turned off + for hidden objects. Defaults to ``False``. + """ + + def __init__(self, frame, attached_to_link=None, attached_to_tool=None, grasp=None, is_hidden=False): + if attached_to_link and attached_to_tool: + raise ValueError("A RigidBodyState cannot be attached to both a link and a tool.") diff --git a/src/compas_fab/utilities/__init__.py b/src/compas_fab/utilities/__init__.py index 0403ab03c..d7b59cf72 100644 --- a/src/compas_fab/utilities/__init__.py +++ b/src/compas_fab/utilities/__init__.py @@ -84,6 +84,7 @@ ) from .transformation import ( from_tcf_to_t0cf, + from_t0cf_to_tcf, ) __all__ = [ @@ -110,4 +111,5 @@ "sign", # transformation "from_tcf_to_t0cf", + "from_t0cf_to_tcf", ] diff --git a/src/compas_fab/utilities/transformation.py b/src/compas_fab/utilities/transformation.py index a0d2c6665..5c906b356 100644 --- a/src/compas_fab/utilities/transformation.py +++ b/src/compas_fab/utilities/transformation.py @@ -29,3 +29,31 @@ def from_tcf_to_t0cf(tcf_frame_in_wcf, tool_coordinate_frame): TCF_T0CF = Transformation.from_frame(T0CF_TCF).inverse() W_TCF = Transformation.from_frame(tcf_frame_in_wcf) return Frame.from_transformation(W_TCF * TCF_T0CF) + + +def from_t0cf_to_tcf(t0cf_frame_in_wcf, tool_coordinate_frame): + """Converts a frame describing the robot's flange (tool0 frame) relative to WCF + to a frame describing the robot's tool tip (tcf frame), relative to WCF. + + Let: W_T0CF = t0cf_frame_in_wcf + Let: T0CF_TCF = tool_coordinate_frame + Let: frames in TCF relative to WCF = W_TCF + Then: W_TCF = W_T0CF * T0CF_TCF + + Parameters + ---------- + tcf_frame_in_wcf : :class:`~compas.geometry.Frame` + Frame (in WCF) of the robot's tool tip (tcf). + tool_coordinate_frame : :class:`~compas.geometry.Frame` + Tool tip coordinate frame relative to the flange of the robot. + + Returns + ------- + :class:`~compas.geometry.Frame` + Frame (in WCF) of the robot's flange (tool0). + + """ + W_T0CF = Transformation.from_frame(t0cf_frame_in_wcf) + T0CF_TCF = Transformation.from_frame(tool_coordinate_frame) + W_TCF = W_T0CF * T0CF_TCF + return Frame.from_transformation(W_TCF) diff --git a/tests/backends/kinematics/test_inverse_kinematics.py b/tests/backends/kinematics/test_inverse_kinematics.py index c3e27fc18..e563fe940 100644 --- a/tests/backends/kinematics/test_inverse_kinematics.py +++ b/tests/backends/kinematics/test_inverse_kinematics.py @@ -51,7 +51,7 @@ def test_kinematics_client(): robot = client.load_robot(urdf_filename) client.load_semantics(robot, srdf_filename) solutions = list( - robot.iter_inverse_kinematics( + client.iter_inverse_kinematics( frame_WCF, options={"solver": "ur5", "check_collision": True, "keep_order": False} ) ) diff --git a/tests/backends/pybullet/test_pybullet_planner.py b/tests/backends/pybullet/test_pybullet_planner.py index a19788bfb..8d2643bbd 100644 --- a/tests/backends/pybullet/test_pybullet_planner.py +++ b/tests/backends/pybullet/test_pybullet_planner.py @@ -10,6 +10,8 @@ from compas.tolerance import Tolerance from compas_fab.robots import RobotLibrary +from compas_fab.robots import FrameTarget +from compas_fab.robots import RobotCellState # The tolerance for the tests are set to 1e-4 meters, equivalent to 0.1 mm # Relative tolerance is set to 1e-3 (0.1%) @@ -123,12 +125,11 @@ def _test_pybullet_ik_fk_agreement(robot, ik_target_frames): for ik_target_frame in ik_target_frames: # IK Query to the planner (Frame to Configuration) try: - # Note: The inverse_kinematics method returns a generator + joint_positions, joint_names = next( - planner.inverse_kinematics( - robot, - ik_target_frame, - start_configuration=robot.zero_configuration(), + planner.iter_inverse_kinematics( + FrameTarget(ik_target_frame), + RobotCellState.from_robot_configuration(robot), group=planning_group, options=ik_options, ) @@ -246,9 +247,8 @@ def test_pybullet_ik_out_of_reach_ur5(): # Note: The inverse_kinematics method returns a generator joint_positions, joint_names = next( planner.inverse_kinematics( - robot, - ik_target_frame, - start_configuration=robot.zero_configuration(), + FrameTarget(ik_target_frame), + RobotCellState.from_robot_configuration(robot), group=planning_group, options=ik_options, )