diff --git a/docs/examples/05_backends_pybullet/files/04_plan_cartesian_motion.py b/docs/examples/05_backends_pybullet/files/04_plan_cartesian_motion.py new file mode 100644 index 000000000..8f77b993b --- /dev/null +++ b/docs/examples/05_backends_pybullet/files/04_plan_cartesian_motion.py @@ -0,0 +1,42 @@ +import compas_fab +from compas_robots import Configuration +from compas_fab.backends import PyBulletClient +from compas_fab.backends import PyBulletPlanner +from compas_fab.robots import RobotCell +from compas_fab.robots import RobotCellState +from compas_fab.robots import RobotLibrary + + +# ############################################# +# Headless (no-GUI) forwards kinematics example +# ############################################# + +# 'direct' mode +with PyBulletClient("direct") as client: + # The robot in this example is loaded from a URDF file + robot = RobotLibrary.ur5() + planner = PyBulletPlanner(client) + + # This is a simple robot cell with only the robot + robot_cell = RobotCell(robot) + planner.set_robot_cell(robot_cell) + + # Create the starting configuration + configuration = Configuration.from_revolute_values([-2.238, -1.153, -2.174, 0.185, 0.667, 0.0]) + # The `RobotCellState.from_robot_configuration` method can be used when the robot is the only element in the cell + robot_cell_state = RobotCellState.from_robot_configuration(robot, configuration) + # In this demo, the default planning group is used for the forward kinematics + frame_WCF = planner.forward_kinematics(robot_cell_state) + +# Set the start and goal frames +start_frame = Frame([0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]) +goal_frame = Frame([1.0, 1.0, 1.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]) + +# Create a planner +planner = client.get_planner() + +# Plan a cartesian motion using the pybullet backend +trajectory = planner.plan_cartesian_motion(robot, start_frame, goal_frame) + +# Execute the planned trajectory +client.execute_trajectory(robot, trajectory) diff --git a/src/compas_fab/backends/exceptions.py b/src/compas_fab/backends/exceptions.py index f595038d1..4ad7593f6 100644 --- a/src/compas_fab/backends/exceptions.py +++ b/src/compas_fab/backends/exceptions.py @@ -57,6 +57,8 @@ def __init__(self): class CollisionCheckError(BackendError): """Indicates a collision check exception.""" + # TODO: Rewrite this to be a collection of CC errors + def __init__(self, message): super(CollisionCheckError, self).__init__(message) @@ -73,16 +75,18 @@ class CollisionCheckInCollisionError(BackendError): Attributes ---------- - object1_type : str + object1_type : int Type of the first object. object1_name : str Name of the first object. - object2_type : str + object2_type : int Type of the second object. object2_name : str Name of the second object. """ + # TODO: Rename this class, add the type int as constants and add to a string mapping + def __init__(self, object1_name, object2_name, object1_type=None, object2_type=None): # type(str, str, Optional[int], Optional[int]) -> None message = "Collision between '{}' and '{}'".format(object1_name, object2_name) diff --git a/src/compas_fab/backends/kinematics/backend_features/analytical_pybullet_inverse_kinematics.py b/src/compas_fab/backends/kinematics/backend_features/analytical_pybullet_inverse_kinematics.py index 0a10af412..43d2f04ae 100644 --- a/src/compas_fab/backends/kinematics/backend_features/analytical_pybullet_inverse_kinematics.py +++ b/src/compas_fab/backends/kinematics/backend_features/analytical_pybullet_inverse_kinematics.py @@ -1,16 +1,12 @@ -from compas_fab.backends.exceptions import BackendError -from compas_fab.backends.exceptions import BackendTargetNotSupportedError -from compas_fab.backends.exceptions import InverseKinematicsError -from compas_fab.backends.interfaces import InverseKinematics -from .analytical_inverse_kinematics import AnalyticalInverseKinematics import compas +from .analytical_inverse_kinematics import AnalyticalInverseKinematics if not compas.IPY: from typing import TYPE_CHECKING if TYPE_CHECKING: - from compas_fab.backends import AnalyticalKinematicsPlanner # noqa: F401 + from compas_fab.backends import AnalyticalPyBulletPlanner # noqa: F401 from compas_fab.robots import RobotCellState # noqa: F401 from compas_fab.robots import Robot # noqa: F401 from compas_fab.robots import Target # noqa: F401 @@ -18,10 +14,10 @@ from typing import Dict, List, Optional, Tuple # noqa: F401 from compas_fab.backends import PyBulletClient # noqa: F401 - -from compas_fab.robots import FrameTarget from compas_robots import Configuration +from compas_fab.backends import CollisionCheckError +from compas_fab.robots import FrameTarget from ..utils import joint_angles_to_configurations from ..utils import try_to_fit_configurations_between_bounds @@ -46,19 +42,25 @@ class AnalyticalPybulletInverseKinematics(AnalyticalInverseKinematics): that supports ``"check_collision"``, so for now only the `PyBulletClient`. """ - def iter_inverse_kinematics_frame_target(self, target, start_state=None, group=None, options=None): - # type: (FrameTarget, Optional[RobotCellState], Optional[str], Optional[Dict]) -> Generator[Tuple[List[float], List[str]], None, None] + def iter_inverse_kinematics_frame_target(self, target, start_state, group=None, options=None): + # type: (FrameTarget, RobotCellState, Optional[str], Optional[Dict]) -> Generator[Tuple[List[float], List[str]], None, None] """This function overrides the iter_inverse_kinematics_frame_target function from AnalyticalInverseKinematics to include the PyBulletClient for collision checking. It depends on the PyBulletClient to be able to check for collisions. """ + # TODO: This function is migrated but not finished with new CC functions. + options = options or {} - planner = self # type: AnalyticalKinematicsPlanner + planner = self # type: AnalyticalPyBulletPlanner robot = planner.robot_cell.robot # type: Robot client = self.client # type: PyBulletClient + # Set robot cell state to start state if provided + if start_state: + planner.set_robot_cell_state(start_state) + # Scale Target and get target frame target = self._scale_input_target(target) target_frame = target.target_frame @@ -79,8 +81,11 @@ def iter_inverse_kinematics_frame_target(self, target, start_state=None, group=N if options.get("check_collision", False): try: joint_values, joint_names = solution - client.check_collisions(Configuration.from_revolute_values(joint_values, joint_names)) - except BackendError as e: + # To save some time, we only set the robot configuration but avoid calling set_robot_cell_states + client.set_robot_configuration(Configuration(joint_names=joint_names, joint_values=joint_values)) + # Passing the `_skip_set_robot_cell_state` option to the collision check function + planner.check_collision(None, options={"_skip_set_robot_cell_state": True}) + except CollisionCheckError as e: # If keep order is true, yield (None, None) to keep the order of the solutions if options.get("keep_order", False): yield (None, None) diff --git a/src/compas_fab/backends/kinematics/planner.py b/src/compas_fab/backends/kinematics/planner.py index 8cb210001..1e5453f96 100644 --- a/src/compas_fab/backends/kinematics/planner.py +++ b/src/compas_fab/backends/kinematics/planner.py @@ -47,13 +47,15 @@ def __init__(self, kinematics_solver): class AnalyticalPyBulletPlanner( - PyBulletForwardKinematics, - AnalyticalPybulletInverseKinematics, AnalyticalPlanCartesianMotion, PyBulletSetRobotCell, PyBulletSetRobotCellState, + PyBulletCheckCollision, + PyBulletForwardKinematics, + AnalyticalPybulletInverseKinematics, PlannerInterface, ): + # TODO: This class need rework """Combination of PyBullet as the client for Collision Detection and Analytical Inverse Kinematics. This planner is based on the the PyBullet client for collision detection and use diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py index bca9e5f00..16c8ef2ce 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py @@ -33,7 +33,7 @@ def forward_kinematics(self, robot_cell_state, group=None, options=None): Collision checking can be enabled by setting ``"check_collision"`` to ``True`` in the options. This will cause the backend to perform collision checking on the robot's configuration and raise a :class:`CollisionCheckError` if the robot is in collision. This is equivalent to calling - :meth:`compas_fab.backends.PyBulletClient.check_collisions`. + :meth:`compas_fab.backends.PyBulletCheckCollision.check_collision`. Parameters ---------- @@ -52,7 +52,7 @@ def forward_kinematics(self, robot_cell_state, group=None, options=None): - ``"link"``: (:obj:`str`, optional) The name of the link to calculate the forward kinematics for. Defaults to the end effector. - ``"check_collision"``: (:obj:`str`, optional) When ``True``, - :meth:`compas_fab.backends.PyBulletClient.check_collisions` will be called. + :meth:`compas_fab.backends.PyBulletCheckCollision.check_collision` will be called. Defaults to ``False``. Returns @@ -80,7 +80,7 @@ def forward_kinematics(self, robot_cell_state, group=None, options=None): # Check for collisions if requested, it will throw an exception if the robot is in collision if options.get("check_collision"): - client.check_collisions(configuration) + planner.check_collision(None, options={"_skip_set_robot_cell_state": True}) # If a link name provided, return the frame of that link link_name = options.get("link") 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 new file mode 100644 index 000000000..24777e91b --- /dev/null +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py @@ -0,0 +1,295 @@ +from compas_fab.robots import Waypoints +from compas_fab.robots import FrameWaypoints +from compas_fab.robots import PointAxisWaypoints +from compas_fab.robots import FrameTarget + +from compas_fab.backends import CollisionCheckInCollisionError +import compas + +if compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from compas_fab.backends import PyBulletClient # noqa: F401 + from compas_fab.backends import PyBulletPlanner # noqa: F401 + from compas_fab.robots import RobotCellState # noqa: F401 + from compas_fab.robots import JointTrajectory # noqa: F401 + from compas_fab.robots import JointTrajectoryPoint # 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 typing import Optional # noqa: F401 + from typing import Dict # noqa: F401 + +__all__ = [ + "PyBulletPlanCartesianMotion", +] +from compas_fab.backends.interfaces import PlanCartesianMotion + + +class PyBulletPlanCartesianMotion(PlanCartesianMotion): + """Callable to calculate a cartesian motion path (linear in tool space).""" + + def plan_cartesian_motion(self, waypoints, start_state, group=None, options=None): + # type: (Waypoints, Configuration, RobotCellState, Optional[Dict]) -> JointTrajectory + """Calculates a cartesian motion path (linear in tool space) for Waypoints. + + Supports FrameWaypoints and PointAxisWaypoints. + + For more information such as planner specific behaviors, see + :meth:`~compas_fab.backends.PyBulletClient.plan_cartesian_motion_point_axis_waypoints` for + :class:`~compas_fab.robots.PointAxisWaypoints` and + :meth:`~compas_fab.backends.PyBulletClient.plan_cartesian_motion_frame_waypoints` for + :class:`~compas_fab.robots.FrameWaypoints`. + + Parameters + ---------- + waypoints : :class:`compas_fab.robots.Waypoints` + The waypoints for the robot to follow. + start_state : :class:`compas_fab.robots.RobotCellState` + The starting state of the robot cell at the beginning of the motion. + The attribute `robot_configuration`, must be provided. + group: str, optional + The planning group used for calculation. Defaults to the robot's + main planning group. + options: dict, optional + Dictionary containing additional options, see specific planner for details. + + - ``"check_collision"``: (:obj:`str`, optional) When ``True``, + :meth:`compas_fab.backends.PyBulletCheckCollision.check_collision` will be called. + Defaults to ``True``. Setting this to False may help during interactive debugging. + + Returns + ------- + :class:`compas_fab.robots.JointTrajectory` + The calculated trajectory. + + Raises + ------ + CollisionCheckError + If ``check_collision`` is enabled and the configuration is in collision. + """ + + # NOTE: The conversion of Waypoints Targets from TCF to T0CF must happen inside the + # corresponding planner method. This is because for PointAxisWaypoints, the planner + # must be able to spin around the TCF Z axis freely. + # This is not possible anymore if the targets are converted to T0CF representation here. + + # Unit conversion from user scale to meter scale can be done here because they are shared. + robot = self.client.robot # type: Robot + if robot.need_scaling: + waypoints = waypoints.scaled(1.0 / robot.scale_factor) + + if isinstance(waypoints, PointAxisWaypoints): + return self.plan_cartesian_motion_point_axis_waypoints(waypoints, start_state, group, options) + elif isinstance(waypoints, FrameWaypoints): + return self.plan_cartesian_motion_frame_waypoints(waypoints, start_state, group, options) + else: + raise NotImplementedError("Only PointAxisWaypoints and FrameWaypoints are supported.") + + def plan_cartesian_motion_point_axis_waypoints(self, waypoints, start_state, group=None, options=None): + # type: (PointAxisWaypoints, RobotCellState, Optional[str], Optional[Dict]) -> JointTrajectory + """Calculates a cartesian motion path (linear in tool space) for Point Axis Waypoints.""" + + raise NotImplementedError("plan_cartesian_motion_point_axis_waypoints() is not implemented yet.") + + def plan_cartesian_motion_frame_waypoints(self, waypoints, start_state, group=None, options=None): + # type: (FrameWaypoints, RobotCellState, Optional[str], Optional[Dict]) -> JointTrajectory + """Calculates a cartesian motion path (linear in tool space) for Frame Waypoints. + + The starting state of the robot cell at the beginning of the motion must be provided. + It must match the objects in the robot cell previously set using :meth:`compas_fab.backends.PyBulletClient.set_robot_cell`. + If tools are attached to the robot, it must be reflected in the starting state. + The robot's full configuration, i.e. values for all configurable joints of the entire robot, + must be provided using the ``start_state.robot_configuration`` parameter. + The ``start_state.robot_flange_frame`` parameter is not used by this function. + + The ``waypoints`` parameter is used to defined single or multiple segments of path, it is not necessary to include + the starting pose in the waypoints, doing so may result in a redundant trajectory point. + The waypoints refers to the robot's Tool0 Coordinate Frame (T0CF) if no tools are attached, + or Tool Coordinate Frame (TCF) if a tool is attached. + Each segment is interpolated linearly in Cartesian space, where position is interpolated linearly in Cartesian space + and orientation is interpolated spherically using quaternion interpolation. + + The function will return a :class:`compas_fab.robots.JointTrajectory` object, + which contains a series of joint trajectory points. + Note that there maybe more than one trajectory point per path segment due to interpolation. + + The ``check_collision`` parameter can be used to enable or disable collision checking during the planning process. + If enabled, the planner will check for collision similar to the :meth:`compas_fab.backends.PyBulletClient.check_collision` method. + + There are three parameters that can be used to control the interpolation process, all of which control + the maximum allowed distance between consecutive points in the result. + The ``max_step`` parameter, controls the Cartesian distance between the interpolated points (TCF or T0CF). + The ``max_jump_prismatic`` and ``max_jump_revolute`` parameter, can be used to control the maximum allowed joint distance + between consecutive points. Units are in meters for prismatic joints and radians for revolute and continuous joints. + The interpolator will respect these parameters and will not exceed them, however, this also means that the resulting + trajectory points may not be equidistant in Cartesian or Joint space. + + Note that the resulting trajectory does not separate the trajectory points into segments based on the waypoints. + If the user wants to separate the path into segments, they should consider calling this function multiple times. + + Notes + ----- + + This planning function is deterministic, meaning that the same input will always result in the same output. + If this function fails, retrying with the same input will not change the result. + + The planner will not attempt to avoid collision, but simply check for them and raise an error if a collision is found. + + This planning function is not asynchronous, meaning that it will block the main thread until the planning is complete + or an error is raised. + + Parameters + ---------- + waypoints : :class:`compas_fab.robots.Waypoints` + The waypoints for the robot to follow. + start_state : :class:`compas_fab.robots.RobotCellState` + The starting state of the robot cell at the beginning of the motion. + The attribute `robot_configuration`, must be provided. + group: str, optional + The planning group used for calculation. Defaults to the robot's + main planning group. + options: dict, optional + Dictionary containing the following key-value pairs: + + - ``"max_step"``: (:obj:`float`, optional) + The max Cartesian distance between two consecutive points in the result. + calculated points. + Unit is in meters, defaults to ``0.01``. + - ``"max_jump_prismatic"``: (:obj:`float`, optional) + The maximum allowed distance of prismatic joint positions + between consecutive trajectory points. + Unit is in meters, defaults to ``0.1``. + Setting this to ``0`` will disable this check. + - ``"max_jump_revolute"``: (:obj:`float`, optional) + The maximum allowed distance of revolute and continuous joint positions + between consecutive trajectory points. + Unit is in radians, defaults to :math:`\\pi / 2` (90 degrees). + Setting this to ``0`` will disable this check. + - ``"check_collision"``: (:obj:`bool`, optional) + Whether or not to avoid collision. Defaults to ``True``. + + Returns + ------- + :class:`compas_fab.robots.JointTrajectory` + The calculated trajectory. + + Raises + ------ + CollisionCheckError + If ``check_collision`` is enabled and the configuration is in collision. + InverseKinematicsError + If no IK solution could be found by the kinematic solver. + The target frame may be unreachable. + + """ + # Set default option values + options = options or {} + options["max_step"] = options.get("max_step", 0.01) # meters + options["max_jump_prismatic"] = options.get("max_jump_prismatic", 0.1) + options["max_jump_revolute"] = options.get("max_jump_revolute", 3.14 / 2) + options["check_collision"] = options.get("check_collision", True) + + # Housekeeping for intellisense + planner = self # type: PyBulletPlanner + client = planner.client # type: PyBulletClient + robot = client.robot # type: Robot + + # Check input sanity + if not start_state.robot_configuration: + raise ValueError("start_state.robot_configuration must be provided.") + if not start_state.robot_configuration.joint_names: + raise ValueError("start_state.robot_configuration.joint_names must be provided.") + client.robot_cell.assert_cell_state_match(start_state) + + # Get default group name if not provided + group = group or robot.main_group_name + + # Setting robot cell state + planner.set_robot_cell_state(start_state) + + # Check start state input for collision if requested, it will throw an exception if the robot is in collision + if options.get("check_collision"): + # This allows options such as `full_report` and `verbose` to pass through to the check_collision method + cc_options = options.copy() + cc_options.update({"_skip_set_robot_cell_state": True}) + try: + # Note: This is using the CheckCollision Backend Feature + planner.check_collision(start_state, options={"_skip_set_robot_cell_state": True}) + except CollisionCheckInCollisionError as e: + e.message = "The start_state is in collision." + e.message + raise e + + # Options for Inverse Kinematics + options["high_accuracy"] = options.get("high_accuracy", True) + options["high_accuracy_threshold"] = options.get("high_accuracy_threshold", 1e-4) + options["high_accuracy_max_iter"] = options.get("high_accuracy_threshold", 20) + options["max_results"] = options.get("max_results", 1) + # max_results = 1 removes the random search of the IK Engine + + # Iterate over the waypoints as segments + intermediate_state = start_state.copy() # type: RobotCellState + joint_names = start_state.robot_configuration.joint_names + start_configuration = start_state.robot_configuration + trajectory = JointTrajectory(joint_names=joint_names, start_configuration=start_configuration) + + # Add the start configuration as the first point + trajectory.points.append(JointTrajectoryPoint(list(start_configuration.joint_values))) + for i in range(len(waypoints.target_frames) - 1): + start_frame = waypoints.target_frames[i] + end_frame = waypoints.target_frames[i + 1] + + frame_interpolator = FrameInterpolator(start_frame, end_frame, options) + # First round of interpolation is performed with respect to the max_step parameter + number_of_points = frame_interpolator.num_points + current_step = 1 + while current_step <= number_of_points: + # Prepare the input for IK + frame = frame_interpolator.get_frame(current_step) + # get_frame() expects `current_step` to be a whole number for equal spacing + target = FrameTarget(frame) + intermediate_state.robot_configuration.joint_values = trajectory.points[-1].joint_values + # Note that the PyBullet IK also performs collision checking + # Note that the PyBullet IK is gradient based and will snap to the nearest configuration of the start state + # Note that we are running this IK solver without random search to ensure determinism + # Note that planner.inverse_kinematics() will raise an InverseKinematicsError if no solution is found + joint_positions, joint_names_sorted = planner.inverse_kinematics( + target, start_configuration, group, options + ) + trajectory.points.append(JointTrajectoryPoint(joint_positions)) + print( + "Segment {} of {}, Point {} of {} = {}".format( + i + 1, len(waypoints.target_frames) - 1, current_step, number_of_points, joint_positions + ) + ) + current_step += 1 + + +class FrameInterpolator(object): + """A class to interpolate between two frames. + + Linear Cartesian interpolation is used for position + and spherical interpolation is used for orientation. + + This object allows for custom control of interpolation spacing between two frames + """ + + def __init__(self, start_frame, end_frame, options): + # type: (Frame, Frame, Dict) -> None + self.start_frame = start_frame + self.end_frame = end_frame + self.options = options + + # Compute the Cartesian distance between the two frames + # This is used to determine the number of interpolated points + # with the max_step parameter + + distance = start_frame.point.distance_to_point(end_frame.point) + # Floor + 1 to ensure at least one point + self.num_points = int(distance / options["max_step"]) + 1 + + def get_frame(self, step): + # type: (float) -> Frame + """Interpolates the waypoints and returns a joint trajectory.""" + raise NotImplementedError("interpolator() is not implemented yet.") diff --git a/src/compas_fab/backends/pybullet/client.py b/src/compas_fab/backends/pybullet/client.py index 872664243..348f21f27 100644 --- a/src/compas_fab/backends/pybullet/client.py +++ b/src/compas_fab/backends/pybullet/client.py @@ -473,209 +473,6 @@ def robot_model_to_urdf(self, robot_model, concavity=False): # Functions for collision checking # -------------------------------- - def check_collisions(self, configuration=None, verbose=False, full_report=False): - """Checks whether the current or given configuration is in collision. - - The collision check involves the following steps: - - 1. Check for collisions between each robot link. - 2. Check for collisions between each robot link and each tool. - 3. Check for collisions between each robot link and each rigid body. - 4. Check for collisions between each attached rigid body and all other rigid bodies. - 5. Check for collisions between each tool and each rigid body. - - In each of the above steps, the collision check is skipped if the collision is allowed - by the semantics of the robot, tool, or rigid body. For details, see in-line comments in code. - - Parameters - ---------- - robot : :class:`compas_fab.robots.Robot` - Robot whose configuration may be in collision. - configuration : :class:`compas_fab.robots.Configuration` - Configuration to be checked for collisions. If ``None`` is given, the current - configuration will be checked. Defaults to ``None``. - verbose : :obj:`bool` - When ``True``, additional information is printed. Note that this significantly - reduces performance. Defaults to ``False``. - full_report : :obj:`bool` - A collision report is returned with the CollisionCheckError exception message. - Typically, the exception is raised on the first collision detected and the rest of - the collision pairs are not checked. - If ``full_report`` is set to ``True``, the exception will be raised after all - collision pairs are checked, and the report will contain all failing collision pairs. - Defaults to ``False``. - - Raises - ------ - :class:`compas_fab.backends.pybullet.DetectedCollision` - """ - - if configuration: - self.set_robot_configuration(configuration) - - # This stores all the collision messages for a detailed collision report - collision_messages = [] - - def verbose_print(msg): - if verbose: - print(msg) - - # CC Step 1: Between each Robot Link - link_names = list(self.robot_link_puids.keys()) - for link_1_name, link_2_name in combinations(link_names, 2): - cc_pair_info = "CC.1 between robot link '{}' and robot link '{}'".format(link_1_name, link_2_name) - # Allowed collision originates from the robot semantics (SRDF) - if {link_1_name, link_2_name} in self.unordered_disabled_collisions: - verbose_print(cc_pair_info + " - SKIPPED (SEMANTICS)") - continue - link_1_id = self.robot_link_puids[link_1_name] - link_2_id = self.robot_link_puids[link_2_name] - try: - self._check_collision( - self.robot_puid, - "robot_" + link_1_name, - self.robot_puid, - "robot_" + link_2_name, - link_1_id, - link_2_id, - ) - verbose_print(cc_pair_info + " - PASS") - except CollisionCheckInCollisionError: - verbose_print(cc_pair_info + " - COLLISION") - collision_messages.append(cc_pair_info + " - COLLISION") - if not full_report: # Fail on first error if full_report is not requested - raise CollisionCheckError("\n".join(collision_messages)) - - # CC Step 2: Between each Robot Link and each of the tools - for link_name, link_id in self.robot_link_puids.items(): - for tool_name, tool_id in self.tools_puids.items(): - tool_state = self.robot_cell_state.tool_states[tool_name] - cc_pair_info = "CC.2 between robot link '{}' and tool '{}'".format(link_name, tool_name) - # Skip over hidden tools - if tool_state.is_hidden: - verbose_print(cc_pair_info + " - SKIPPED (HIDDEN)") - continue - - # Allowed collision originates from ToolState - if link_name in tool_state.touch_links: - verbose_print(cc_pair_info + " - SKIPPED (ALLOWED TOUCH LINK)") - continue - try: - self._check_collision( - self.robot_puid, "robot_" + link_name, tool_id, tool_name, link_index_1=link_id - ) - verbose_print(cc_pair_info + " - PASS") - - except CollisionCheckInCollisionError: - verbose_print(cc_pair_info + " - COLLISION") - collision_messages.append(cc_pair_info + " - COLLISION") - if not full_report: # Fail on first error if full_report is not requested - raise CollisionCheckError("\n".join(collision_messages)) - - # CC Step 3: Between each link and each rigid body - for link_name, link_id in self.robot_link_puids.items(): - for body_name, body_ids in self.rigid_bodies_puids.items(): - rigid_body_state = self.robot_cell_state.rigid_body_states[body_name] - cc_pair_info = "CC.3 between robot link '{}' and rigid body '{}'".format(link_name, body_name) - # Skip over hidden bodies - if rigid_body_state.is_hidden: - verbose_print(cc_pair_info + " - SKIPPED (HIDDEN)") - continue - # Skip over touch_links that are allowed to touch the rigid body - if link_name in rigid_body_state.touch_links: - verbose_print(cc_pair_info + " - SKIPPED (ALLOWED TOUCH LINK)") - continue - # Perform collision check - for body_id in body_ids: - try: - self._check_collision(self.robot_puid, "robot_" + link_name, body_id, body_name, link_id) - verbose_print(cc_pair_info + " body_id '{}' - PASS".format(body_id)) - except CollisionCheckInCollisionError: - verbose_print(cc_pair_info + " body_id '{}' - COLLISION".format(body_id)) - collision_messages.append(cc_pair_info + " body_id '{}' - COLLISION".format(body_id)) - if not full_report: # Fail on first error if full_report is not requested - raise CollisionCheckError("\n".join(collision_messages)) - - # CC Step 4: Between each attached rigid body and all other rigid body - for body_name, body_ids in self.rigid_bodies_puids.items(): - cc_pair_info = "CC.4 between attached rigid body '{}' and other rigid bodies".format(body_name) - # Skip over hidden bodies - if self.robot_cell_state.rigid_body_states[body_name].is_hidden: - verbose_print(cc_pair_info + " - SKIPPED (HIDDEN)") - continue - # Skip over non-attached bodies because there is no need to check between two static bodies - if not ( - self.robot_cell_state.rigid_body_states[body_name].attached_to_tool - or self.robot_cell_state.rigid_body_states[body_name].attached_to_link - ): - verbose_print(cc_pair_info + " - SKIPPED (NOT ATTACHED TO LINK)") - continue - for other_body_name, other_body_ids in self.rigid_bodies_puids.items(): - # Skip over hidden bodies - if self.robot_cell_state.rigid_body_states[other_body_name].is_hidden: - continue - # Skip over same body pairs - if body_name == other_body_name: - continue - # Skip over allowed touch bodies - if body_name in self.robot_cell_state.rigid_body_states[other_body_name].touch_bodies: - verbose_print(cc_pair_info + " - SKIPPED (ALLOWED TOUCH BODY)") - continue - if other_body_name in self.robot_cell_state.rigid_body_states[body_name].touch_bodies: - verbose_print(cc_pair_info + " - SKIPPED (ALLOWED TOUCH BODY)") - continue - # Perform collision check - for body_id in body_ids: - for other_body_id in other_body_ids: - cc_pair_info = "CC.4 between attached rigid body '{}' and rigid body '{}'".format( - body_name, other_body_name - ) - try: - self._check_collision(body_id, body_name, other_body_id, other_body_name) - verbose_print(cc_pair_info + " - PASS") - except CollisionCheckInCollisionError: - verbose_print(cc_pair_info + " - COLLISION") - collision_messages.append(cc_pair_info + " - COLLISION") - if not full_report: # Fail on first error if full_report is not requested - raise CollisionCheckError("\n".join(collision_messages)) - - ## CC Step 5: Between each tool and each rigid body - for tool_name, tool_id in self.tools_puids.items(): - tool_state = self.robot_cell_state.tool_states[tool_name] - cc_pair_info = "CC.5 between tool '{}' and rigid bodies".format(tool_name) - # Skip over hidden tools - if tool_state.is_hidden: - verbose_print(cc_pair_info + " - SKIPPED (TOOL HIDDEN)") - continue - for body_name, body_ids in self.rigid_bodies_puids.items(): - rigid_body_state = self.robot_cell_state.rigid_body_states[body_name] - cc_pair_info = "CC.5 between tool '{}' and rigid body '{}'".format(tool_name, body_name) - # Skip over hidden bodies - if rigid_body_state.is_hidden: - verbose_print(cc_pair_info + " - SKIPPED (RB HIDDEN)") - continue - # Skip over bodies that are attached to the tool - if rigid_body_state.attached_to_tool == tool_name: - verbose_print(cc_pair_info + " - SKIPPED (RB ATTACHED TO TOOL)") - continue - # Skip over touch_bodies that are allowed to touch the RB - if tool_name in rigid_body_state.touch_bodies: - verbose_print(cc_pair_info + " - SKIPPED (TOOL IS TOUCH BODY IN RB)") - continue - # Perform collision check - for body_id in body_ids: - try: - self._check_collision(tool_id, tool_name, body_id, body_name) - verbose_print(cc_pair_info + " body_id '{}' - PASS".format(body_id)) - except CollisionCheckInCollisionError: - verbose_print(cc_pair_info + " body_id '{}' - COLLISION".format(body_id)) - collision_messages.append(cc_pair_info + " body_id '{}' - COLLISION".format(body_id)) - if not full_report: # Fail on first error if full_report is not requested - raise CollisionCheckError("\n".join(collision_messages)) - - if collision_messages: - raise CollisionCheckError("\n".join(collision_messages)) - def _check_collision(self, body_1_id, body_1_name, body_2_id, body_2_name, link_index_1=None, link_index_2=None): # type: (int, str, int, str, int, int) -> None """Internal low-level API to interface with Pybullet for collision checking.