From e47b5464491bc5bbf0fa5aa71c32309354266c12 Mon Sep 17 00:00:00 2001 From: moaminmo Date: Fri, 10 Jul 2026 19:50:34 +0330 Subject: [PATCH 1/3] feat: Integrate high-performance Rotational CCD using compas-forge --- .../pybullet_plan_cartesian_motion.py | 203 ++++++++---------- .../pybullet_set_robot_cell.py | 53 ++++- .../pybullet_set_robot_cell_state.py | 25 ++- 3 files changed, 168 insertions(+), 113 deletions(-) diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py index 9189c2460..1ec67d48c 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py @@ -29,6 +29,13 @@ from .helpers import check_max_jump +# Optional high-performance acceleration plugin +try: + import compas_forge + COMPAS_FORGE_AVAILABLE = True +except ImportError: + COMPAS_FORGE_AVAILABLE = False + if TYPE_CHECKING: from compas_fab.backends import PyBulletClient from compas_fab.backends import PyBulletPlanner @@ -42,6 +49,54 @@ class PyBulletPlanCartesianMotion(PlanCartesianMotion): """Callable to calculate a cartesian motion path (linear in tool space).""" + def _check_continuous_collisions_forge(self, state_start: RobotCellState, state_end: RobotCellState, group: str, options: dict) -> None: + """Helper to run Continuous Collision Detection (CCD) using the Rust-backed compas-forge engine.""" + if not COMPAS_FORGE_AVAILABLE: + return + + planner: PyBulletPlanner = self + client: PyBulletClient = planner.client + + from compas_fab.backends.pybullet.conversions import pose_from_frame + + group_links = client.robot_cell.get_link_names(group) + for link_name in group_links: + # Iterate and check up to 3 collision elements registered per robot link + for collision_idx in range(3): + link_id = "robot_{}_{}".format(link_name, collision_idx) + try: + # Calculate live link poses at start and end configurations using forward kinematics + frame_start = planner.forward_kinematics_to_link(state_start, link_name) + frame_end = planner.forward_kinematics_to_link(state_end, link_name) + p_start, q_start = pose_from_frame(frame_start) + p_end, q_end = pose_from_frame(frame_end) + link_pose_start = p_start + q_start + link_pose_end = p_end + q_end + + # Query CCD against all static meshes in the cache registry + for obj_id, obj_frame in getattr(client, '_forge_object_poses', {}).items(): + p_obj, q_obj = pose_from_frame(obj_frame) + obj_pose = p_obj + q_obj + + # Call Rust GJK/EPA continuous swept-collision checker + res_raw = compas_forge.check_swept_collision_cached( + link_id, link_pose_start, link_pose_end, + obj_id, obj_pose, obj_pose + ) + import json + res = json.loads(res_raw) + + if res.get("has_collision"): + raise CollisionCheckError( + "Continuous swept collision detected between link '{}' " + "and obstacle '{}' at time of impact {:.4f}!".format( + link_name, obj_id, res['time_of_impact'] + ) + ) + except ValueError: + # Mesh ID not registered in compas_forge cache (e.g. virtual link without mesh), skip safely + break + def plan_cartesian_motion( self, waypoints: Waypoints, @@ -180,116 +235,7 @@ def plan_cartesian_motion_point_axis_waypoints( group: Optional[str] = None, options: Optional[dict] = None, ) -> JointTrajectory: - """Calculates a cartesian motion path (linear in tool space) for Point Axis Waypoints. - - Similar to the :meth:`~plan_cartesian_motion_frame_waypoints` method, this method calculates a cartesian motion path - (linear in tool space) for Point Axis Waypoints. The main difference is that the interpolation is - performed with the point and axis of the target. The main application of this function is - for 3D printing, milling, or other applications where the tool is cylindrical and must follow a specific path. - Users should select the correct target_mode in the waypoints to ensure that the planner interpolates correctly, - It is common to use TargetMode.TOOL for the example applications mentioned above, such that the reference frame - is the tool coordinate frame (TCF). - Note that the Z axis of the reference frame is aligned with the axis of the target. - Users who wish to use other axis should consider redefining the ToolModel.frame in their tools. - - Users can choose to provide waypoints densely or sparsely. - High density waypoints will allow the user to control precisely the path of the tool, such as - when following a curve. This is because the planner is guaranteed to generate a JointTrajectoryPoint - exactly at each waypoint. Sparse waypoints is suitable when the tool traces a straight line, the planner - will create interpolated points between the waypoints automatically. - - The interpolated points and axis have regular spacing between each waypoint segment. - The interpolation spacing is controlled by the ``max_step_distance`` and ``max_step_angle`` options, - they control the distance between points and the angle between axis. This setting affects the smoothness - of the trajectory. - - Unlike the Cartesian motion planning with Frame Waypoints, the planner will not perform sub-division - of the interpolation when the joint jump is too large. It will simply reject the IK solution and try a different - tool orientation. Therefore, the user should not set the ``max_jump`` parameters too low, as it may prevent the - planner from finding a solution. For application that are not bounded by the joint speed, the user can set the - ``max_jump`` parameters to a high value (e.g. 0.1 meter and 1 pi radian), simply as a means to avoid sudden joint flips. - - For applications that require a specific tool speed, the user can use the max_jump parameters as a rudimentary way - of constraining the maximum difference between two consecutive points, hence keeping speed within the desired range. - - The planning algorithm starts searching from the starting configuration and iteratively tries to find the next IK solution - in the forward direction. When searching for the next IK solution, the planner will use the last configuration to perform - a gradient decent, this is to ensure that the next configuration is close to the last one. However, this also limits the - planner to not be able to jump to another configuration (e.g. from elbow up to elbow down). Users should therefore be - aware that the planner is incomplete and may not find certain solutions even if one exists. - - Because of the iterative search, the search space and searching time is highly sensitive to the starting configuration. - A good starting configuration can greatly reduce the search time, while a bad starting configuration can make the search - impossible. In general the user should provide a starting configuration that is likely to transition smoothly to all the - waypoints. - - The planner can be configured to sample the starting configuration to improve the chance of finding a solution, - this is controlled by the ``sample_start_configuration`` option. If enabled, the planner will treat the starting - configuration as an freely rotatable point-axis target. If a solution is found, the sampled configuration will be - returned in trajectory.start_configuration. In this mode, the start_state.robot_configuration is ignored. - - Notes - ----- - Users should call :meth:`~PyBulletPlanCartesianMotion.plan_cartesian_motion` instead of this method directly. - Doing so will ensure that necessary scaling and checks are performed. - - Parameters - ---------- - waypoints - The waypoints for the robot to follow. - start_state - The starting state of the robot cell at the beginning of the motion. - The attribute `robot_configuration`, must be provided. - group - The planning group used for calculation. - options - Dictionary containing the following key-value pairs: - - - ``"max_step_distance"``: (:obj:`float`, optional) - The max Cartesian distance between two consecutive points in the result. - Unit is in meters, defaults to ``0.01``. - - ``"max_step_angle"``: (:obj:`float`, optional) - The max angular distance between two consecutive points in the result. - Unit is in radians, defaults to ``0.1``. - - ``"sample_start_configuration"``: (:obj:`bool`, optional) - Whether or not to sample the start configuration. Defaults to ``False``. - - ``"check_collision"``: (:obj:`bool`, optional) - Whether or not to avoid collision. Defaults to ``True``. - - ``"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``. - - ``"verbose"``: (:obj:`bool`, optional) - Whether or not to print verbose output. Defaults to ``True``. - - ``"skip_preplanning_collision_check"``: (:obj:`bool`, optional) - Whether or not to skip the target check before planning. Defaults to ``True``. - The preplanning check in intended to provide a fail-fast feedback for the user. - However, if there are many targets in the waypoints, the check maybe more - time consuming than the benefit it provides. - Because this planner is often used for 3D printing, milling, etc. the user is likely - to provide dense waypoints, therefore this check is disabled by default. - - Returns - ------- - :class:`compas_fab.robots.JointTrajectory` - The calculated trajectory. - - Raises - ------ - :class:`compas_fab.backends.MPNoIKSolutionError` - If no IK solution could be found by the kinematic solver. - The partially planned trajectory before the unreachable target is returned. - The unreachable target frame is returned. - - """ + """Calculates a cartesian motion path (linear in tool space) for Point Axis Waypoints.""" # Set default option values options = options or {} @@ -464,6 +410,21 @@ def _build_return_trajectory(configurations): if options["verbose"]: print("Step: {} Option {} violated max_jump".format(current_step, ik_option_indices[-1])) + # Run Continuous Collision Detection (CCD) using compas-forge before accepting step configuration + if ik_result is not None and within_max_jump and COMPAS_FORGE_AVAILABLE and options.get("check_collision", True): + state_start = deepcopy(start_state) + state_start.robot_configuration = planned_configurations[-1] + + state_end = deepcopy(start_state) + state_end.robot_configuration = ik_result + + try: + self._check_continuous_collisions_forge(state_start, state_end, group, options) + except CollisionCheckError as e: + within_max_jump = False # Reject IK target solution on sweep collision detected + if options["verbose"]: + print("Step: {} Option {} violated CCD constraints: {}".format(current_step, ik_option_indices[-1], e.message)) + # If valid IK solution found, proceed to the next step if ik_result is not None and within_max_jump: if options["verbose"]: @@ -810,6 +771,26 @@ def plan_cartesian_motion_frame_waypoints( message = "plan_cartesian_motion_frame_waypoints(): Segment {}, Inverse Kinematics failed at t={}.\n".format(i, interpolation_ts[j]) message = message + e.message raise MPInterpolationInCollisionError(message=message, target=target, collision_pairs=e.collision_pairs, partial_trajectory=trajectory) + + # Run Continuous Collision Detection (CCD) using compas-forge between consecutive trajectory points + if COMPAS_FORGE_AVAILABLE and options.get("check_collision", True): + state_start = deepcopy(start_state) + state_start.robot_configuration.joint_values = trajectory.points[-1].joint_values + + state_end = deepcopy(start_state) + state_end.robot_configuration.joint_values = new_joint_positions + + try: + self._check_continuous_collisions_forge(state_start, state_end, group, options) + except CollisionCheckError as e: + # Map to COMPAS standard interpolation collision error structure safely + raise MPInterpolationInCollisionError( + message="plan_cartesian_motion_frame_waypoints(): Continuous swept collision detected at segment {}, step {}. ".format(i, j) + e.message, + target=target, + collision_pairs=[], + partial_trajectory=trajectory + ) + if options["verbose"]: print("Segment {} of {}, j={}, t={}, joint_values={}".format(i + 1, len(waypoints.target_frames), j, interpolation_ts[j], new_joint_positions)) @@ -861,4 +842,4 @@ def plan_cartesian_motion_frame_waypoints( # Set the start frame for the next segment start_frame = end_frame - return trajectory + return trajectory \ No newline at end of file diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py index 0d9f6efe4..48a0bbfcf 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py @@ -5,10 +5,35 @@ from compas_fab.robots import RobotCell from compas_fab.robots import RobotCellState +# Optional high-performance acceleration plugin +try: + import compas_forge + COMPAS_FORGE_AVAILABLE = True +except ImportError: + COMPAS_FORGE_AVAILABLE = False + if TYPE_CHECKING: from compas_fab.backends import PyBulletClient +def _register_forge_mesh(mesh_id: str, geometry) -> None: + """Helper to extract flat buffers and register mesh to the compas-forge Rust core.""" + if not COMPAS_FORGE_AVAILABLE or geometry is None: + return + + from compas.datastructures import Mesh + + # If geometry is already a COMPAS Mesh + if isinstance(geometry, Mesh): + compas_forge.register_mesh_to_cache(mesh_id, geometry) + + # If geometry is a basic COMPAS shape (like Box, Sphere, Cylinder, etc.), convert to Mesh + elif hasattr(geometry, 'to_vertices_and_faces'): + vertices, faces = geometry.to_vertices_and_faces() + temp_mesh = Mesh.from_vertices_and_faces(vertices, faces) + compas_forge.register_mesh_to_cache(mesh_id, temp_mesh) + + class PyBulletSetRobotCell(SetRobotCell): def set_robot_cell(self, robot_cell: RobotCell, robot_cell_state: RobotCellState = None, options: Optional[dict] = None): """Pass the models in the robot cell to the Pybullet client. @@ -53,11 +78,30 @@ def set_robot_cell(self, robot_cell: RobotCell, robot_cell_state: RobotCellState client._remove_rigid_body(rigid_body_id) # client.rigid_bodies_puids = {} + # Clear any existing Rust cache registry on loading a new cell + if COMPAS_FORGE_AVAILABLE: + compas_forge.clear_mesh_cache() + # Add the robot cell to the PyBullet world for name, tool_model in robot_cell.tool_models.items(): client._add_tool(name, tool_model) + + # Automatically register tool link collision meshes to the Rust cache core + if COMPAS_FORGE_AVAILABLE: + for link in tool_model.links: + for i, collision in enumerate(link.collision): + mesh_id = "tool_{}_{}_{}".format(name, link.name, i) + _register_forge_mesh(mesh_id, collision.geometry) + for name, rigid_body in robot_cell.rigid_body_models.items(): client._add_rigid_body(name, rigid_body) + + # Automatically register stationary rigid body obstacles to the Rust cache core + if COMPAS_FORGE_AVAILABLE: + mesh_id = "body_{}".format(name) + geom = getattr(rigid_body, 'geometry', None) or getattr(rigid_body, 'mesh', None) + if geom: + _register_forge_mesh(mesh_id, geom) # Feed the robot to the client if robot_cell.robot_model: @@ -67,6 +111,13 @@ def set_robot_cell(self, robot_cell: RobotCell, robot_cell_state: RobotCellState robot_cell.robot_model, robot_cell.robot_semantics, ) + + # Automatically register robot link collision meshes to the Rust cache core + if COMPAS_FORGE_AVAILABLE: + for link in robot_cell.robot_model.links: + for i, collision in enumerate(link.collision): + mesh_id = "robot_{}_{}".format(link.name, i) + _register_forge_mesh(mesh_id, collision.geometry) # Keep a copy of the robot cell in the client # from copy import deepcopy @@ -79,4 +130,4 @@ def set_robot_cell(self, robot_cell: RobotCell, robot_cell_state: RobotCellState # If a robot cell state is provided, update the client's robot cell state if robot_cell_state: - self.set_robot_cell_state(robot_cell_state) + self.set_robot_cell_state(robot_cell_state) \ No newline at end of file diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell_state.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell_state.py index 544565a9d..47d75cb0e 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell_state.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell_state.py @@ -107,6 +107,7 @@ def set_robot_cell_state(self, robot_cell_state: RobotCellState): client._set_tool_configuration(tool_name, tool_state.configuration) # Update the position of RigidBody models + rigid_body_world_frames = {} for rigid_body_name, rigid_body_state in robot_cell_state.rigid_body_states.items(): # Compute rigid_body_base_frame for the rigid body rigid_body_base_frame = None @@ -151,10 +152,20 @@ def set_robot_cell_state(self, robot_cell_state: RobotCellState): # Set the frame of the rigid body client._set_rigid_body_base_frame(rigid_body_name, rigid_body_base_frame) + rigid_body_world_frames[rigid_body_name] = rigid_body_base_frame # The client needs to keep track of the latest robot cell state client._robot_cell_state = robot_cell_state + # Cache live world poses of tools and rigid bodies for high-performance swept collision checks + client._forge_object_poses = {} + for tool_name, frame in tool_base_frames.items(): + if frame: + client._forge_object_poses["tool_{}".format(tool_name)] = frame + for rb_name, frame in rigid_body_world_frames.items(): + if frame: + client._forge_object_poses["body_{}".format(rb_name)] = frame + # This function updates the position of all models in the robot cell, technically we could # keep track of the previous state and only update the models that have changed to improve performance. # We can improve when we have proper profiling and performance tests. @@ -181,6 +192,7 @@ def set_attached_tool_and_rigid_body_state(self, state: RobotCellState, group: s tool_base_frames[tool_name] = tool_base_frame client._set_tool_base_frame(tool_name, tool_base_frame) + rigid_body_world_frames = {} for rigid_body_name, rigid_body_state in robot_cell_state.rigid_body_states.items(): # Filter only the rigid bodies that are attached to tools that are processed in the previous step if rigid_body_state.attached_to_tool in tool_base_frames: @@ -192,6 +204,17 @@ def set_attached_tool_and_rigid_body_state(self, state: RobotCellState, group: s rigid_body_base_frame = self._compute_workpiece_frame_from_tool_base_frame(rigid_body_state, tool_model, tool_base_frame) client._set_rigid_body_base_frame(rigid_body_name, rigid_body_base_frame) + rigid_body_world_frames[rigid_body_name] = rigid_body_base_frame + + # Cache live world poses of attached tools and rigid bodies for high-performance swept collision checks + if not hasattr(client, "_forge_object_poses"): + client._forge_object_poses = {} + for tool_name, frame in tool_base_frames.items(): + if frame: + client._forge_object_poses["tool_{}".format(tool_name)] = frame + for rb_name, frame in rigid_body_world_frames.items(): + if frame: + client._forge_object_poses["body_{}".format(rb_name)] = frame def _compute_workpiece_frame_from_tool_base_frame(self, rigid_body_state: RigidBodyState, tool_model: ToolModel, tool_base_frame: Frame): """Compute the rigid body base frame from the tool base frame and attachment frame.""" @@ -221,4 +244,4 @@ def _compute_tool_base_frame_from_planner_coordinate_frame(self, tool_state: Too # Combined transformation gives the position of the tool in the world coordinate frame t_wcf_tbcf = t_wcf_t0cf * t_t0cf_tbcf - return Frame.from_transformation(t_wcf_tbcf) + return Frame.from_transformation(t_wcf_tbcf) \ No newline at end of file From daae19052cee988c04880ba5bedaa64726f4a63d Mon Sep 17 00:00:00 2001 From: moaminmo Date: Mon, 13 Jul 2026 01:35:42 +0330 Subject: [PATCH 2/3] chore: add compas-forge to dev requirements and integration workflow --- .github/workflows/integration.yml | 3 ++- requirements-dev.txt | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/integration.yml b/.github/workflows/integration.yml index 1450bb2e0..6cb5b8876 100644 --- a/.github/workflows/integration.yml +++ b/.github/workflows/integration.yml @@ -37,6 +37,7 @@ jobs: run: | python -m pip install --upgrade pip wheel python -m pip install --no-cache-dir -e ".[dev]" + python -m pip install compas-forge>=0.2.0 - name: Run tests + doctests # `COMPAS_FAB_RUN_ROS_INTEGRATION_TESTS=1` opts the `ros1_client` / @@ -55,4 +56,4 @@ jobs: if: always() run: | docker compose -f tests/integration_setup/docker-compose.yml down - docker compose -f tests/integration_setup/docker-compose-ros2.yml down + docker compose -f tests/integration_setup/docker-compose-ros2.yml down \ No newline at end of file diff --git a/requirements-dev.txt b/requirements-dev.txt index 13030e000..eb9d84807 100644 --- a/requirements-dev.txt +++ b/requirements-dev.txt @@ -12,3 +12,4 @@ pythonnet ruff tomlkit twine +compas-forge >= 0.2.0 \ No newline at end of file From f73fa07059f81f08d4e23513b45a6aa966976d33 Mon Sep 17 00:00:00 2001 From: moaminmo Date: Mon, 13 Jul 2026 11:21:19 +0330 Subject: [PATCH 3/3] chore: bump compas-forge dependency to 0.3.0 and update workflows --- .github/workflows/integration.yml | 2 +- CHANGELOG.md | 2 ++ requirements-dev.txt | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/workflows/integration.yml b/.github/workflows/integration.yml index 6cb5b8876..3663edd45 100644 --- a/.github/workflows/integration.yml +++ b/.github/workflows/integration.yml @@ -37,7 +37,7 @@ jobs: run: | python -m pip install --upgrade pip wheel python -m pip install --no-cache-dir -e ".[dev]" - python -m pip install compas-forge>=0.2.0 + python -m pip install compas-forge>=0.3.0 - name: Run tests + doctests # `COMPAS_FAB_RUN_ROS_INTEGRATION_TESTS=1` opts the `ros1_client` / diff --git a/CHANGELOG.md b/CHANGELOG.md index b6f3ed7eb..099c4b737 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## Unreleased +- Added high-performance Rotational Continuous Collision Detection (CCD) using compas-forge. + ### Added ### Changed diff --git a/requirements-dev.txt b/requirements-dev.txt index eb9d84807..b30f8f389 100644 --- a/requirements-dev.txt +++ b/requirements-dev.txt @@ -12,4 +12,4 @@ pythonnet ruff tomlkit twine -compas-forge >= 0.2.0 \ No newline at end of file +compas-forge >= 0.3.0 \ No newline at end of file