diff --git a/CHANGELOG.md b/CHANGELOG.md index 8ac23bd39d..3752fabb5a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added +* Added unit test for `PyBulletClient` and `PyBulletPlanner` backend features, including Ik and FK agreement tests. +* Added `PyBulletClient.load_existing_robot` to load from an existing Robot, such as those in RobotLibrary. * Added `compas_fab.robots.Waypoints` class to represent a sequence of targets. It has two child classes: `FrameWaypoints` and `PointAxisWaypoints`. * Added `compas_fab.robots.Target` class to represent a motion planning target. * Added also child classes `FrameTarget`, `PointAxisTarget`, `ConfigurationTarget`, `ConstraintSetTarget` @@ -17,6 +19,14 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed +* 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. +* Renamed `PybulletClient.ensure_cached_robot` to `PybulletClient.ensure_cached_robot_model`. +* Renamed `PybulletClient.ensure_cached_robot_geometry` to `PybulletClient.ensure_cached_robot_model_geometry`. +* Renamed `PybulletClient.cache_robot` to `PybulletClient.cache_robot_model`. +* Backend planners now use multi-inherence instead of `__call__` to include the backend functions. This allows for better generated documentation. +* `Robot.plan_cartesian_motion()` now accepts `Waypoints` as target. Implementation for `FrameWaypoints` is supported with same functionality as before. Simply wrap `Frame` objects using `FrameWaypoints(frames)`. * Changed `BoundingVolume`, `Constraint`, `JointConstraint`, `OrientationConstraint`, `PositionConstraint` to inherit from `compas.data.Data` class. * Change the signature of `plan_motion()` to use `target` (`Target` class) instead of `goal_constraints`. Only one target is accepted. Users who wish to compose their own constraint sets can still use `ConstraintSetTarget`. * Moved `Robot.orientation_constraint_from_frame()` to `OrientationConstraint.from_frame()`, as constraints are no longer intended for users to use directly. diff --git a/docs/developer/backends.rst b/docs/developer/backends.rst index fe43c02608..991bd40fdb 100644 --- a/docs/developer/backends.rst +++ b/docs/developer/backends.rst @@ -82,3 +82,15 @@ Backend interfaces ================== .. automodule:: compas_fab.backends.interfaces + +Implemented backend features +============================ + +The following backend features are implemented for the ROS backend: + +.. automodule:: compas_fab.backends.ros.backend_features + +The following backend features are implemented for the PyBullet backend: + +.. automodule:: compas_fab.backends.pybullet.backend_features + diff --git a/docs/examples/02_description_models/03_targets.rst b/docs/examples/02_description_models/03_targets.rst index de0bf9ce62..021c59346c 100644 --- a/docs/examples/02_description_models/03_targets.rst +++ b/docs/examples/02_description_models/03_targets.rst @@ -1,16 +1,15 @@ .. _targets: ******************************************************************************* -Targets +Targets and Waypoints ******************************************************************************* ----------------------- -Single Targets (Static) +Targets (Single Goal) ----------------------- Target classes are used to describe the goal condition (i.e. end condition) of a robot -for motion planning. They can be used for both Free Motion Planning (FMP) and Cartesian -Motion Planning (CMP). +for motion planning. They can be used for Free-space Motion Planning with :meth:`compas_fab.robots.Robot.plan_motion`. The :class:`compas_fab.robots.FrameTarget` is the most common target for motion planning. It defines the complete pose of the end-effector (or the robot flange, if no tool is attached). @@ -38,3 +37,29 @@ The :class:`compas_fab.robots.ConstraintSetTarget` class is used to specify a li constraints as a planning target. This is intended for advanced users who want to create custom combination of constraints. See :class:`compas_fab.robots.Constraint` for available constraints. At the moment, only the ROS MoveIt planning backend supports this target type. + +.. _waypoints: + +------------------------------------------ +Waypoints (Multiple Points / Segments) +------------------------------------------ + +Waypoints classes are used to describe a sequence of +waypoints that the robot should pass through in a planned motion. They are similar to Targets classes +but contain a list of targets instead of a single target, which is useful for tasks such as +drawing, welding or 3D printing. +They can be used for Cartesian Motion Planning with :meth:`compas_fab.robots.Robot.plan_cartesian_motion`. + +The :class:`compas_fab.robots.FrameWaypoints` is the most common waypoint for Cartesian motion planning. +It defines a list of complete pose for the end-effector (or the robot flange, if no tool is attached). +It is created by a list of :class:`compas.geometry.Frame` objects or alternatively from a list of +:class:`compas.geometry.Transformation` objects. + +The :class:`compas_fab.robots.PointAxisWaypoints` class is used for specifying a list of waypoints based on +the Point-Axis concept used in the :class:`compas_fab.robots.PointAxisTarget`. Compared to +:class:`~compas_fab.robots.FrameWaypoints`, this class allows for specifying targets where the rotation +around the Z-axis is not fixed. This is useful for example when the robot is using a cylindrical tool +to perform a task, for example 3D printing, welding or drilling. The freely rotating axis is defined relative +to the Z-axis of the tool coordinate frame (TCF). Note that the orientation of the tool +at the end of the motion is not determined until after the motion is planned. + diff --git a/docs/examples/03_backends_ros/files/04_plan_cartesian_motion.py b/docs/examples/03_backends_ros/files/04_plan_cartesian_motion.py index 69b1be796c..7b74fcd9fb 100644 --- a/docs/examples/03_backends_ros/files/04_plan_cartesian_motion.py +++ b/docs/examples/03_backends_ros/files/04_plan_cartesian_motion.py @@ -1,25 +1,25 @@ from compas.geometry import Frame from compas_fab.backends import RosClient +from compas_fab.robots import FrameWaypoints with RosClient() as client: robot = client.load_robot() - assert robot.name == 'ur5_robot' + assert robot.name == "ur5_robot" frames = [] frames.append(Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])) frames.append(Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0])) + waypoints = FrameWaypoints(frames) start_configuration = robot.zero_configuration() start_configuration.joint_values = (-0.042, 0.033, -2.174, 5.282, -1.528, 0.000) options = { - 'max_step': 0.01, - 'avoid_collisions': True, + "max_step": 0.01, + "avoid_collisions": True, } - trajectory = robot.plan_cartesian_motion(frames, - start_configuration, - options=options) + trajectory = robot.plan_cartesian_motion(waypoints, start_configuration, options=options) print("Computed cartesian path with %d configurations, " % len(trajectory.points)) print("following %d%% of requested trajectory." % (trajectory.fraction * 100)) diff --git a/docs/examples/03_backends_ros/files/gh_plan_cartesian_motion.py b/docs/examples/03_backends_ros/files/gh_plan_cartesian_motion.py index 1a72566fd8..89446644e5 100644 --- a/docs/examples/03_backends_ros/files/gh_plan_cartesian_motion.py +++ b/docs/examples/03_backends_ros/files/gh_plan_cartesian_motion.py @@ -22,11 +22,12 @@ :class:`compas_fab.robots.JointTrajectory` The calculated trajectory. """ + from __future__ import print_function import scriptcontext as sc from compas.geometry import Frame - +from compas_fab.robots import FrameWaypoints guid = str(ghenv.Component.InstanceGuid) response_key = "response_" + guid @@ -38,14 +39,14 @@ if robot and robot.client and start_configuration and compute: if robot.client.is_connected: options = { - 'max_step': float(max_step), - 'avoid_collisions': bool(avoid_collisions), - 'attached_collision_meshes': list(attached_colllision_meshes), + "max_step": float(max_step), + "avoid_collisions": bool(avoid_collisions), + "attached_collision_meshes": list(attached_colllision_meshes), } - sc.sticky[response_key] = robot.plan_cartesian_motion(frames, - start_configuration=start_configuration, - group=group, - options=options) + waypoints = FrameWaypoints(frames) + sc.sticky[response_key] = robot.plan_cartesian_motion( + waypoints, start_configuration=start_configuration, group=group, options=options + ) else: print("Robot client is not connected") diff --git a/docs/examples/06_backends_kinematics/files/04_cartesian_path_analytic_pybullet.py b/docs/examples/06_backends_kinematics/files/04_cartesian_path_analytic_pybullet.py index 42d339bb9f..9e311a70f4 100644 --- a/docs/examples/06_backends_kinematics/files/04_cartesian_path_analytic_pybullet.py +++ b/docs/examples/06_backends_kinematics/files/04_cartesian_path_analytic_pybullet.py @@ -2,6 +2,7 @@ from compas.geometry import Frame from compas_fab.backends import AnalyticalPyBulletClient +from compas_fab.robots import FrameWaypoints frames_WCF = [ Frame((0.407, 0.073, 0.320), (0.922, 0.000, 0.388), (0.113, 0.956, -0.269)), @@ -16,7 +17,8 @@ options = {"solver": "ur5", "check_collision": True} start_configuration = list(robot.iter_inverse_kinematics(frames_WCF[0], options=options))[-1] - trajectory = robot.plan_cartesian_motion(frames_WCF, start_configuration=start_configuration, options=options) + waypoints = FrameWaypoints(frames_WCF) + trajectory = robot.plan_cartesian_motion(waypoints, start_configuration=start_configuration, options=options) assert trajectory.fraction == 1.0 j = [c.joint_values for c in trajectory.points] diff --git a/src/compas_fab/backends/__init__.py b/src/compas_fab/backends/__init__.py index a635e6a543..9501fdb169 100644 --- a/src/compas_fab/backends/__init__.py +++ b/src/compas_fab/backends/__init__.py @@ -149,6 +149,7 @@ ) if not compas.IPY: + # PyBullet do not work in IronPython from .pybullet import ( PyBulletClient, CollisionError, diff --git a/src/compas_fab/backends/interfaces/__init__.py b/src/compas_fab/backends/interfaces/__init__.py index 0c2f7d4455..1e297bab1c 100644 --- a/src/compas_fab/backends/interfaces/__init__.py +++ b/src/compas_fab/backends/interfaces/__init__.py @@ -21,6 +21,7 @@ :toctree: generated/ :nosignatures: + BackendFeature ForwardKinematics InverseKinematics PlanMotion @@ -46,6 +47,7 @@ from .backend_features import AddAttachedCollisionMesh from .backend_features import AddCollisionMesh from .backend_features import AppendCollisionMesh +from .backend_features import BackendFeature from .backend_features import ForwardKinematics from .backend_features import GetPlanningScene from .backend_features import InverseKinematics @@ -58,17 +60,18 @@ from .client import PlannerInterface __all__ = [ + "AddAttachedCollisionMesh", + "AddCollisionMesh", + "AppendCollisionMesh", + "BackendFeature", + "ClientInterface", "ForwardKinematics", + "GetPlanningScene", "InverseKinematics", - "PlanMotion", "PlanCartesianMotion", - "GetPlanningScene", - "AddCollisionMesh", - "AppendCollisionMesh", + "PlanMotion", + "PlannerInterface", "RemoveCollisionMesh", - "AddAttachedCollisionMesh", "RemoveAttachedCollisionMesh", "ResetPlanningScene", - "ClientInterface", - "PlannerInterface", ] diff --git a/src/compas_fab/backends/interfaces/backend_features.py b/src/compas_fab/backends/interfaces/backend_features.py index 388f3d93a6..f688cece73 100644 --- a/src/compas_fab/backends/interfaces/backend_features.py +++ b/src/compas_fab/backends/interfaces/backend_features.py @@ -2,18 +2,39 @@ 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_fab.backends.interfaces import ClientInterface # noqa: F401 + + +class BackendFeature(object): + """Base class for all backend features that are implemented by a backend client. + + Attributes + ---------- + client : :class:`compas_fab.backends.interfaces.ClientInterface` + The backend client that supports this feature. -class ForwardKinematics(object): - """Interface for a Planner's forward kinematics feature. Any implementation of - ``ForwardKinematics`` must define the method ``forward_kinematics``. The - ``__call__`` magic method allows an instance of an implementation of - ``ForwardKinematics`` to be treated as its ``forward_kinematics`` method. See - and - . """ - def __call__(self, robot, configuration, group=None, options=None): - return self.forward_kinematics(robot, configuration, group, options) + def __init__(self, client): + # All backend features are assumed to be associated with a backend client. + self.client = client # type: ClientInterface + + +# The code that contains the actual feature implementation is located in the backend's module. +# For example, the features for moveit planner and ros client are located in : +# "src/compas_fab/backends/ros/backend_features/" +# If you cannot a specific feature in the 'backend_features', it means that the planner +# does not support that feature. + + +class ForwardKinematics(BackendFeature): + """Mix-in interface for implementing a planner's forward kinematics feature.""" def forward_kinematics(self, robot, configuration, group=None, options=None): """Calculate the robot's forward kinematic. @@ -22,7 +43,7 @@ def forward_kinematics(self, robot, configuration, group=None, options=None): ---------- robot : :class:`compas_fab.robots.Robot` The robot instance for which forward kinematics is being calculated. - configuration : :class:`compas_fab.robots.Configuration` + configuration : :class:`compas_robots.Configuration` The full configuration to calculate the forward kinematic for. If no full configuration is passed, the zero-joint state for the other configurable joints is assumed. @@ -39,18 +60,12 @@ def forward_kinematics(self, robot, configuration, group=None, options=None): """ pass + # The implementation code is located in the backend's module: + # "src/compas_fab/backends//backend_features/_forward_kinematics.py" -class InverseKinematics(object): - """Interface for a Planner's inverse kinematics feature. Any implementation of - ``InverseKinematics`` must define the method ``inverse_kinematics``. The - ``__call__`` magic method allows an instance of an implementation of - ``InverseKinematics`` to be treated as its ``inverse_kinematics`` method. See - and - . - """ - def __call__(self, robot, frame_WCF, start_configuration=None, group=None, options=None): - return self.inverse_kinematics(robot, frame_WCF, start_configuration, group, options) +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): """Calculate the robot's inverse kinematic for a given frame. @@ -61,12 +76,12 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N ---------- robot : :class:`compas_fab.robots.Robot` The robot instance for which inverse kinematics is being calculated. - frame_WCF: :class:`compas.geometry.Frame` + frame_WCF : :class:`compas.geometry.Frame` The frame to calculate the inverse for. - start_configuration: :class:`compas_fab.robots.Configuration`, optional - group: str, optional + start_configuration : :class:`compas_robots.Configuration`, optional + group : str, optional The planning group used for calculation. - options: dict, optional + options : dict, optional Dictionary containing kwargs for arguments specific to the client being queried. @@ -77,18 +92,12 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N """ pass + # The implementation code is located in the backend's module: + # "src/compas_fab/backends//backend_features/_inverse_kinematics" -class PlanMotion(object): - """Interface for a Planner's plan motion feature. Any implementation of - ``PlanMotion`` must define the method ``plan_motion``. The - ``__call__`` magic method allows an instance of an implementation of - ``PlanMotion`` to be treated as its ``plan_motion`` method. See - and - . - """ - def __call__(self, robot, target, start_configuration=None, group=None, options=None): - return self.plan_motion(robot, target, start_configuration, group, options) +class PlanMotion(BackendFeature): + """Mix-in interface for implementing a planner's plan motion feature.""" def plan_motion(self, robot, target, start_configuration=None, group=None, options=None): """Calculates a motion path. @@ -97,12 +106,12 @@ def plan_motion(self, robot, target, start_configuration=None, group=None, optio ---------- robot : :class:`compas_fab.robots.Robot` The robot instance for which the motion path is being calculated. - target: :class:`compas_fab.robots.Target` + target : :class:`compas_fab.robots.Target` The goal for the robot to achieve. - start_configuration: :class:`compas_fab.robots.Configuration`, optional + start_configuration : :class:`compas_robots.Configuration`, optional The robot's full configuration, i.e. values for all configurable joints of the entire robot, at the starting position. - group: str, optional + group : str, optional The name of the group to plan for. options : dict, optional Dictionary containing kwargs for arguments specific to @@ -115,34 +124,28 @@ def plan_motion(self, robot, target, start_configuration=None, group=None, optio """ pass + # The implementation code is located in the backend's module: + # "src/compas_fab/backends//backend_features/_plan_motion.py" -class PlanCartesianMotion(object): - """Interface for a Planner's plan cartesian motion feature. Any implementation of - ``PlanCartesianMotion`` must define the method ``plan_cartesian_motion``. The - ``__call__`` magic method allows an instance of an implementation of - ``PlanCartesianMotion`` to be treated as its ``plan_cartesian_motion`` method. See - and - . - """ - def __call__(self, robot, frames_WCF, start_configuration=None, group=None, options=None): - return self.plan_cartesian_motion(robot, frames_WCF, start_configuration, group, options) +class PlanCartesianMotion(BackendFeature): + """Mix-in interface for implementing a planner's plan cartesian motion feature.""" - def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, group=None, options=None): + def plan_cartesian_motion(self, robot, waypoints, start_configuration=None, group=None, options=None): """Calculates a cartesian motion path (linear in tool space). Parameters ---------- robot : :class:`compas_fab.robots.Robot` The robot instance for which the cartesian motion path is being calculated. - frames_WCF: list of :class:`compas.geometry.Frame` - The frames through which the path is defined. - start_configuration: :class:`compas_robots.Configuration`, optional + waypoints : :class:`compas_fab.robots.Waypoints` + The waypoints for the robot to follow. + start_configuration : :class:`compas_robots.Configuration`, optional The robot's full configuration, i.e. values for all configurable joints of the entire robot, at the starting position. - group: str, optional + group : str, optional The planning group used for calculation. - options: dict, optional + options : dict, optional Dictionary containing kwargs for arguments specific to the client being queried. @@ -153,18 +156,12 @@ def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, gro """ pass + # The implementation code is located in the backend's module: + # "src/compas_fab/backends//backend_features/_plan_cartesian_motion.py" -class GetPlanningScene(object): - """Interface for a Planner's get planning scene feature. Any implementation of - ``GetPlanningScene`` must define the method ``get_planning_scene``. The - ``__call__`` magic method allows an instance of an implementation of - ``GetPlanningScene`` to be treated as its ``get_planning_scene`` method. See - and - . - """ - def __call__(self, options=None): - return self.get_planning_scene(options) +class GetPlanningScene(BackendFeature): + """Mix-in interface for implementing a planner's get planning scene feature.""" def get_planning_scene(self, options=None): """Retrieve the planning scene. @@ -181,21 +178,15 @@ def get_planning_scene(self, options=None): """ pass + # The implementation code is located in the backend's module: + # "src/compas_fab/backends//backend_features/_get_planning_scene.py" -class ResetPlanningScene(object): - """Interface for a Planner's reset planning scene feature. Any implementation of - ``ResetPlanningScene`` must define the method ``reset_planning_scene``. The - ``__call__`` magic method allows an instance of an implementation of - ``ResetPlanningScene`` to be treated as its ``reset_planning_scene`` method. See - and - . - """ - def __call__(self, options=None): - return self.reset_planning_scene(options) +class ResetPlanningScene(BackendFeature): + """Mix-in interface for implementing a planner's reset planning scene feature.""" def reset_planning_scene(self, options=None): - """Retrieve the planning scene. + """Resets the planning scene, removing all added collision meshes. Parameters ---------- @@ -209,18 +200,12 @@ def reset_planning_scene(self, options=None): """ pass + # The implementation code is located in the backend's module: + # "src/compas_fab/backends//backend_features/_reset_planning_scene.py" -class AddCollisionMesh(object): - """Interface for a Planner's add collision mesh feature. Any implementation of - ``AddCollisionMesh`` must define the method ``add_collision_mesh``. The - ``__call__`` magic method allows an instance of an implementation of - ``AddCollisionMesh`` to be treated as its ``add_collision_mesh`` method. See - and - . - """ - def __call__(self, collision_mesh, options=None): - return self.add_collision_mesh(collision_mesh, options) +class AddCollisionMesh(BackendFeature): + """Mix-in interface for implementing a planner's add collision mesh feature.""" def add_collision_mesh(self, collision_mesh, options=None): """Add a collision mesh to the planning scene. @@ -239,18 +224,12 @@ def add_collision_mesh(self, collision_mesh, options=None): """ pass + # The implementation code is located in the backend's module: + # "src/compas_fab/backends//backend_features/_add_collision_mesh.py" -class RemoveCollisionMesh(object): - """Interface for a Planner's remove collision mesh feature. Any implementation of - ``RemoveCollisionMesh`` must define the method ``remove_collision_mesh``. The - ``__call__`` magic method allows an instance of an implementation of - ``RemoveCollisionMesh`` to be treated as its ``remove_collision_mesh`` method. See - and - . - """ - def __call__(self, id, options=None): - return self.remove_collision_mesh(id, options) +class RemoveCollisionMesh(BackendFeature): + """Mix-in interface for implementing a planner's remove collision mesh feature.""" def remove_collision_mesh(self, id, options=None): """Remove a collision mesh from the planning scene. @@ -270,17 +249,8 @@ def remove_collision_mesh(self, id, options=None): pass -class AppendCollisionMesh(object): - """Interface for a Planner's append collision mesh feature. Any implementation of - ``AppendCollisionMesh`` must define the method ``append_collision_mesh``. The - ``__call__`` magic method allows an instance of an implementation of - ``AppendCollisionMesh`` to be treated as its ``append_collision_mesh`` method. See - and - . - """ - - def __call__(self, collision_mesh, options=None): - return self.append_collision_mesh(collision_mesh, options) +class AppendCollisionMesh(BackendFeature): + """Mix-in interface for implementing a planner's append collision mesh feature.""" def append_collision_mesh(self, collision_mesh, options=None): """Append a collision mesh to the planning scene. @@ -299,18 +269,12 @@ def append_collision_mesh(self, collision_mesh, options=None): """ pass + # The implementation code is located in the backend's module: + # "src/compas_fab/backends//backend_features/_append_collision_mesh.py" -class AddAttachedCollisionMesh(object): - """Interface for a Planner's add attached collision mesh feature. Any implementation of - ``AddAttachedCollisionMesh`` must define the method ``add_attached_collision_mesh``. The - ``__call__`` magic method allows an instance of an implementation of - ``AddAttachedCollisionMesh`` to be treated as its ``add_attached_collision_mesh`` method. See - and - . - """ - def __call__(self, attached_collision_mesh, options=None): - return self.add_attached_collision_mesh(attached_collision_mesh, options) +class AddAttachedCollisionMesh(BackendFeature): + """Mix-in interface for implementing a planner's add attached collision mesh feature.""" def add_attached_collision_mesh(self, attached_collision_mesh, options=None): """Add a collision mesh and attach it to the robot. @@ -329,18 +293,12 @@ def add_attached_collision_mesh(self, attached_collision_mesh, options=None): """ pass + # The implementation code is located in the backend's module: + # "src/compas_fab/backends//backend_features/_add_attached_collision_mesh.py" -class RemoveAttachedCollisionMesh(object): - """Interface for a Planner's remove attached collision mesh feature. Any implementation of - ``RemoveAttachedCollisionMesh`` must define the method ``remove_attached_collision_mesh``. The - ``__call__`` magic method allows an instance of an implementation of - ``RemoveAttachedCollisionMesh`` to be treated as its ``remove_attached_collision_mesh`` method. See - and - . - """ - def __call__(self, id, options=None): - return self.remove_attached_collision_mesh(id, options) +class RemoveAttachedCollisionMesh(BackendFeature): + """Mix-in interface for implementing a planner's remove attached collision mesh feature.""" def remove_attached_collision_mesh(self, id, options=None): """Remove an attached collision mesh from the robot. @@ -358,3 +316,6 @@ def remove_attached_collision_mesh(self, id, options=None): ``None`` """ pass + + # The implementation code is located in the backend's module: + # "src/compas_fab/backends//backend_features/_remove_attached_collision_mesh.py" diff --git a/src/compas_fab/backends/interfaces/client.py b/src/compas_fab/backends/interfaces/client.py index 057c04e53f..ecde965209 100644 --- a/src/compas_fab/backends/interfaces/client.py +++ b/src/compas_fab/backends/interfaces/client.py @@ -106,7 +106,7 @@ class PlannerInterface(object): """ def __init__(self, client): - super(PlannerInterface, self).__init__() + # super(PlannerInterface, self).__init__() self.client = client # ========================================================================== diff --git a/src/compas_fab/backends/kinematics/analytical_inverse_kinematics.py b/src/compas_fab/backends/kinematics/analytical_inverse_kinematics.py index 3990c0e7ea..59c6075020 100644 --- a/src/compas_fab/backends/kinematics/analytical_inverse_kinematics.py +++ b/src/compas_fab/backends/kinematics/analytical_inverse_kinematics.py @@ -103,7 +103,7 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N if options.get("check_collision", False) is True: acms = options.get("attached_collision_meshes", []) for acm in acms: - cached_robot_model = self.client.get_cached_robot(robot) + cached_robot_model = self.client.get_cached_robot_model(robot) if not cached_robot_model.get_link_by_name(acm.collision_mesh.id): self.client.add_attached_collision_mesh(acm, options={"robot": robot}) for touch_link in acm.touch_links: diff --git a/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py b/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py index 28ea26e126..7dccb1d0e4 100644 --- a/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py +++ b/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py @@ -7,6 +7,9 @@ from compas_fab.backends.kinematics.utils import smallest_joint_angles from compas_fab.robots import JointTrajectory from compas_fab.robots import JointTrajectoryPoint +from compas_fab.robots import FrameWaypoints +from compas_fab.robots import PointAxisWaypoints +from compas_fab.utilities import from_tcf_to_t0cf class AnalyticalPlanCartesianMotion(PlanCartesianMotion): @@ -15,23 +18,22 @@ class AnalyticalPlanCartesianMotion(PlanCartesianMotion): def __init__(self, client=None): self.client = client - def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, group=None, options=None): + def plan_cartesian_motion(self, robot, waypoints, start_configuration=None, group=None, options=None): """Calculates a cartesian motion path (linear in tool space). Parameters ---------- robot : :class:`compas_fab.robots.Robot` The robot instance for which the cartesian motion path is being calculated. - frames_WCF : list of :class:`compas.geometry.Frame` - The frames through which the path is defined. + waypoints : :class:`compas_fab.robots.Waypoints` + The waypoints for the robot to follow. start_configuration : :class:`compas_robots.Configuration`, optional The robot's full configuration, i.e. values for all configurable joints of the entire robot, at the starting position. group : str, optional The planning group used for calculation. options : dict, optional - Dictionary containing kwargs for arguments specific to - the client being queried. + Dictionary containing the key-value pairs that are passed to :func:`compas_fab.robots.Robot.iter_inverse_kinematics` Returns ------- @@ -42,22 +44,55 @@ def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, gro ----- This will only work with robots that have 6 revolute joints. """ - # what is the expected behaviour of that function? - # - Return all possible paths or select only the one that is closest to the start_configuration? - # - Do we use a stepsize to sample in between frames or use only the input frames? + + if isinstance(waypoints, FrameWaypoints): + return self._plan_cartesian_motion_with_frame_waypoints( + robot, waypoints, start_configuration, group, options + ) + elif isinstance(waypoints, PointAxisWaypoints): + return self._plan_cartesian_motion_with_point_axis_waypoints( + robot, waypoints, start_configuration, group, options + ) + else: + raise TypeError("Unsupported waypoints type {}".format(type(waypoints))) + + def _plan_cartesian_motion_with_frame_waypoints( + self, robot, waypoints, start_configuration=None, group=None, options=None + ): + """Calculates a cartesian motion path with frame waypoints. + + Planner behavior: + - If multiple paths are possible (i.e. due to multiple IK results), only the one that is closest to the start_configuration is returned. + - The path is checked to ensure that the joint values are continuous and that revolution values are the smallest possible. + - There is no interpolation in between frames (i.e. 'max_step' parameter is not supported), only the input frames are used. + """ + # convert the target frames to the robot's base frame + if waypoints.tool_coordinate_frame is not None: + frames_WCF = [from_tcf_to_t0cf(frame, waypoints.tool_coordinate_frame) for frame in waypoints.target_frames] + else: + frames_WCF = waypoints.target_frames # convert the frame WCF to RCF base_frame = robot.get_base_frame(group=group, full_configuration=start_configuration) frames_RCF = [base_frame.to_local_coordinates(frame_WCF) for frame_WCF in frames_WCF] + # 'keep_order' is set to True, so that iter_inverse_kinematics will return the configurations in the same order across all frames options = options or {} options.update({"keep_order": True}) + # iterate over all input frames and calculate the inverse kinematics, no interpolation in between frames configurations_along_path = [] for frame in frames_RCF: configurations = list(robot.iter_inverse_kinematics(frame, options=options)) configurations_along_path.append(configurations) + # Analytical backend only supports robots with finite IK solutions + # For 6R articulated robots, there is a maximum of 8 possible paths, corresponding to the 8 possible IK solutions for each frame + # The `options.update({"keep_order": True})` ensures that the order of the configurations is the same across all frames + # but this also cause some configurations to be None, if no solution was found. + + # The `all(configurations)` below is used to check if all configurations in a path are present. + # indicating that a complete trajectory was found. paths = [] for configurations in zip(*configurations_along_path): if all(configurations): @@ -75,11 +110,20 @@ def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, gro path = self.smooth_configurations(path) trajectory = JointTrajectory() trajectory.fraction = len(path) / len(frames_RCF) + # Technically trajectory.fraction should always be 1.0 because otherwise, the path would be rejected earlier trajectory.joint_names = path[0].joint_names trajectory.points = [JointTrajectoryPoint(config.joint_values, config.joint_types) for config in path] trajectory.start_configuration = robot.merge_group_with_full_configuration(path[0], start_configuration, group) return trajectory + def _plan_cartesian_motion_with_point_axis_waypoints( + self, robot, waypoints, start_configuration=None, group=None, options=None + ): + """Planning Cartesian motion with PointAxisWaypoints is not yet implemented in the Analytical backend.""" + raise NotImplementedError( + "Planning Cartesian motion with PointAxisWaypoints is not yet implemented in the Analytical backend." + ) + def smooth_configurations(self, configurations): joint_values_corrected = [] prev = smallest_joint_angles(configurations[0].joint_values) diff --git a/src/compas_fab/backends/pybullet/backend_features/__init__.py b/src/compas_fab/backends/pybullet/backend_features/__init__.py index 1a3fada10a..3b1be94c9c 100644 --- a/src/compas_fab/backends/pybullet/backend_features/__init__.py +++ b/src/compas_fab/backends/pybullet/backend_features/__init__.py @@ -1,5 +1,26 @@ +""" +PyBullet backend features +========================= + +.. autosummary:: + :toctree: generated/ + :nosignatures: + + PyBulletAddAttachedCollisionMesh + PyBulletAddCollisionMesh + PyBulletAppendCollisionMesh + PyBulletForwardKinematics + PyBulletInverseKinematics + PyBulletRemoveAttachedCollisionMesh + PyBulletRemoveCollisionMesh + + + +""" + from __future__ import absolute_import + from compas_fab.backends.pybullet.backend_features.pybullet_add_attached_collision_mesh import ( PyBulletAddAttachedCollisionMesh, ) @@ -17,8 +38,8 @@ "PyBulletAddAttachedCollisionMesh", "PyBulletAddCollisionMesh", "PyBulletAppendCollisionMesh", - "PyBulletRemoveCollisionMesh", - "PyBulletRemoveAttachedCollisionMesh", "PyBulletForwardKinematics", "PyBulletInverseKinematics", + "PyBulletRemoveAttachedCollisionMesh", + "PyBulletRemoveCollisionMesh", ] diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_add_attached_collision_mesh.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_add_attached_collision_mesh.py index 35774f0efc..ee14a2815b 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_add_attached_collision_mesh.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_add_attached_collision_mesh.py @@ -25,9 +25,6 @@ class PyBulletAddAttachedCollisionMesh(AddAttachedCollisionMesh): """Callable to add a collision mesh and attach it to the robot.""" - def __init__(self, client): - self.client = client - def add_attached_collision_mesh(self, attached_collision_mesh, options=None): """Add a collision mesh and attach it to the robot. @@ -63,7 +60,7 @@ def add_attached_collision_mesh(self, attached_collision_mesh, options=None): ``None`` """ robot = options["robot"] - self.client.ensure_cached_robot_geometry(robot) + self.client.ensure_cached_robot_model_geometry(robot) mass = options.get("mass", 1.0) concavity = options.get("concavity", False) @@ -71,7 +68,7 @@ def add_attached_collision_mesh(self, attached_collision_mesh, options=None): inertial_origin = options.get("inertial_origin", Frame.worldXY()) collision_origin = options.get("collision_origin", Frame.worldXY()) - cached_robot_model = self.client.get_cached_robot(robot) + cached_robot_model = self.client.get_cached_robot_model(robot) # add link mesh = attached_collision_mesh.collision_mesh.mesh diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_add_collision_mesh.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_add_collision_mesh.py index 1cad8fdd5c..81fbace793 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_add_collision_mesh.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_add_collision_mesh.py @@ -14,9 +14,6 @@ class PyBulletAddCollisionMesh(AddCollisionMesh): """Callable to add a collision mesh to the planning scene.""" - def __init__(self, client): - self.client = client - def add_collision_mesh(self, collision_mesh, options=None): """Add a collision mesh to the planning scene. diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_append_collision_mesh.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_append_collision_mesh.py index 416618769d..f24ce0727f 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_append_collision_mesh.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_append_collision_mesh.py @@ -12,9 +12,6 @@ class PyBulletAppendCollisionMesh(AppendCollisionMesh): """Callable to append a collision mesh to the planning scene.""" - def __init__(self, client): - self.client = client - def append_collision_mesh(self, collision_mesh, options=None): """Append a collision mesh to the planning scene. 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 37742bfbb9..f988aaed18 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 @@ -8,9 +8,6 @@ class PyBulletForwardKinematics(ForwardKinematics): """Callable to calculate the robot's forward kinematic.""" - def __init__(self, client): - self.client = client - def forward_kinematics(self, robot, configuration, group=None, options=None): """Calculate the robot's forward kinematic. @@ -39,10 +36,12 @@ def forward_kinematics(self, robot, configuration, group=None, options=None): :class:`Frame` The frame in the world's coordinate system (WCF). """ + options = options or {"link": None, "check_collision": False} + link_name = options.get("link") or robot.get_end_effector_link_name(group) - cached_robot = self.client.get_cached_robot(robot) - body_id = self.client.get_uid(cached_robot) - link_id = self.client._get_link_id_by_name(link_name, cached_robot) + 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) self.client.set_robot_configuration(robot, configuration, group) frame = self.client._get_link_frame(link_id, body_id) if options.get("check_collision"): 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 0a6eaa1153..eee4e1f2e3 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 @@ -23,9 +23,6 @@ class PyBulletInverseKinematics(InverseKinematics): """Callable to calculate the robot's inverse kinematics for a given frame.""" - def __init__(self, client): - self.client = client - def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=None, options=None): """Calculate the robot's inverse kinematic for a given frame. @@ -77,12 +74,12 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N 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 = self.client.get_cached_robot(robot) - body_id = self.client.get_uid(cached_robot) - link_id = self.client._get_link_id_by_name(link_name, cached_robot) + 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) - joints = cached_robot.get_configurable_joints() + joints = cached_robot_model.get_configurable_joints() joints.sort(key=lambda j: j.attr["pybullet"]["id"]) joint_names = [joint.name for joint in joints] @@ -141,7 +138,7 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N ik_options.update( dict( joints=joints, - threshold=options.get("high_accuracy_threshold", 1e-6), + threshold=options.get("high_accuracy_threshold", 1e-4), max_iter=options.get("high_accuracy_max_iter", 20), ) ) @@ -186,7 +183,6 @@ def _accurate_inverse_kinematics(self, joints, threshold, max_iter, **kwargs): # https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_kinematics_husky_kuka.py#L81 close_enough = False iter = 0 - distance = None joint_ids = [joint.attr["pybullet"]["id"] for joint in joints] body_id = kwargs["bodyUniqueId"] link_id = kwargs["endEffectorLinkIndex"] @@ -205,8 +201,10 @@ def _accurate_inverse_kinematics(self, joints, threshold, max_iter, **kwargs): target_position[1] - new_pose[1], target_position[2] - new_pose[2], ] - distance = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2] - close_enough = distance < threshold + # 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 + close_enough = distance_squared < threshold * threshold kwargs["restPoses"] = joint_poses iter += 1 diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_attached_collision_mesh.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_attached_collision_mesh.py index 1bbedf0010..698b5192cd 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_attached_collision_mesh.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_attached_collision_mesh.py @@ -16,9 +16,6 @@ class PyBulletRemoveAttachedCollisionMesh(RemoveAttachedCollisionMesh): """Callable to remove an attached collision mesh from the robot.""" - def __init__(self, client): - self.client = client - def remove_attached_collision_mesh(self, id, options=None): """Remove an attached collision mesh from the robot. @@ -37,9 +34,9 @@ def remove_attached_collision_mesh(self, id, options=None): ``None`` """ robot = options["robot"] - self.client.ensure_cached_robot_geometry(robot) + self.client.ensure_cached_robot_model_geometry(robot) - cached_robot_model = self.client.get_cached_robot(robot) + cached_robot_model = self.client.get_cached_robot_model(robot) # remove link and fixed joint cached_robot_model.remove_link(id) diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_collision_mesh.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_collision_mesh.py index 226909a65f..7179b784b7 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_collision_mesh.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_collision_mesh.py @@ -17,9 +17,6 @@ class PyBulletRemoveCollisionMesh(RemoveCollisionMesh): """Callable to remove a collision mesh from the planning scene.""" - def __init__(self, client): - self.client = client - def remove_collision_mesh(self, id, options=None): """Remove a collision mesh from the planning scene. diff --git a/src/compas_fab/backends/pybullet/client.py b/src/compas_fab/backends/pybullet/client.py index df374a21cb..ab1d8cad13 100644 --- a/src/compas_fab/backends/pybullet/client.py +++ b/src/compas_fab/backends/pybullet/client.py @@ -32,6 +32,11 @@ pybullet = LazyLoader("pybullet", globals(), "pybullet") +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import list __all__ = [ "PyBulletClient", @@ -123,7 +128,7 @@ def is_connected(self): class PyBulletClient(PyBulletBase, ClientInterface): """Interface to use pybullet as backend. - :class:`compasfab.backends.PyBulletClient` is a context manager type, so it's best + :class:`compas_fab.backends.PyBulletClient` is a context manager type, so it's best used in combination with the ``with`` statement to ensure resource deallocation. @@ -199,9 +204,9 @@ def load_ur5(self, load_geometry=False, concavity=False): robot.attributes["pybullet"] = {} if load_geometry: - self.cache_robot(robot, concavity) + self.cache_robot_model(robot, concavity) else: - robot.attributes["pybullet"]["cached_robot"] = robot.model + robot.attributes["pybullet"]["cached_robot_model"] = robot.model robot.attributes["pybullet"]["cached_robot_filepath"] = compas_fab.get( "robot_library/ur5_robot/urdf/robot_description.urdf" ) @@ -213,8 +218,39 @@ def load_ur5(self, load_geometry=False, concavity=False): return robot + def load_existing_robot(self, robot): + # type: (Robot) -> Robot + """Load an existing robot to PyBullet. + The robot must have its geometry and semantics loaded. + + Parameters + ---------- + robot : :class:`compas_fab.robots.Robot` + The robot to be saved for use with PyBullet. + + Returns + ------- + :class:`compas_fab.robots.Robot` + A robot instance. + """ + robot.client = self + robot.attributes["pybullet"] = {} + + robot.ensure_geometry() + robot.ensure_semantics() + self.cache_robot_model(robot) + + urdf_fp = robot.attributes["pybullet"]["cached_robot_filepath"] + self._load_robot_to_pybullet(urdf_fp, robot) + self.disabled_collisions = robot.semantics.disabled_collisions + + return robot + def load_robot(self, urdf_file, resource_loaders=None, concavity=False, precision=None): - """Create a pybullet robot using the input urdf file. + """Create a robot from URDF and load it into PyBullet. + + Robot geometry of the robot can be loaded using the resource loaders. + Robot semantics are loaded separately using the :meth:`load_semantics` method. Parameters ---------- @@ -244,9 +280,9 @@ def load_robot(self, urdf_file, resource_loaders=None, concavity=False, precisio robot.attributes["pybullet"] = {} if resource_loaders: robot_model.load_geometry(*resource_loaders, precision=precision) - self.cache_robot(robot, concavity) + self.cache_robot_model(robot, concavity) else: - robot.attributes["pybullet"]["cached_robot"] = robot.model + robot.attributes["pybullet"]["cached_robot_model"] = robot.model robot.attributes["pybullet"]["cached_robot_filepath"] = urdf_file urdf_fp = robot.attributes["pybullet"]["cached_robot_filepath"] @@ -265,20 +301,20 @@ def load_semantics(self, robot, srdf_filename): srdf_filename : :obj:`str` or file object Absolute file path to the srdf file name. """ - cached_robot = self.get_cached_robot(robot) - robot.semantics = RobotSemantics.from_srdf_file(srdf_filename, cached_robot) + cached_robot_model = self.get_cached_robot_model(robot) + robot.semantics = RobotSemantics.from_srdf_file(srdf_filename, cached_robot_model) self.disabled_collisions = robot.semantics.disabled_collisions def _load_robot_to_pybullet(self, urdf_file, robot): - cached_robot = self.get_cached_robot(robot) + cached_robot_model = self.get_cached_robot_model(robot) with redirect_stdout(enabled=not self.verbose): pybullet_uid = pybullet.loadURDF( urdf_file, useFixedBase=True, physicsClientId=self.client_id, flags=pybullet.URDF_USE_SELF_COLLISION ) - cached_robot.attr["uid"] = pybullet_uid + cached_robot_model.attr["uid"] = pybullet_uid - self._add_ids_to_robot_joints(cached_robot) - self._add_ids_to_robot_links(cached_robot) + self._add_ids_to_robot_joints(cached_robot_model) + self._add_ids_to_robot_links(cached_robot_model) def reload_from_cache(self, robot): """Reloads the PyBullet server with the robot's cached model. @@ -290,7 +326,7 @@ def reload_from_cache(self, robot): """ current_configuration = self.get_robot_configuration(robot) - cached_robot_model = self.get_cached_robot(robot) + cached_robot_model = self.get_cached_robot_model(robot) cached_robot_filepath = self.get_cached_robot_filepath(robot) robot_uid = self.get_uid(cached_robot_model) pybullet.removeBody(robot_uid, physicsClientId=self.client_id) @@ -303,7 +339,7 @@ def reload_from_cache(self, robot): self.set_robot_configuration(robot, current_configuration) self.step_simulation() - def cache_robot(self, robot, concavity=False): + def cache_robot_model(self, robot, concavity=False): """Saves an editable copy of the robot's model and its meshes for shadowing the state of the robot on the PyBullet server. @@ -345,27 +381,28 @@ def cache_robot(self, robot, concavity=False): mesh.attrib["filename"] = address_dict[filename] # write urdf - cached_robot_file_name = str(robot.model.guid) + ".urdf" - cached_robot_filepath = os.path.join(self._cache_dir.name, cached_robot_file_name) + cached_robot_model_file_name = str(robot.model.guid) + ".urdf" + cached_robot_filepath = os.path.join(self._cache_dir.name, cached_robot_model_file_name) urdf.to_file(cached_robot_filepath, prettify=True) - cached_robot = RobotModel.from_urdf_file(cached_robot_filepath) - robot.attributes["pybullet"]["cached_robot"] = cached_robot + cached_robot_model = RobotModel.from_urdf_file(cached_robot_filepath) + robot.attributes["pybullet"]["cached_robot_model"] = cached_robot_model robot.attributes["pybullet"]["cached_robot_filepath"] = cached_robot_filepath robot.attributes["pybullet"]["robot_geometry_cached"] = True @staticmethod - def ensure_cached_robot(robot): + def ensure_cached_robot_model(robot): """Checks if a :class:`compas_fab.robots.Robot` has been cached for use with PyBullet.""" - if not robot.attributes["pybullet"]["cached_robot"]: + if not robot.attributes["pybullet"]["cached_robot_model"]: raise Exception("This method is only callable once the robot has been cached.") @staticmethod - def ensure_cached_robot_geometry(robot): + def ensure_cached_robot_model_geometry(robot): """Checks if the geometry of a :class:`compas_fab.robots.Robot` has been cached for use with PyBullet.""" if not robot.attributes["pybullet"].get("robot_geometry_cached"): raise Exception("This method is only callable once the robot with loaded geometry has been cached.") - def get_cached_robot(self, robot): + def get_cached_robot_model(self, robot): + # type: (Robot) -> RobotModel """Returns the editable copy of the robot's model for shadowing the state of the robot on the PyBullet server. @@ -384,10 +421,11 @@ def get_cached_robot(self, robot): If the robot has not been cached. """ - self.ensure_cached_robot(robot) - return robot.attributes["pybullet"]["cached_robot"] + self.ensure_cached_robot_model(robot) + return robot.attributes["pybullet"]["cached_robot_model"] def get_cached_robot_filepath(self, robot): + # type: (Robot) -> str """Returns the filepath of the editable copy of the robot's model for shadowing the state of the robot on the PyBullet server. @@ -406,16 +444,16 @@ def get_cached_robot_filepath(self, robot): If the robot has not been cached. """ - self.ensure_cached_robot(robot) + self.ensure_cached_robot_model(robot) return robot.attributes["pybullet"]["cached_robot_filepath"] - def get_uid(self, cached_robot): + def get_uid(self, cached_robot_model): """Returns the internal PyBullet id of the robot's model for shadowing the state of the robot on the PyBullet server. Parameters ---------- - cached_robot : :class:`compas_robots.RobotModel` + cached_robot_model : :class:`compas_robots.RobotModel` The robot model saved for use with PyBullet. Returns @@ -423,37 +461,37 @@ def get_uid(self, cached_robot): :obj:`int` """ - return cached_robot.attr["uid"] + return cached_robot_model.attr["uid"] - def _add_ids_to_robot_joints(self, cached_robot): - body_id = self.get_uid(cached_robot) + def _add_ids_to_robot_joints(self, cached_robot_model): + body_id = self.get_uid(cached_robot_model) joint_ids = self._get_joint_ids(body_id) for joint_id in joint_ids: joint_name = self._get_joint_name(joint_id, body_id) - joint = cached_robot.get_joint_by_name(joint_name) + joint = cached_robot_model.get_joint_by_name(joint_name) pybullet_attr = {"id": joint_id} joint.attr.setdefault("pybullet", {}).update(pybullet_attr) - def _add_ids_to_robot_links(self, cached_robot): - body_id = self.get_uid(cached_robot) + def _add_ids_to_robot_links(self, robot_model): + body_id = self.get_uid(robot_model) joint_ids = self._get_joint_ids(body_id) for link_id in joint_ids: link_name = self._get_link_name(link_id, body_id) - link = cached_robot.get_link_by_name(link_name) + link = robot_model.get_link_by_name(link_name) pybullet_attr = {"id": link_id} link.attr.setdefault("pybullet", {}).update(pybullet_attr) - def _get_joint_id_by_name(self, name, cached_robot): - return cached_robot.get_joint_by_name(name).attr["pybullet"]["id"] + def _get_joint_id_by_name(self, name, robot_model): + return robot_model.get_joint_by_name(name).attr["pybullet"]["id"] - def _get_joint_ids_by_name(self, names, cached_robot): - return tuple(self._get_joint_id_by_name(name, cached_robot) for name in names) + def _get_joint_ids_by_name(self, names, robot_model): + return tuple(self._get_joint_id_by_name(name, robot_model) for name in names) - def _get_link_id_by_name(self, name, cached_robot): - return cached_robot.get_link_by_name(name).attr["pybullet"]["id"] + def _get_link_id_by_name(self, name, robot_model): + return robot_model.get_link_by_name(name).attr["pybullet"]["id"] - def _get_link_ids_by_name(self, names, cached_robot): - return tuple(self._get_link_id_by_name(name, cached_robot) for name in names) + def _get_link_ids_by_name(self, names, robot_model): + return tuple(self._get_link_id_by_name(name, robot_model) for name in names) def filter_configurations_in_collision(self, robot, configurations): """Filters from a list of configurations those which are in collision. @@ -495,10 +533,10 @@ def check_collisions(self, robot, configuration=None): ------ :class:`compas_fab.backends.pybullet.DetectedCollision` """ - cached_robot = self.get_cached_robot(robot) - body_id = self.get_uid(cached_robot) + cached_robot_model = self.get_cached_robot_model(robot) + body_id = self.get_uid(cached_robot_model) if configuration: - joint_ids = self._get_joint_ids_by_name(configuration.joint_names, cached_robot) + joint_ids = self._get_joint_ids_by_name(configuration.joint_names, cached_robot_model) self._set_joint_positions(joint_ids, configuration.joint_values, body_id) self.check_collision_with_objects(robot) self.check_robot_self_collision(robot) @@ -518,7 +556,7 @@ def check_collision_with_objects(self, robot): """ for name, body_ids in self.collision_objects.items(): for body_id in body_ids: - self._check_collision(self.get_uid(self.get_cached_robot(robot)), "robot", body_id, name) + self._check_collision(self.get_uid(self.get_cached_robot_model(robot)), "robot", body_id, name) def check_robot_self_collision(self, robot): """Checks whether the robot and its attached collision objects with its current @@ -533,15 +571,15 @@ def check_robot_self_collision(self, robot): ------ :class:`compas_fab.backends.pybullet.DetectedCollision` """ - cached_robot = self.get_cached_robot(robot) - body_id = self.get_uid(cached_robot) - link_names = [link.name for link in cached_robot.iter_links() if link.collision] + cached_robot_model = self.get_cached_robot_model(robot) + body_id = self.get_uid(cached_robot_model) + link_names = [link.name for link in cached_robot_model.iter_links() if link.collision] # check for collisions between robot links for link_1_name, link_2_name in combinations(link_names, 2): if {link_1_name, link_2_name} in self.unordered_disabled_collisions: continue - link_1_id = self._get_link_id_by_name(link_1_name, cached_robot) - link_2_id = self._get_link_id_by_name(link_2_name, cached_robot) + link_1_id = self._get_link_id_by_name(link_1_name, cached_robot_model) + link_2_id = self._get_link_id_by_name(link_2_name, cached_robot_model) self._check_collision(body_id, link_1_name, body_id, link_2_name, link_1_id, link_2_id) def check_collision_objects_for_collision(self): @@ -603,6 +641,7 @@ def _get_num_joints(self, body_id): return pybullet.getNumJoints(body_id, physicsClientId=self.client_id) def _get_joint_ids(self, body_id): + # type: (int) -> list[int] return list(range(self._get_num_joints(body_id))) def _get_joint_name(self, joint_id, body_id): @@ -652,11 +691,11 @@ def set_robot_configuration(self, robot, configuration, group=None): The planning group used for calculation. Defaults to the robot's main planning group. """ - cached_robot = self.get_cached_robot(robot) - body_id = self.get_uid(cached_robot) + cached_robot_model = self.get_cached_robot_model(robot) + body_id = self.get_uid(cached_robot_model) default_config = robot.zero_configuration() full_configuration = robot.merge_group_with_full_configuration(configuration, default_config, group) - joint_ids = self._get_joint_ids_by_name(full_configuration.joint_names, cached_robot) + joint_ids = self._get_joint_ids_by_name(full_configuration.joint_names, cached_robot_model) self._set_joint_positions(joint_ids, full_configuration.joint_values, body_id) return full_configuration @@ -671,10 +710,10 @@ def get_robot_configuration(self, robot): ------- :class:`compas_robots.Configuration` """ - cached_robot = self.get_cached_robot(robot) - body_id = self.get_uid(cached_robot) + cached_robot_model = self.get_cached_robot_model(robot) + body_id = self.get_uid(cached_robot_model) default_config = robot.zero_configuration() - joint_ids = self._get_joint_ids_by_name(default_config.joint_names, cached_robot) + joint_ids = self._get_joint_ids_by_name(default_config.joint_names, cached_robot_model) joint_values = self._get_joint_positions(joint_ids, body_id) default_config.joint_values = joint_values return default_config @@ -827,8 +866,15 @@ def _get_geometry_args(path, concavity=False, scale=1.0): class AnalyticalPyBulletClient(PyBulletClient): - def inverse_kinematics(self, *args, **kwargs): - return AnalyticalInverseKinematics(self)(*args, **kwargs) + """Combination of PyBullet as the client for COllision Detection and Analytical Inverse Kinematics.""" + + def __init__(self, connection_type="gui", verbose=False): + PyBulletClient.__init__(self, connection_type=connection_type, verbose=verbose) + + def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=None, options=None): + planner = AnalyticalInverseKinematics(self) + return planner.inverse_kinematics(robot, frame_WCF, start_configuration, group, options) - def plan_cartesian_motion(self, *args, **kwargs): - return AnalyticalPlanCartesianMotion(self)(*args, **kwargs) + def plan_cartesian_motion(self, robot, waypoints, start_configuration=None, group=None, options=None): + planner = AnalyticalPlanCartesianMotion(self) + return planner.plan_cartesian_motion(robot, waypoints, start_configuration, group, options) diff --git a/src/compas_fab/backends/pybullet/planner.py b/src/compas_fab/backends/pybullet/planner.py index 1cab15b67a..cd45bd4560 100644 --- a/src/compas_fab/backends/pybullet/planner.py +++ b/src/compas_fab/backends/pybullet/planner.py @@ -3,7 +3,8 @@ from __future__ import print_function from compas_fab.backends.interfaces.client import PlannerInterface -from compas_fab.backends.interfaces.client import forward_docstring + +# from compas_fab.backends.interfaces.client import forward_docstring from compas_fab.backends.pybullet.backend_features.pybullet_add_attached_collision_mesh import ( PyBulletAddAttachedCollisionMesh, ) @@ -21,36 +22,17 @@ ] -class PyBulletPlanner(PlannerInterface): +class PyBulletPlanner( + PyBulletAddAttachedCollisionMesh, + PyBulletAddCollisionMesh, + PyBulletAppendCollisionMesh, + PyBulletRemoveCollisionMesh, + PyBulletRemoveAttachedCollisionMesh, + PyBulletForwardKinematics, + PyBulletInverseKinematics, + PlannerInterface, +): """Implement the planner backend interface for PyBullet.""" def __init__(self, client): - super(PyBulletPlanner, self).__init__(client) - - @forward_docstring(PyBulletAddAttachedCollisionMesh) - def add_attached_collision_mesh(self, *args, **kwargs): - return PyBulletAddAttachedCollisionMesh(self.client)(*args, **kwargs) - - @forward_docstring(PyBulletAddCollisionMesh) - def add_collision_mesh(self, *args, **kwargs): - return PyBulletAddCollisionMesh(self.client)(*args, **kwargs) - - @forward_docstring(PyBulletAppendCollisionMesh) - def append_collision_mesh(self, *args, **kwargs): - return PyBulletAppendCollisionMesh(self.client)(*args, **kwargs) - - @forward_docstring(PyBulletRemoveCollisionMesh) - def remove_collision_mesh(self, *args, **kwargs): - return PyBulletRemoveCollisionMesh(self.client)(*args, **kwargs) - - @forward_docstring(PyBulletRemoveAttachedCollisionMesh) - def remove_attached_collision_mesh(self, *args, **kwargs): - return PyBulletRemoveAttachedCollisionMesh(self.client)(*args, **kwargs) - - @forward_docstring(PyBulletForwardKinematics) - def forward_kinematics(self, *args, **kwargs): - return PyBulletForwardKinematics(self.client)(*args, **kwargs) - - @forward_docstring(PyBulletInverseKinematics) - def inverse_kinematics(self, *args, **kwargs): - return PyBulletInverseKinematics(self.client)(*args, **kwargs) + self.client = client diff --git a/src/compas_fab/backends/ros/backend_features/__init__.py b/src/compas_fab/backends/ros/backend_features/__init__.py index f9ea0a3012..40d5695115 100644 --- a/src/compas_fab/backends/ros/backend_features/__init__.py +++ b/src/compas_fab/backends/ros/backend_features/__init__.py @@ -1,3 +1,25 @@ +""" +ROS backend features +==================== + +.. autosummary:: + :toctree: generated/ + :nosignatures: + + MoveItAddAttachedCollisionMesh + MoveItAddCollisionMesh + MoveItAppendCollisionMesh + MoveItForwardKinematics + MoveItInverseKinematics + MoveItPlanCartesianMotion + MoveItPlanMotion + MoveItPlanningScene + MoveItRemoveAttachedCollisionMesh + MoveItRemoveCollisionMesh + MoveItResetPlanningScene + +""" + from __future__ import absolute_import from compas_fab.backends.ros.backend_features.move_it_add_attached_collision_mesh import MoveItAddAttachedCollisionMesh diff --git a/src/compas_fab/backends/ros/backend_features/move_it_add_attached_collision_mesh.py b/src/compas_fab/backends/ros/backend_features/move_it_add_attached_collision_mesh.py index f1dc9d9e69..d7f4b94a46 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_add_attached_collision_mesh.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_add_attached_collision_mesh.py @@ -28,9 +28,6 @@ class MoveItAddAttachedCollisionMesh(AddAttachedCollisionMesh): ApplyPlanningSceneResponse, ) - def __init__(self, ros_client): - self.ros_client = ros_client - def add_attached_collision_mesh(self, attached_collision_mesh, options=None): """Add a collision mesh and attach it to the robot. @@ -56,5 +53,5 @@ def add_attached_collision_mesh_async(self, callback, errback, attached_collisio aco.object.operation = CollisionObject.ADD robot_state = RobotState(attached_collision_objects=[aco], is_diff=True) scene = PlanningScene(robot_state=robot_state, is_diff=True) - request = scene.to_request(self.ros_client.ros_distro) - self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback) + request = scene.to_request(self.client.ros_distro) + self.APPLY_PLANNING_SCENE(self.client, request, callback, errback) diff --git a/src/compas_fab/backends/ros/backend_features/move_it_add_collision_mesh.py b/src/compas_fab/backends/ros/backend_features/move_it_add_collision_mesh.py index a33c9f5980..1b3018ffc0 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_add_collision_mesh.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_add_collision_mesh.py @@ -27,9 +27,6 @@ class MoveItAddCollisionMesh(AddCollisionMesh): ApplyPlanningSceneResponse, ) - def __init__(self, ros_client): - self.ros_client = ros_client - def add_collision_mesh(self, collision_mesh, options=None): """Add a collision mesh to the planning scene. @@ -55,5 +52,5 @@ def add_collision_mesh_async(self, callback, errback, collision_mesh): co.operation = CollisionObject.ADD world = PlanningSceneWorld(collision_objects=[co]) scene = PlanningScene(world=world, is_diff=True) - request = scene.to_request(self.ros_client.ros_distro) - self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback) + request = scene.to_request(self.client.ros_distro) + self.APPLY_PLANNING_SCENE(self.client, request, callback, errback) diff --git a/src/compas_fab/backends/ros/backend_features/move_it_append_collision_mesh.py b/src/compas_fab/backends/ros/backend_features/move_it_append_collision_mesh.py index e483f72abe..43c46fb33c 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_append_collision_mesh.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_append_collision_mesh.py @@ -27,9 +27,6 @@ class MoveItAppendCollisionMesh(AppendCollisionMesh): ApplyPlanningSceneResponse, ) - def __init__(self, ros_client): - self.ros_client = ros_client - def append_collision_mesh(self, collision_mesh, options=None): """Append a collision mesh to the planning scene. @@ -55,5 +52,5 @@ def append_collision_mesh_async(self, callback, errback, collision_mesh): co.operation = CollisionObject.APPEND world = PlanningSceneWorld(collision_objects=[co]) scene = PlanningScene(world=world, is_diff=True) - request = scene.to_request(self.ros_client.ros_distro) - self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback) + request = scene.to_request(self.client.ros_distro) + self.APPLY_PLANNING_SCENE(self.client, request, callback, errback) diff --git a/src/compas_fab/backends/ros/backend_features/move_it_forward_kinematics.py b/src/compas_fab/backends/ros/backend_features/move_it_forward_kinematics.py index 14b9918646..cbcd0d68e4 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_forward_kinematics.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_forward_kinematics.py @@ -26,9 +26,6 @@ class MoveItForwardKinematics(ForwardKinematics): "/compute_fk", "GetPositionFK", GetPositionFKRequest, GetPositionFKResponse, validate_response ) - def __init__(self, ros_client): - self.ros_client = ros_client - def forward_kinematics(self, robot, configuration, group=None, options=None): """Calculate the robot's forward kinematic. @@ -83,9 +80,9 @@ def forward_kinematics_async(self, callback, errback, configuration, options): header = Header(frame_id=base_link) joint_state = JointState(name=configuration.joint_names, position=configuration.joint_values, header=header) robot_state = RobotState(joint_state, MultiDOFJointState(header=header)) - robot_state.filter_fields_for_distro(self.ros_client.ros_distro) + robot_state.filter_fields_for_distro(self.client.ros_distro) def convert_to_frame(response): callback(response.pose_stamped[0].pose.frame) - self.GET_POSITION_FK(self.ros_client, (header, fk_link_names, robot_state), convert_to_frame, errback) + self.GET_POSITION_FK(self.client, (header, fk_link_names, robot_state), convert_to_frame, errback) 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 35438b6651..e865fa8d8d 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 @@ -32,9 +32,6 @@ class MoveItInverseKinematics(InverseKinematics): "/compute_ik", "GetPositionIK", GetPositionIKRequest, GetPositionIKResponse, validate_response ) - def __init__(self, ros_client): - self.ros_client = ros_client - def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=None, options=None): """Calculate the robot's inverse kinematic for a given frame. @@ -115,7 +112,7 @@ def inverse_kinematics_async( start_state.attached_collision_objects.append(aco) # Filter needs to happen after all objects have been added - start_state.filter_fields_for_distro(self.ros_client.ros_distro) + start_state.filter_fields_for_distro(self.client.ros_distro) constraints = convert_constraints_to_rosmsg(options.get("constraints"), header) @@ -135,10 +132,10 @@ def inverse_kinematics_async( # The field `attempts` was removed in Noetic (and higher) # so it needs to be removed from the message otherwise it causes a serialization error # https://github.com/ros-planning/moveit/pull/1288 - if self.ros_client.ros_distro not in (RosDistro.KINETIC, RosDistro.MELODIC): + if self.client.ros_distro not in (RosDistro.KINETIC, RosDistro.MELODIC): del ik_request.attempts def convert_to_positions(response): callback((response.solution.joint_state.position, response.solution.joint_state.name)) - self.GET_POSITION_IK(self.ros_client, (ik_request,), convert_to_positions, errback) + self.GET_POSITION_IK(self.client, (ik_request,), convert_to_positions, errback) diff --git a/src/compas_fab/backends/ros/backend_features/move_it_plan_cartesian_motion.py b/src/compas_fab/backends/ros/backend_features/move_it_plan_cartesian_motion.py index 70a017fb0d..cc2c5a426e 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_plan_cartesian_motion.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_plan_cartesian_motion.py @@ -18,6 +18,9 @@ from compas_fab.backends.ros.messages import RobotState from compas_fab.backends.ros.service_description import ServiceDescription +from compas_fab.robots import FrameWaypoints +from compas_fab.robots import PointAxisWaypoints + __all__ = [ "MoveItPlanCartesianMotion", ] @@ -34,18 +37,15 @@ class MoveItPlanCartesianMotion(PlanCartesianMotion): validate_response, ) - def __init__(self, ros_client): - self.ros_client = ros_client - - def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, group=None, options=None): + def plan_cartesian_motion(self, robot, waypoints, start_configuration=None, group=None, options=None): """Calculates a cartesian motion path (linear in tool space). Parameters ---------- robot : :class:`compas_fab.robots.Robot` The robot instance for which the cartesian motion plan is being calculated. - frames_WCF: list of :class:`compas.geometry.Frame` - The frames through which the path is defined. + waypoints : :class:`compas_fab.robots.Waypoints` + The waypoints for the robot to follow. start_configuration: :class:`compas_robots.Configuration`, optional The robot's full configuration, i.e. values for all configurable joints of the entire robot, at the starting position. Defaults to @@ -85,7 +85,7 @@ def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, gro options = options or {} kwargs = {} kwargs["options"] = options - kwargs["frames_WCF"] = frames_WCF + kwargs["waypoints"] = waypoints kwargs["start_configuration"] = start_configuration kwargs["group"] = group @@ -99,16 +99,30 @@ def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, gro if options["link"] not in robot.get_link_names(group): raise ValueError("Link name {} does not exist in planning group".format(options["link"])) - return await_callback(self.plan_cartesian_motion_async, **kwargs) + # This function wraps multiple implementations depending on the type of waypoints + if isinstance(waypoints, FrameWaypoints): + return await_callback(self.plan_cartesian_motion_with_frame_waypoints_async, **kwargs) + elif isinstance(waypoints, PointAxisWaypoints): + return self.plan_cartesian_motion_with_point_axis_waypoints_async(**kwargs) + else: + raise TypeError("Unsupported waypoints type {} for MoveIt planning backend.".format(type(waypoints))) - def plan_cartesian_motion_async( - self, callback, errback, frames_WCF, start_configuration=None, group=None, options=None + def plan_cartesian_motion_with_frame_waypoints_async( + self, callback, errback, waypoints, start_configuration=None, group=None, options=None ): - """Asynchronous handler of MoveIt cartesian motion planner service.""" + """Asynchronous handler of MoveIt cartesian motion planner service. + + :class:`compas_fab.robots.FrameWaypoints` are converted to :class:`compas_fab.backends.ros.messages.Pose` that is native to ROS communication + + """ + joints = options["joints"] header = Header(frame_id=options["base_link"]) - waypoints = [Pose.from_frame(frame) for frame in frames_WCF] + + # Convert compas_fab.robots.FrameWaypoints to list of Pose for ROS + list_of_pose = [Pose.from_frame(frame) for frame in waypoints.target_frames] + joint_state = JointState( header=header, name=start_configuration.joint_names, position=start_configuration.joint_values ) @@ -120,7 +134,7 @@ def plan_cartesian_motion_async( start_state.attached_collision_objects.append(aco) # Filter needs to happen after all objects have been added - start_state.filter_fields_for_distro(self.ros_client.ros_distro) + start_state.filter_fields_for_distro(self.client.ros_distro) path_constraints = convert_constraints_to_rosmsg(options.get("path_constraints"), header) @@ -129,7 +143,7 @@ def plan_cartesian_motion_async( start_state=start_state, group_name=group, link_name=options["link"], - waypoints=waypoints, + waypoints=list_of_pose, max_step=float(options.get("max_step", 0.01)), jump_threshold=float(options.get("jump_threshold", 1.57)), avoid_collisions=bool(options.get("avoid_collisions", True)), @@ -145,4 +159,13 @@ def response_handler(response): except Exception as e: errback(e) - self.GET_CARTESIAN_PATH(self.ros_client, request, response_handler, errback) + self.GET_CARTESIAN_PATH(self.client, request, response_handler, errback) + + def plan_cartesian_motion_with_point_axis_waypoints_async( + self, callback, errback, waypoints, start_configuration=None, group=None, options=None + ): + """Asynchronous handler of MoveIt cartesian motion planner service. + + AFAIK MoveIt does not support planning for a relaxed axis under this + """ + raise NotImplementedError("PointAxisWaypoints are not supported by MoveIt backend") diff --git a/src/compas_fab/backends/ros/backend_features/move_it_plan_motion.py b/src/compas_fab/backends/ros/backend_features/move_it_plan_motion.py index e4a5ea3b61..f07141ea25 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_plan_motion.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_plan_motion.py @@ -29,9 +29,6 @@ class MoveItPlanMotion(PlanMotion): "/plan_kinematic_path", "GetMotionPlan", MotionPlanRequest, MotionPlanResponse, validate_response ) - def __init__(self, ros_client): - self.ros_client = ros_client - def plan_motion(self, robot, target, start_configuration=None, group=None, options=None): """Calculates a motion path. @@ -113,7 +110,7 @@ def plan_motion_async(self, callback, errback, target, start_configuration=None, start_state.attached_collision_objects.append(aco) # Filter needs to happen after all objects have been added - start_state.filter_fields_for_distro(self.ros_client.ros_distro) + start_state.filter_fields_for_distro(self.client.ros_distro) # convert constraints ee_link_name = options["ee_link_name"] @@ -150,4 +147,4 @@ def response_handler(response): except Exception as e: errback(e) - self.GET_MOTION_PLAN(self.ros_client, request, response_handler, errback) + self.GET_MOTION_PLAN(self.client, request, response_handler, errback) diff --git a/src/compas_fab/backends/ros/backend_features/move_it_planning_scene.py b/src/compas_fab/backends/ros/backend_features/move_it_planning_scene.py index 95090dfb93..c931d02b19 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_planning_scene.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_planning_scene.py @@ -22,9 +22,6 @@ class MoveItPlanningScene(GetPlanningScene): "/get_planning_scene", "GetPlanningScene", GetPlanningSceneRequest, GetPlanningSceneResponse ) - def __init__(self, ros_client): - self.ros_client = ros_client - def get_planning_scene(self, options=None): """Retrieve the planning scene. @@ -54,4 +51,4 @@ def get_planning_scene_async(self, callback, errback): | PlanningSceneComponents.OBJECT_COLORS ) ) - self.GET_PLANNING_SCENE(self.ros_client, request, callback, errback) + self.GET_PLANNING_SCENE(self.client, request, callback, errback) diff --git a/src/compas_fab/backends/ros/backend_features/move_it_remove_attached_collision_mesh.py b/src/compas_fab/backends/ros/backend_features/move_it_remove_attached_collision_mesh.py index 2c5e234ed4..fe6b2bb710 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_remove_attached_collision_mesh.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_remove_attached_collision_mesh.py @@ -28,9 +28,6 @@ class MoveItRemoveAttachedCollisionMesh(RemoveAttachedCollisionMesh): ApplyPlanningSceneResponse, ) - def __init__(self, ros_client): - self.ros_client = ros_client - def remove_attached_collision_mesh(self, id, options=None): """Remove an attached collision mesh from the robot. @@ -57,5 +54,5 @@ def remove_attached_collision_mesh_async(self, callback, errback, id): aco.object.operation = CollisionObject.REMOVE robot_state = RobotState(attached_collision_objects=[aco], is_diff=True) scene = PlanningScene(robot_state=robot_state, is_diff=True) - request = scene.to_request(self.ros_client.ros_distro) - self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback) + request = scene.to_request(self.client.ros_distro) + self.APPLY_PLANNING_SCENE(self.client, request, callback, errback) diff --git a/src/compas_fab/backends/ros/backend_features/move_it_remove_collision_mesh.py b/src/compas_fab/backends/ros/backend_features/move_it_remove_collision_mesh.py index cf284ff103..46d965340d 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_remove_collision_mesh.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_remove_collision_mesh.py @@ -27,9 +27,6 @@ class MoveItRemoveCollisionMesh(RemoveCollisionMesh): ApplyPlanningSceneResponse, ) - def __init__(self, ros_client): - self.ros_client = ros_client - def remove_collision_mesh(self, id, options=None): """Remove a collision mesh from the planning scene. @@ -56,5 +53,5 @@ def remove_collision_mesh_async(self, callback, errback, id): co.operation = CollisionObject.REMOVE world = PlanningSceneWorld(collision_objects=[co]) scene = PlanningScene(world=world, is_diff=True) - request = scene.to_request(self.ros_client.ros_distro) - self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback) + request = scene.to_request(self.client.ros_distro) + self.APPLY_PLANNING_SCENE(self.client, request, callback, errback) diff --git a/src/compas_fab/backends/ros/backend_features/move_it_reset_planning_scene.py b/src/compas_fab/backends/ros/backend_features/move_it_reset_planning_scene.py index e26bcdedb1..5c1a7f7750 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_reset_planning_scene.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_reset_planning_scene.py @@ -25,9 +25,6 @@ class MoveItResetPlanningScene(ResetPlanningScene): ApplyPlanningSceneResponse, ) - def __init__(self, ros_client): - self.ros_client = ros_client - def reset_planning_scene(self, options=None): """Resets the planning scene, removing all added collision meshes. @@ -46,12 +43,12 @@ def reset_planning_scene(self, options=None): return await_callback(self.reset_planning_scene_async, **kwargs) def reset_planning_scene_async(self, callback, errback): - scene = self.ros_client.get_planning_scene() + scene = self.client.get_planning_scene() for collision_object in scene.world.collision_objects: collision_object.operation = CollisionObject.REMOVE for collision_object in scene.robot_state.attached_collision_objects: collision_object.object["operation"] = CollisionObject.REMOVE scene.is_diff = True scene.robot_state.is_diff = True - request = scene.to_request(self.ros_client.ros_distro) - self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback) + request = scene.to_request(self.client.ros_distro) + self.APPLY_PLANNING_SCENE(self.client, request, callback, errback) diff --git a/src/compas_fab/backends/ros/planner.py b/src/compas_fab/backends/ros/planner.py index 2742a83fad..4506431845 100644 --- a/src/compas_fab/backends/ros/planner.py +++ b/src/compas_fab/backends/ros/planner.py @@ -1,5 +1,6 @@ """ Internal implementation of the planner backend interface for MoveIt! + """ from __future__ import absolute_import @@ -7,7 +8,8 @@ from __future__ import print_function from compas_fab.backends.interfaces.client import PlannerInterface -from compas_fab.backends.interfaces.client import forward_docstring + +# from compas_fab.backends.interfaces.client import forward_docstring from compas_fab.backends.ros.backend_features import MoveItResetPlanningScene from compas_fab.backends.ros.backend_features.move_it_add_attached_collision_mesh import MoveItAddAttachedCollisionMesh from compas_fab.backends.ros.backend_features.move_it_add_collision_mesh import MoveItAddCollisionMesh @@ -27,52 +29,21 @@ ] -class MoveItPlanner(PlannerInterface): +class MoveItPlanner( + MoveItForwardKinematics, + MoveItInverseKinematics, + MoveItPlanMotion, + MoveItPlanCartesianMotion, + MoveItPlanningScene, + MoveItResetPlanningScene, + MoveItAddCollisionMesh, + MoveItRemoveCollisionMesh, + MoveItAppendCollisionMesh, + MoveItAddAttachedCollisionMesh, + MoveItRemoveAttachedCollisionMesh, + PlannerInterface, +): """Implement the planner backend interface based on MoveIt!""" def __init__(self, client): - super(MoveItPlanner, self).__init__(client) - - @forward_docstring(MoveItForwardKinematics) - def forward_kinematics(self, *args, **kwargs): - return MoveItForwardKinematics(self.client)(*args, **kwargs) - - @forward_docstring(MoveItInverseKinematics) - def inverse_kinematics(self, *args, **kwargs): - return MoveItInverseKinematics(self.client)(*args, **kwargs) - - @forward_docstring(MoveItPlanCartesianMotion) - def plan_cartesian_motion(self, *args, **kwargs): - return MoveItPlanCartesianMotion(self.client)(*args, **kwargs) - - @forward_docstring(MoveItPlanMotion) - def plan_motion(self, *args, **kwargs): - return MoveItPlanMotion(self.client)(*args, **kwargs) - - @forward_docstring(MoveItPlanningScene) - def get_planning_scene(self, *args, **kwargs): - return MoveItPlanningScene(self.client)(*args, **kwargs) - - @forward_docstring(MoveItResetPlanningScene) - def reset_planning_scene(self, *args, **kwargs): - return MoveItResetPlanningScene(self.client)(*args, **kwargs) - - @forward_docstring(MoveItAddCollisionMesh) - def add_collision_mesh(self, *args, **kwargs): - return MoveItAddCollisionMesh(self.client)(*args, **kwargs) - - @forward_docstring(MoveItRemoveCollisionMesh) - def remove_collision_mesh(self, *args, **kwargs): - return MoveItRemoveCollisionMesh(self.client)(*args, **kwargs) - - @forward_docstring(MoveItAppendCollisionMesh) - def append_collision_mesh(self, *args, **kwargs): - return MoveItAppendCollisionMesh(self.client)(*args, **kwargs) - - @forward_docstring(MoveItAddAttachedCollisionMesh) - def add_attached_collision_mesh(self, *args, **kwargs): - return MoveItAddAttachedCollisionMesh(self.client)(*args, **kwargs) - - @forward_docstring(MoveItRemoveAttachedCollisionMesh) - def remove_attached_collision_mesh(self, *args, **kwargs): - return MoveItRemoveAttachedCollisionMesh(self.client)(*args, **kwargs) + self.client = client diff --git a/src/compas_fab/ghpython/components/Cf_PlanCartesianMotion/code.py b/src/compas_fab/ghpython/components/Cf_PlanCartesianMotion/code.py index 305f4c3820..2d0c8811ff 100644 --- a/src/compas_fab/ghpython/components/Cf_PlanCartesianMotion/code.py +++ b/src/compas_fab/ghpython/components/Cf_PlanCartesianMotion/code.py @@ -9,6 +9,7 @@ from scriptcontext import sticky as st from compas_fab.ghpython.components import create_id +from compas_fab.robots import FrameWaypoints class PlanCartesianMotion(component): @@ -23,8 +24,9 @@ def RunScript( if robot and robot.client and robot.client.is_connected and start_configuration and planes and compute: frames = [plane_to_compas_frame(plane) for plane in planes] + frame_waypoints = FrameWaypoints(frames) st[key] = robot.plan_cartesian_motion( - frames, + frame_waypoints, start_configuration=start_configuration, group=group, options=dict( diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index 8cc68350fd..c6fac3ad0b 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -13,6 +13,7 @@ from compas_fab.robots.constraints import Constraint + __all__ = [ "Robot", ] @@ -392,6 +393,9 @@ def get_link_names(self, group=None): def get_link_names_with_collision_geometry(self): """Get the names of the links with collision geometry. + Note that returned names does not imply that the link has collision geometry loaded. + Use :meth:`ensure_geometry()` to ensure that collision geometry is loaded. + Returns ------- :obj:`list` of :obj:`str` @@ -1313,25 +1317,23 @@ def forward_kinematics(self, configuration, group=None, use_attached_tool_frame= return frame_WCF - def plan_cartesian_motion( - self, frames_WCF, start_configuration=None, group=None, use_attached_tool_frame=True, options=None - ): + def plan_cartesian_motion(self, waypoints, start_configuration=None, group=None, options=None): """Calculate a cartesian motion path (linear in tool space). Parameters ---------- - frames_WCF : :obj:`list` of :class:`compas.geometry.Frame` - The frames through which the path is defined. + waypoints : :class:`compas_fab.robots.Waypoints` + The waypoints for the robot to follow. + For more information on how to define waypoints, see :ref:`waypoints`. + In addition, note that not all planning backends support all waypoint types, + check documentation of the backend in use for more details. start_configuration : :class:`compas_robots.Configuration`, optional The robot's full configuration, i.e. values for all configurable - joints of the entire robot, at the starting position. Defaults to - the all-zero configuration. + joints of the entire robot, at the start of the motion. + Defaults to the all-zero configuration. group : :obj:`str`, optional - The planning group used for 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, it will use its TCF - instead of the T0CF to calculate cartesian paths. Defaults to ``True``. + The name of the planning group used for motion planning. + Defaults to the robot's main planning group. options : :obj:`dict`, optional Dictionary containing the following key-value pairs: @@ -1360,42 +1362,52 @@ def plan_cartesian_motion( >>> with RosClient() as client: # doctest: +SKIP #: This doctest can pass locally but persistently fails on CI in GitHub. "roslibpy.core.RosTimeoutError: Failed to connect to ROS" ... robot = client.load_robot() - ... frames = [Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]),\ + ... target_frames = [Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]),\ Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0])] + ... waypoints = FrameWaypoints(target_frames) ... start_configuration = Configuration.from_revolute_values([-0.042, 0.033, -2.174, 5.282, -1.528, 0.000]) ... group = robot.main_group_name ... options = {'max_step': 0.01,\ 'jump_threshold': 1.57,\ 'avoid_collisions': True} - ... trajectory = robot.plan_cartesian_motion(frames,\ + ... trajectory = robot.plan_cartesian_motion(waypoints,\ start_configuration,\ group=group,\ options=options) ... len(trajectory.points) > 1 True """ + # The plan_cartesian_motion method in the Robot class is a wrapper around planing backend's + # plan_cartesian_motion method. This method is responsible for scaling the waypoints, start_configuration and the planned trajectory. + # Some options may also need scaling, like max_step. This should be removed in the future. + # TODO: Discuss if we can have all options passed with a fixed unit to avoid scaling. + # Attached tools are also managed by this function. + options = options or {} - max_step = options.get("max_step") - path_constraints = options.get("path_constraints") - attached_collision_meshes = options.get("attached_collision_meshes") or [] self.ensure_client() if not group: group = self.main_group_name # ensure semantics + # ======= + # Scaling + # ======= + need_scaling = self.scale_factor != 1.0 + + if need_scaling: + waypoints = waypoints.scaled(1.0 / self.scale_factor) + + max_step = options.get("max_step") + if need_scaling and max_step: + options["max_step"] = max_step / self.scale_factor + # NOTE: start_configuration has to be a full robot configuration, such # that all configurable joints of the whole robot are defined for planning. start_configuration, start_configuration_scaled = self._check_full_configuration_and_scale(start_configuration) - attached_tool = self.attached_tools.get(group) - if use_attached_tool_frame and attached_tool: - frames_WCF = self.from_tcf_to_t0cf(frames_WCF, group) - - frames_WCF_scaled = [] - for frame in frames_WCF: - frames_WCF_scaled.append(Frame(frame.point * 1.0 / self.scale_factor, frame.xaxis, frame.yaxis)) - - if path_constraints: + # Path constraints are only relevant to ROS Backend + path_constraints = options.get("path_constraints") + if need_scaling and path_constraints: path_constraints_WCF_scaled = [] for c in path_constraints: cp = c.copy() @@ -1406,26 +1418,36 @@ def plan_cartesian_motion( else: cp.scale(1.0 / self.scale_factor) path_constraints_WCF_scaled.append(cp) - else: - path_constraints_WCF_scaled = None + options["path_constraints"] = path_constraints_WCF_scaled + # ===================== + # Attached CM and Tools + # ===================== + + attached_collision_meshes = options.get("attached_collision_meshes") or [] + # Collect attached collision meshes from attached tools + # TODO: Discuss why scaling is not happening here? for _, tool in self.attached_tools.items(): if tool: attached_collision_meshes.extend(tool.attached_collision_meshes) - options["attached_collision_meshes"] = attached_collision_meshes - options["path_constraints"] = path_constraints - if max_step: - options["max_step"] = max_step / self.scale_factor + + # ======== + # Planning + # ======== trajectory = self.client.plan_cartesian_motion( robot=self, - frames_WCF=frames_WCF_scaled, + waypoints=waypoints, start_configuration=start_configuration_scaled, group=group, options=options, ) + # ========================= + # Scaling result trajectory + # ========================= + # Scale everything back to robot's scale for pt in trajectory.points: pt.scale(self.scale_factor) diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index 64ff1b3c6b..fc08393eea 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -22,9 +22,9 @@ class Target(Data): pose, configuration, and joint constraints. Dynamic targets such as velocity, acceleration, and jerk are not yet supported. - Targets are intended to be used as arguments for the Backend's motion - planning methods. Different backends might support different types of - targets. + Targets are intended to be used for motion planning with a planning backend + by using :meth:`compas_fab.robot.plan_motion`. + Note that different backends support different types of targets. Attributes ---------- @@ -490,14 +490,16 @@ class Waypoints(Data): Waypoints are useful for tasks like painting, welding, or 3D printing, where the programmer wants to define the waypoints the robot should pass through. - Waypoints are intended to be used for motion planning with a planning backend by using :meth:`compas_fab.robot.plan_motion_with_waypoints`. - Different backends might support different types of waypoints. + Waypoints are intended to be used for motion planning with a planning backend by using :meth:`compas_fab.robot.plan_cartesian_motion`. + Note that different backends support different types of waypoints. The method of interpolation between the waypoints is controlled by the motion planner backend. Attributes ---------- name : str , optional, default = 'target' A human-readable name for identifying the target. + tool_coordinate_frame : :class:`compas.geometry.Frame` or :class:`compas.geometry.Transformation`, optional + The tool tip coordinate frame relative to the flange of the robot. See Also -------- @@ -505,13 +507,13 @@ class Waypoints(Data): :class:`FrameWaypoints` """ - def __init__(self, name="Generic Waypoints"): + def __init__(self, tool_coordinate_frame, name="Generic Waypoints"): super(Waypoints, self).__init__() self.name = name - - @property - def __data__(self): - raise NotImplementedError + # If the user provides a transformation, convert it to a Frame + if isinstance(tool_coordinate_frame, Transformation): + tool_coordinate_frame = Frame.from_transformation(tool_coordinate_frame) + self.tool_coordinate_frame = tool_coordinate_frame def scaled(self, factor): """Returns a scaled copy of the waypoints. @@ -568,13 +570,10 @@ def __init__( tool_coordinate_frame=None, name="Frame Waypoints", ): - super(FrameWaypoints, self).__init__(name=name) + super(FrameWaypoints, self).__init__(tool_coordinate_frame=tool_coordinate_frame, name=name) self.target_frames = target_frames self.tolerance_position = tolerance_position self.tolerance_orientation = tolerance_orientation - if isinstance(tool_coordinate_frame, Transformation): - tool_coordinate_frame = Frame.from_transformation(tool_coordinate_frame) - self.tool_coordinate_frame = tool_coordinate_frame @property def __data__(self): @@ -668,7 +667,7 @@ class PointAxisWaypoints(Waypoints): tolerance_position : float, optional The tolerance for the position of the target point. If not specified, the default value from the planner is used. - tool_coordinate_frame : :class:`compas.geometry.Frame`, optional + tool_coordinate_frame : :class:`compas.geometry.Frame` or :class:`compas.geometry.Transformation`, optional The tool tip coordinate frame relative to the flange coordinate frame of the robot. If not specified, the target point is relative to the robot's flange (T0CF) and the Z axis of the flange can rotate around the target axis. @@ -685,10 +684,9 @@ def __init__( tool_coordinate_frame=None, name="Point-Axis Waypoints", ): - super(PointAxisWaypoints, self).__init__(name=name) + super(PointAxisWaypoints, self).__init__(tool_coordinate_frame=tool_coordinate_frame, name=name) self.target_points_and_axes = target_points_and_axes self.tolerance_position = tolerance_position - self.tool_coordinate_frame = tool_coordinate_frame @property def __data__(self): diff --git a/tests/backends/kinematics/test_inverse_kinematics.py b/tests/backends/kinematics/test_inverse_kinematics.py index abfee5b2a3..c3e27fc18a 100644 --- a/tests/backends/kinematics/test_inverse_kinematics.py +++ b/tests/backends/kinematics/test_inverse_kinematics.py @@ -1,3 +1,5 @@ +import pytest + import compas from compas.geometry import Frame from compas_robots import Configuration @@ -6,6 +8,7 @@ from compas_fab.backends import AnalyticalInverseKinematics from compas_fab.robots import Tool from compas_fab.robots import RobotLibrary +from compas_fab.robots import FrameWaypoints if not compas.IPY: from compas_fab.backends import AnalyticalPyBulletClient @@ -95,10 +98,10 @@ def test_kinematics_client_with_attached_tool(): assert len(solutions) == 6 -def test_kinematics_cartesian(): - if compas.IPY: - return - frames_WCF = [ +@pytest.fixture +def frames_WCF(): + """A list of frames in the world coordinate frame for planning tests""" + return [ Frame((0.407, 0.073, 0.320), (0.922, 0.000, 0.388), (0.113, 0.956, -0.269)), Frame((0.404, 0.057, 0.324), (0.919, 0.000, 0.394), (0.090, 0.974, -0.210)), Frame((0.390, 0.064, 0.315), (0.891, 0.000, 0.454), (0.116, 0.967, -0.228)), @@ -106,11 +109,47 @@ def test_kinematics_cartesian(): Frame((0.376, 0.087, 0.299), (0.850, 0.000, 0.528), (0.184, 0.937, -0.296)), ] + +@pytest.fixture +def frame_waypoints(frames_WCF): + """A FrameWaypoints Object for planning tests""" + return FrameWaypoints(frames_WCF) + + +def test_kinematics_cartesian(frame_waypoints): + if compas.IPY: + return + + with AnalyticalPyBulletClient(connection_type="direct") as client: + robot = client.load_robot(urdf_filename) + client.load_semantics(robot, srdf_filename) + + options = {"solver": "ur5", "check_collision": True} + + trajectory = robot.plan_cartesian_motion(frame_waypoints, options=options) + + # Assert that the trajectory is complete + assert trajectory.fraction == 1.0 + # Assert that the trajectory has the correct number of points + assert len(trajectory.points) == len(frame_waypoints.target_frames) + + +def test_kinematics_cartesian_with_tool_coordinate_frame(frame_waypoints): + if compas.IPY: + return + frame_waypoints.tool_coordinate_frame = Frame([0.01, 0.02, -0.03], [1, 0, 0], [0, 1, 0]) + with AnalyticalPyBulletClient(connection_type="direct") as client: robot = client.load_robot(urdf_filename) client.load_semantics(robot, srdf_filename) options = {"solver": "ur5", "check_collision": True} - start_configuration = list(robot.iter_inverse_kinematics(frames_WCF[0], options=options))[-1] - trajectory = robot.plan_cartesian_motion(frames_WCF, start_configuration=start_configuration, options=options) + + trajectory = robot.plan_cartesian_motion(frame_waypoints, options=options) + + # Assert that the trajectory is complete assert trajectory.fraction == 1.0 + # Assert that the trajectory has the correct number of points + # NOTE: At the moment the AnalyticalPyBulletClient does not perform any interpolation between frames + # NOTE: if future implementation fixes this, the following test will not be valid anymore + assert len(trajectory.points) == len(frame_waypoints.target_frames) diff --git a/tests/backends/pybullet/test_pybullet_client.py b/tests/backends/pybullet/test_pybullet_client.py new file mode 100644 index 0000000000..a076404652 --- /dev/null +++ b/tests/backends/pybullet/test_pybullet_client.py @@ -0,0 +1,65 @@ +import compas_fab +import pytest + +from compas_fab.backends import PyBulletClient +from compas_robots import RobotModel + +from compas_robots.resources import LocalPackageMeshLoader + + +def test_pybullet_client_connection_direct(): + with PyBulletClient(connection_type="direct") as client: + assert client.is_connected + + +def test_pybullet_client_load_robot(): + 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) + assert robot is not None + assert robot.name == "ur5_robot" + # Check that the RobotModel is present + assert isinstance(robot.model, RobotModel) + # Check that the robot do not have any geometry + with pytest.raises(Exception): + robot.ensure_geometry() + + +def test_pybullet_client_load_robot_with_meshes(): + with PyBulletClient(connection_type="direct") as client: + urdf_filename = compas_fab.get("robot_library/ur5_robot/urdf/robot_description.urdf") + mesh_loader = LocalPackageMeshLoader(compas_fab.get("robot_library/ur5_robot"), "") + robot = client.load_robot(urdf_filename, [mesh_loader]) + assert robot is not None + assert robot.name == "ur5_robot" + assert isinstance(robot.model, RobotModel) + link_names = robot.get_link_names_with_collision_geometry() + assert set(link_names) == set( + [ + "base_link_inertia", + "shoulder_link", + "upper_arm_link", + "forearm_link", + "wrist_1_link", + "wrist_2_link", + "wrist_3_link", + ] + ) + # Check that the robot has geometry + robot.ensure_geometry() + + +def test_pybullet_client_load_robot_with_sementics(): + with PyBulletClient(connection_type="direct") as client: + urdf_filename = compas_fab.get("robot_library/ur5_robot/urdf/robot_description.urdf") + mesh_loader = LocalPackageMeshLoader(compas_fab.get("robot_library/ur5_robot"), "") + robot = client.load_robot(urdf_filename, [mesh_loader]) + srdf_filename = compas_fab.get("robot_library/ur5_robot/robot_description_semantic.srdf") + client.load_semantics(robot, srdf_filename) + # Check that the robot has geometry + robot.ensure_geometry() + # Check that the robot has semantics + robot.ensure_semantics() + + +# TODO: After implementing the stateless backend, we should test methods related to planning scene and scene state management. diff --git a/tests/backends/pybullet/test_pybullet_planner.py b/tests/backends/pybullet/test_pybullet_planner.py new file mode 100644 index 0000000000..a19788bfb0 --- /dev/null +++ b/tests/backends/pybullet/test_pybullet_planner.py @@ -0,0 +1,259 @@ +import compas + +if not compas.IPY: + from compas_fab.backends import PyBulletClient + from compas_fab.backends import PyBulletPlanner + +from compas.geometry import Point +from compas.geometry import Vector +from compas.geometry import Frame + +from compas.tolerance import Tolerance +from compas_fab.robots import RobotLibrary + +# 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%) +# Angular tolerance is set to 2e-3 radians, equivalent to 0.11 degrees +TOL = Tolerance(unit="m", absolute=1e-4, relative=1e-3, angular=2e-3) + + +def validate_planner_model_fk_with_truth(planner_result, model_result, true_result): + """Helper function to validate planner results with known truth and model results + + Planner result comes from the planning backend. + Model result comes from the RobotModel FK calculation. + True result is the known ground truth result. + """ + + # Check with known ground truth result + assert TOL.is_allclose( + planner_result.point, true_result.point + ), f"Planner Result Pt {planner_result.point} != Known Truth Pt{true_result.point}" + assert TOL.is_angle_zero( + planner_result.xaxis.angle(true_result.xaxis) + ), f"Planner Result X {planner_result.xaxis} angle discrepancy with Known Truth X {true_result.xaxis}" + assert TOL.is_angle_zero( + planner_result.yaxis.angle(true_result.yaxis) + ), f"Planner Result Y {planner_result.yaxis} angle discrepancy with Known Truth Y {true_result.yaxis}" + # Check with RobotModel FK result + assert TOL.is_allclose( + model_result.point, true_result.point + ), f"Model Result Pt {model_result.point} != Known Truth Pt {true_result.point}" + assert TOL.is_angle_zero( + model_result.xaxis.angle(true_result.xaxis) + ), f"Model Result X {model_result.xaxis} angle discrepancy with Known Truth X {true_result.xaxis}" + assert TOL.is_angle_zero( + model_result.yaxis.angle(true_result.yaxis) + ), f"Model Result Y {model_result.yaxis} angle discrepancy with Known Truth Y {true_result.yaxis}" + + +def validate_ik_with_fk(ik_target_frame, fk_result_frame): + # Check if the IK target frame is close to the FK result frame + assert TOL.is_allclose( + ik_target_frame.point, fk_result_frame.point + ), f"IK Target Pt {ik_target_frame.point} != FK Result Pt {fk_result_frame.point}" + assert TOL.is_angle_zero( + ik_target_frame.xaxis.angle(fk_result_frame.xaxis) + ), f"IK Target X {ik_target_frame.xaxis} angle discrepancy with FK Result X {fk_result_frame.xaxis}" + assert TOL.is_angle_zero( + ik_target_frame.yaxis.angle(fk_result_frame.yaxis) + ), f"IK Target Y {ik_target_frame.yaxis} angle discrepancy with FK Result Y {fk_result_frame.yaxis}" + + +def _test_fk_with_pybullet_planner(robot, true_result): + with PyBulletClient(connection_type="direct") as client: + robot = client.load_existing_robot(robot) + planner = PyBulletPlanner(client) + + planning_group = robot.main_group_name + end_effector_link = robot.get_end_effector_link_name(planning_group) + + planner_fk_result = planner.forward_kinematics(robot, robot.zero_configuration(), planning_group) + print(planner_fk_result) + model_fk_result = robot.model.forward_kinematics(robot.zero_configuration(), end_effector_link) + print(model_fk_result) + + validate_planner_model_fk_with_truth(planner_fk_result, model_fk_result, true_result) + + +def test_pybullet_planner_fk_ur5(): + robot = RobotLibrary.ur5(load_geometry=True) + true_result = Frame( + point=Point(x=0.8172500133514404, y=0.19144999980926514, z=-0.005491000134497881), + xaxis=Vector(x=-1.0, y=0.0, z=0.0), + yaxis=Vector(x=0.0, y=0.0, z=1.0), + ) + _test_fk_with_pybullet_planner(robot, true_result) + + +def test_pybullet_planner_fk_abb_irb4600_40_255(): + robot = RobotLibrary.abb_irb4600_40_255(load_geometry=True) + true_result = Frame( + point=Point(x=1.58, y=0.0, z=1.765), + xaxis=Vector(x=0.0, y=0.0, z=-1.0), + yaxis=Vector(x=0.0, y=1.0, z=0.0), + ) + _test_fk_with_pybullet_planner(robot, true_result) + + +def test_pybullet_planner_fk_ur10e(): + robot = RobotLibrary.ur10e(load_geometry=True) + true_result = Frame( + point=Point(x=1.18425, y=0.2907, z=0.0608), + xaxis=Vector(x=-1.0, y=0.0, z=-0.0), + yaxis=Vector(x=0.0, y=0.0, z=1.0), + ) + _test_fk_with_pybullet_planner(robot, true_result) + + +###################################################### +# Testing the IK-FK agreement for the PyBullet backend +###################################################### + + +def _test_pybullet_ik_fk_agreement(robot, ik_target_frames): + # These options are set to ensure that the IK solver converges to a high accuracy + # Threshold is set to 1e-5 meters to be larger than the tolerance used for comparison (1e-4 meters) + ik_options = {"high_accuracy_max_iter": 50, "high_accuracy": True, "high_accuracy_threshold": 1e-5} + + with PyBulletClient(connection_type="direct") as client: + robot = client.load_existing_robot(robot) + planner = PyBulletPlanner(client) + planning_group = robot.main_group_name + 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(), + group=planning_group, + options=ik_options, + ) + ) + except StopIteration: + assert False, f"No IK Solution found for frame {ik_target_frame}" + + # Reconstruct the full Configuration from the returned joint positions + ik_result = robot.zero_configuration() + for name, value in zip(joint_names, joint_positions): + ik_result[name] = value + + # FK Query to the planner (Configuration to Frame) + fk_result = planner.forward_kinematics(robot, ik_result, planning_group) + + # Compare the frames + validate_ik_with_fk(ik_target_frame, fk_result) + + +def test_pybullet_ik_fk_agreement_ur5(): + robot = RobotLibrary.ur5(load_geometry=True) + + ik_center_frame = Frame( + Point(x=0.4, y=0.1, z=0.3), + Vector(x=-1.0, y=0.0, z=0.0), + Vector(x=0.0, y=0.0, z=1.0), + ) + + ik_target_frames = [] + ik_target_frames.append(ik_center_frame) + ik_target_frames.append(ik_center_frame.translated(Vector(-0.01, -0.01, -0.01))) + ik_target_frames.append(ik_center_frame.translated(Vector(0.0, 0.01, 0.0))) + ik_target_frames.append(ik_center_frame.translated(Vector(0.0, 0.0, 0.01))) + ik_target_frames.append(ik_center_frame.translated(Vector(0.1, 0.1, 0.2))) + ik_target_frames.append(ik_center_frame.translated(Vector(-0.1, -0.1, 0.0))) + + _test_pybullet_ik_fk_agreement(robot, ik_target_frames) + + +def test_pybullet_ik_fk_agreement_ur10e(): + robot = RobotLibrary.ur10e(load_geometry=True) + + ik_center_frame = Frame( + Point(x=0.4, y=0.1, z=0.3), + Vector(x=-1.0, y=0.0, z=0.0), + Vector(x=0.0, y=0.0, z=1.0), + ) + + ik_target_frames = [] + ik_target_frames.append(ik_center_frame) + ik_target_frames.append(ik_center_frame.translated(Vector(-0.01, -0.01, -0.01))) + ik_target_frames.append(ik_center_frame.translated(Vector(0.0, 0.01, 0.0))) + ik_target_frames.append(ik_center_frame.translated(Vector(0.0, 0.0, 0.01))) + ik_target_frames.append(ik_center_frame.translated(Vector(0.1, 0.1, 0.2))) + ik_target_frames.append(ik_center_frame.translated(Vector(-0.1, -0.1, 0.0))) + + _test_pybullet_ik_fk_agreement(robot, ik_target_frames) + + +def test_pybullet_ik_fk_agreement_abb_irb4600_40_255(): + robot = RobotLibrary.abb_irb4600_40_255(load_geometry=True) + + ik_center_frame = Frame( + Point(x=1.0, y=0.3, z=1.3), + Vector(x=-1.0, y=0.0, z=0.0), + Vector(x=0.0, y=0.0, z=1.0), + ) + + ik_target_frames = [] + ik_target_frames.append(ik_center_frame) + ik_target_frames.append(ik_center_frame.translated(Vector(-0.1, -0.1, -0.1))) + ik_target_frames.append(ik_center_frame.translated(Vector(0.0, 0.1, 0.0))) + ik_target_frames.append(ik_center_frame.translated(Vector(0.0, 0.0, 0.1))) + ik_target_frames.append(ik_center_frame.translated(Vector(0.1, 0.1, 0.2))) + ik_target_frames.append(ik_center_frame.translated(Vector(-0.3, -0.1, -0.3))) + + _test_pybullet_ik_fk_agreement(robot, ik_target_frames) + + +################################################## +# Testing IK out of reach for the PyBullet backend +################################################## + + +def test_pybullet_ik_out_of_reach_ur5(): + robot = RobotLibrary.ur5(load_geometry=True) + + ik_target_frames = [] + + ik_target_frames.append( + Frame( + Point(x=2.4, y=0.1, z=0.3), + Vector(x=-1.0, y=0.0, z=0.0), + Vector(x=0.0, y=0.0, z=1.0), + ) + ) + ik_target_frames.append( + Frame( + Point(x=0.4, y=1.5, z=0.3), + Vector(x=-1.0, y=0.0, z=0.0), + Vector(x=0.0, y=0.0, z=1.0), + ) + ) + + # high_accuracy_max_iter is set to 20 to reduce the number of iterations, faster testing time. + ik_options = {"high_accuracy_max_iter": 20, "high_accuracy": True, "high_accuracy_threshold": 1e-5} + + with PyBulletClient(connection_type="direct") as client: + robot = client.load_existing_robot(robot) + planner = PyBulletPlanner(client) + planning_group = robot.main_group_name + 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(), + group=planning_group, + options=ik_options, + ) + ) + # An error should be thrown here because the IK target is out of reach + assert False, f"IK Solution found when there should be none: frame {ik_target_frame}" + except StopIteration: + continue diff --git a/tests/ipy_test_runner.py b/tests/ipy_test_runner.py index d4448173d7..12e765c0e3 100644 --- a/tests/ipy_test_runner.py +++ b/tests/ipy_test_runner.py @@ -12,4 +12,10 @@ pytest.load_fake_module("Rhino") pytest.load_fake_module("Rhino.Geometry", fake_types=["RTree", "Sphere", "Point3d"]) - pytest.run(HERE) + # Exclude PyBullet tests in Iron Python environment + exclude_list = [ + "tests/backends/pybullet/test_pybullet_client.py", + "tests/backends/pybullet/test_pybullet_planner.py", + ] + + pytest.run(HERE, exclude_list=exclude_list) diff --git a/tests/robots/test_robot.py b/tests/robots/test_robot.py index eb3fc56fab..7e3a131fdf 100644 --- a/tests/robots/test_robot.py +++ b/tests/robots/test_robot.py @@ -83,7 +83,7 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N yield (ik[0], ik[2]) ur5_robot_instance.client = fake_client - ur5_robot_instance.client.inverse_kinematics = FakeInverseKinematics() + ur5_robot_instance.client.inverse_kinematics = FakeInverseKinematics(fake_client).inverse_kinematics return ur5_robot_instance