From bb1bd4dea5c6c03776f44c039f163e2fbd1708ac Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 3 May 2024 15:45:27 +0800 Subject: [PATCH 01/46] add tool_coordinate_frame stub --- src/compas_fab/robots/targets.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index 0f13b20fff..027e2bca33 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -497,6 +497,8 @@ class Waypoints(Data): ---------- 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` + The tool tip coordinate frame relative to the flange of the robot. See Also -------- @@ -509,7 +511,7 @@ def __init__(self, name="Generic Waypoints"): self.name = name @property - def __data__(self): + def tool_coordinate_frame(self): raise NotImplementedError def scaled(self, factor): @@ -687,6 +689,8 @@ def __init__( super(PointAxisWaypoints, self).__init__(name=name) self.target_points_and_axes = target_points_and_axes self.tolerance_position = tolerance_position + if isinstance(tool_coordinate_frame, Transformation): + tool_coordinate_frame = Frame.from_transformation(tool_coordinate_frame) self.tool_coordinate_frame = tool_coordinate_frame @property From 792603287d94c1fccdf0fb58d2f9cbee59bd3e22 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 3 May 2024 16:52:45 +0800 Subject: [PATCH 02/46] Add tool_coordinate_frame parameter to Waypoints constructor --- src/compas_fab/robots/targets.py | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index 027e2bca33..81ab7c7ce1 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -506,13 +506,10 @@ 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 tool_coordinate_frame(self): - raise NotImplementedError + self.tool_coordinate_frame = tool_coordinate_frame def scaled(self, factor): """Returns a scaled copy of the waypoints. From a49f023d85a558400adc07a25670ae7472279739 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 3 May 2024 16:52:55 +0800 Subject: [PATCH 03/46] Refactor plan_cartesian_motion method in Robot class --- src/compas_fab/robots/robot.py | 81 +++++++++++++++++++++------------- 1 file changed, 50 insertions(+), 31 deletions(-) diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index 8cc68350fd..b108a69b2d 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", ] @@ -1313,25 +1314,22 @@ 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. + If a tool is attached to the robot, the :meth:`~compas_fab.robots.Waypoints.tool_coordinate_frame` parameter + should be set. 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 +1358,51 @@ 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, + # options and the planned trajectory. + # 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() @@ -1408,24 +1415,36 @@ def plan_cartesian_motion( 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) From c0f92d73e7225ea80849079702d9a3d1ba175cc2 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 3 May 2024 16:53:11 +0800 Subject: [PATCH 04/46] Refactor plan_cartesian_motion method to accept waypoints instead of frames_WCF --- src/compas_fab/backends/interfaces/backend_features.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/compas_fab/backends/interfaces/backend_features.py b/src/compas_fab/backends/interfaces/backend_features.py index 388f3d93a6..af947b375b 100644 --- a/src/compas_fab/backends/interfaces/backend_features.py +++ b/src/compas_fab/backends/interfaces/backend_features.py @@ -125,18 +125,18 @@ class PlanCartesianMotion(object): . """ - 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) + def __call__(self, robot, waypoints, start_configuration=None, group=None, options=None): + return self.plan_cartesian_motion(robot, waypoints, start_configuration, group, options) - 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. From 1d6c96bcabd6065b2df028621932ca893c83236f Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 07:28:33 +0800 Subject: [PATCH 05/46] Refactor plan_cartesian_motion method to accept waypoints and implement support for FrameWaypoints . PointAxisWaypoints will be in next PR --- .../analytical_plan_cartesian_motion.py | 54 ++++++++++++++++--- 1 file changed, 47 insertions(+), 7 deletions(-) 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..e978eff600 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,10 @@ 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,15 +19,15 @@ 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. @@ -42,22 +46,48 @@ 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. + - 'stepsize' is not used to sample in between frames (i.e. no interpolation), 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.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) + # There is a maximum of 8 possible paths, corresponding to the 8 possible IK solutions for each frame + # The all() function is used to check if all configurations in a path are present. paths = [] for configurations in zip(*configurations_along_path): if all(configurations): @@ -74,12 +104,22 @@ def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, gro path = paths[idx] path = self.smooth_configurations(path) trajectory = JointTrajectory() - trajectory.fraction = len(path) / len(frames_RCF) + trajectory.fraction = len(path) / len( + frames_RCF + ) # Technically this 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 + ): + + 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) From 63b89fbade5ccb6fd9baa2b63540b03c49052800 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 07:37:57 +0800 Subject: [PATCH 06/46] I bit more comments --- src/compas_fab/robots/robot.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index b108a69b2d..d8c57b970d 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -1374,8 +1374,9 @@ def plan_cartesian_motion(self, waypoints, start_configuration=None, group=None, 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, - # options and the planned trajectory. + # 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 {} From b56b144137e24c1bdfef9cfaa9cfe1eb2dc60820 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 09:36:51 +0800 Subject: [PATCH 07/46] ros backend plan_cartesian_motion uses waypoints --- .../move_it_plan_cartesian_motion.py | 46 +++++++++++++++---- 1 file changed, 36 insertions(+), 10 deletions(-) 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..3de0693a54 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", ] @@ -37,15 +40,15 @@ class MoveItPlanCartesianMotion(PlanCartesianMotion): 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 +88,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 +102,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 ) @@ -129,7 +146,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)), @@ -146,3 +163,12 @@ def response_handler(response): errback(e) self.GET_CARTESIAN_PATH(self.ros_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") From 5d10999bb93f928d8cdf9256e1fa9641c1323b2c Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 09:37:09 +0800 Subject: [PATCH 08/46] comment change --- .../backends/kinematics/analytical_plan_cartesian_motion.py | 3 +-- .../ghpython/components/Cf_PlanCartesianMotion/code.py | 4 +++- 2 files changed, 4 insertions(+), 3 deletions(-) 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 e978eff600..38a2f7f34f 100644 --- a/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py +++ b/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py @@ -34,8 +34,7 @@ def plan_cartesian_motion(self, robot, waypoints, start_configuration=None, grou 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 ------- 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( From 1fcadb533f527cf7b038ea6bfbd61d9566e93a70 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 10:34:36 +0800 Subject: [PATCH 09/46] add type check, fix some mistakes --- .../analytical_plan_cartesian_motion.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) 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 38a2f7f34f..58ba487a68 100644 --- a/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py +++ b/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py @@ -12,6 +12,13 @@ from compas_fab.utilities import from_tcf_to_t0cf +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from typing import Optional # noqa: F401 + from compas_fab.robots import Robot # noqa: F401 + from compas_robots import Configuration # noqa: F401 + class AnalyticalPlanCartesianMotion(PlanCartesianMotion): """ """ @@ -60,6 +67,7 @@ def plan_cartesian_motion(self, robot, waypoints, start_configuration=None, grou def _plan_cartesian_motion_with_frame_waypoints( self, robot, waypoints, start_configuration=None, group=None, options=None ): + # type: (Robot, FrameWaypoints, Optional[Configuration], Optional[str], Optional[dict]) -> JointTrajectory """Calculates a cartesian motion path with frame waypoints. Planner behavior: @@ -69,7 +77,9 @@ def _plan_cartesian_motion_with_frame_waypoints( """ # 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.frames] + 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) @@ -114,7 +124,8 @@ def _plan_cartesian_motion_with_frame_waypoints( def _plan_cartesian_motion_with_point_axis_waypoints( self, robot, waypoints, start_configuration=None, group=None, options=None ): - + # type: (Robot, PointAxisWaypoints, Optional[Configuration], Optional[str], Optional[dict]) -> JointTrajectory + """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." ) From 1c23183233907735cd9eb8c0bca9b4994b49c750 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 10:35:10 +0800 Subject: [PATCH 10/46] Fix bugs in Targets --- src/compas_fab/robots/targets.py | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index 81ab7c7ce1..4b63d7491d 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -3,6 +3,12 @@ from compas.geometry import Transformation from compas_robots.model import Joint + +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from compas_robots import Configuration # noqa: F401 + __all__ = [ "Target", "FrameTarget", @@ -230,7 +236,7 @@ class PointAxisTarget(Target): 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. - nane : str, optional + name : str, optional The human-readable name of the target. Defaults to 'Point-Axis Target'. """ @@ -497,7 +503,7 @@ class Waypoints(Data): ---------- 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` + 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 @@ -509,6 +515,9 @@ class Waypoints(Data): def __init__(self, tool_coordinate_frame, name="Generic Waypoints"): super(Waypoints, self).__init__() self.name = name + # 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): @@ -566,13 +575,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): @@ -666,7 +672,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. @@ -683,12 +689,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 - 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): From eb856ec4480b518fbf46003ed44ba636df53a6dd Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 10:35:16 +0800 Subject: [PATCH 11/46] test file --- .../kinematics/test_inverse_kinematics.py | 49 ++++++++++++++++--- 1 file changed, 43 insertions(+), 6 deletions(-) diff --git a/tests/backends/kinematics/test_inverse_kinematics.py b/tests/backends/kinematics/test_inverse_kinematics.py index abfee5b2a3..aa92f7b9a2 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,45 @@ 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 + assert len(trajectory.points) == len(frame_waypoints.target_frames) From 9eb80fb4222fcaad6f84d49b4d9fe51bd66eca91 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 10:42:44 +0800 Subject: [PATCH 12/46] changed 3 doc tests files --- .../files/04_plan_cartesian_motion.py | 12 ++++++------ .../files/gh_plan_cartesian_motion.py | 17 +++++++++-------- .../04_cartesian_path_analytic_pybullet.py | 4 +++- 3 files changed, 18 insertions(+), 15 deletions(-) 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] From 23acbf9a4ccd3675ebeeefeb381402d5a222f30a Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 10:51:46 +0800 Subject: [PATCH 13/46] docs and change log --- CHANGELOG.md | 1 + docs/examples/02_description_models/03_targets.rst | 7 +++++++ 2 files changed, 8 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8ac23bd39d..2895423719 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed +* `Robot.plan_carteisan_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/examples/02_description_models/03_targets.rst b/docs/examples/02_description_models/03_targets.rst index de0bf9ce62..338f20cb1b 100644 --- a/docs/examples/02_description_models/03_targets.rst +++ b/docs/examples/02_description_models/03_targets.rst @@ -38,3 +38,10 @@ 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 Target (Multiple Point Segments) +------------------------------------------ + +The :class:`compas_fab.robots.Waypoint` classes are used to describe a sequence of +waypoints that the robot should pass through. \ No newline at end of file From 02f9753422269737efdfc8d4e2f526c963611ba8 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 13:45:56 +0800 Subject: [PATCH 14/46] fix IPY type checking --- .../kinematics/analytical_plan_cartesian_motion.py | 12 +++++++----- src/compas_fab/robots/targets.py | 10 ++++++---- 2 files changed, 13 insertions(+), 9 deletions(-) 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 58ba487a68..d4672bc8cb 100644 --- a/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py +++ b/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py @@ -1,5 +1,6 @@ import math +import compas from compas.geometry import argmin from compas_fab.backends.interfaces import PlanCartesianMotion @@ -12,12 +13,13 @@ from compas_fab.utilities import from_tcf_to_t0cf -from typing import TYPE_CHECKING +if not compas.IPY: + from typing import TYPE_CHECKING -if TYPE_CHECKING: - from typing import Optional # noqa: F401 - from compas_fab.robots import Robot # noqa: F401 - from compas_robots import Configuration # noqa: F401 + if TYPE_CHECKING: + from typing import Optional # noqa: F401 + from compas_fab.robots import Robot # noqa: F401 + from compas_robots import Configuration # noqa: F401 class AnalyticalPlanCartesianMotion(PlanCartesianMotion): diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index 4b63d7491d..70dc1bad9c 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -1,13 +1,15 @@ +import compas + from compas.data import Data from compas.geometry import Frame from compas.geometry import Transformation from compas_robots.model import Joint +if not compas.IPY: + from typing import TYPE_CHECKING -from typing import TYPE_CHECKING - -if TYPE_CHECKING: - from compas_robots import Configuration # noqa: F401 + if TYPE_CHECKING: + from compas_robots import Configuration # noqa: F401 __all__ = [ "Target", From 49847ed9ccfb0b86a744f28faf7cdf64c94c9f09 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 15:22:18 +0800 Subject: [PATCH 15/46] more stuff --- .../02_description_models/03_targets.rst | 30 ++++++++++++++----- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/docs/examples/02_description_models/03_targets.rst b/docs/examples/02_description_models/03_targets.rst index 338f20cb1b..46681d3054 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 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). @@ -40,8 +39,25 @@ combination of constraints. See :class:`compas_fab.robots.Constraint` for availa constraints. At the moment, only the ROS MoveIt planning backend supports this target type. ------------------------------------------ -Waypoints Target (Multiple Point Segments) +Waypoints (Multiple Points / Segments) ------------------------------------------ -The :class:`compas_fab.robots.Waypoint` classes are used to describe a sequence of -waypoints that the robot should pass through. \ No newline at end of file +The :class:`compas_fab.robots.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 a 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. + From a4f697ff96589b52418ece8370eb3757380b5f39 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 18:32:13 +0800 Subject: [PATCH 16/46] Change planning backend features to use inherence instead of call forwarding. --- .../backends/interfaces/__init__.py | 16 +- .../backends/interfaces/backend_features.py | 166 +++++------------- src/compas_fab/backends/interfaces/client.py | 2 +- .../pybullet_add_attached_collision_mesh.py | 3 - .../pybullet_add_collision_mesh.py | 3 - .../pybullet_append_collision_mesh.py | 3 - .../pybullet_forward_kinematics.py | 3 - .../pybullet_inverse_kinematics.py | 3 - ...pybullet_remove_attached_collision_mesh.py | 3 - .../pybullet_remove_collision_mesh.py | 3 - src/compas_fab/backends/pybullet/client.py | 15 +- src/compas_fab/backends/pybullet/planner.py | 44 ++--- .../move_it_add_attached_collision_mesh.py | 7 +- .../move_it_add_collision_mesh.py | 7 +- .../move_it_append_collision_mesh.py | 7 +- .../move_it_forward_kinematics.py | 7 +- .../move_it_inverse_kinematics.py | 9 +- .../move_it_plan_cartesian_motion.py | 7 +- .../backend_features/move_it_plan_motion.py | 9 +- .../move_it_planning_scene.py | 5 +- .../move_it_remove_attached_collision_mesh.py | 7 +- .../move_it_remove_collision_mesh.py | 7 +- .../move_it_reset_planning_scene.py | 9 +- src/compas_fab/backends/ros/planner.py | 64 ++----- tests/robots/test_robot.py | 2 +- 25 files changed, 122 insertions(+), 289 deletions(-) diff --git a/src/compas_fab/backends/interfaces/__init__.py b/src/compas_fab/backends/interfaces/__init__.py index 0c2f7d4455..ce5ef1fbcc 100644 --- a/src/compas_fab/backends/interfaces/__init__.py +++ b/src/compas_fab/backends/interfaces/__init__.py @@ -46,6 +46,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 +59,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 af947b375b..ae3db3c868 100644 --- a/src/compas_fab/backends/interfaces/backend_features.py +++ b/src/compas_fab/backends/interfaces/backend_features.py @@ -2,18 +2,32 @@ from __future__ import division from __future__ import print_function +import compas -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 - . - """ +if not compas.IPY: + from typing import TYPE_CHECKING - def __call__(self, robot, configuration, group=None, options=None): - return self.forward_kinematics(robot, configuration, group, options) + 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.""" + + 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. @@ -40,17 +54,8 @@ def forward_kinematics(self, robot, configuration, group=None, options=None): pass -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. @@ -78,17 +83,8 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N pass -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. @@ -116,17 +112,8 @@ def plan_motion(self, robot, target, start_configuration=None, group=None, optio pass -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, waypoints, start_configuration=None, group=None, options=None): - return self.plan_cartesian_motion(robot, waypoints, 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, waypoints, start_configuration=None, group=None, options=None): """Calculates a cartesian motion path (linear in tool space). @@ -154,17 +141,8 @@ def plan_cartesian_motion(self, robot, waypoints, start_configuration=None, grou pass -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. @@ -182,20 +160,11 @@ def get_planning_scene(self, options=None): pass -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 ---------- @@ -210,17 +179,8 @@ def reset_planning_scene(self, options=None): pass -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. @@ -240,17 +200,8 @@ def add_collision_mesh(self, collision_mesh, options=None): pass -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 +221,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. @@ -300,17 +242,8 @@ def append_collision_mesh(self, collision_mesh, options=None): pass -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. @@ -330,17 +263,8 @@ def add_attached_collision_mesh(self, attached_collision_mesh, options=None): pass -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. 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/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..87fe42d2df 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. 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..dab8262272 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. 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..a3586e95fd 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. 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..8497331ff9 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. 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..112dd80144 100644 --- a/src/compas_fab/backends/pybullet/client.py +++ b/src/compas_fab/backends/pybullet/client.py @@ -827,8 +827,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 plan_cartesian_motion(self, *args, **kwargs): - return AnalyticalPlanCartesianMotion(self)(*args, **kwargs) + 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, 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/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 3de0693a54..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 @@ -37,9 +37,6 @@ class MoveItPlanCartesianMotion(PlanCartesianMotion): validate_response, ) - def __init__(self, ros_client): - self.ros_client = ros_client - def plan_cartesian_motion(self, robot, waypoints, start_configuration=None, group=None, options=None): """Calculates a cartesian motion path (linear in tool space). @@ -137,7 +134,7 @@ def plan_cartesian_motion_with_frame_waypoints_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) @@ -162,7 +159,7 @@ 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 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..22ecc027b6 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. @@ -93,6 +90,8 @@ def plan_motion(self, robot, target, start_configuration=None, group=None, optio options["joints"] = {j.name: j.type for j in robot.model.joints} options["ee_link_name"] = robot.get_end_effector_link_name(group) + raise TypeError("This method is intentionally broken.") + return await_callback(self.plan_motion_async, **kwargs) def plan_motion_async(self, callback, errback, target, start_configuration=None, group=None, options=None): @@ -113,7 +112,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 +149,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..8c30023c4c 100644 --- a/src/compas_fab/backends/ros/planner.py +++ b/src/compas_fab/backends/ros/planner.py @@ -7,7 +7,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 +28,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/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 From c5b4748ca77b8bbc32a7d9fb505c837ac2ecf205 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 18:45:40 +0800 Subject: [PATCH 17/46] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2895423719..08029bf7f3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed +* Backend planners now use multi-inherence instead of `__call__` to include the backend functions. This allows for better generated documentation. * `Robot.plan_carteisan_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`. From ff98cb0570f2a3322fd34bc8835f482c5c9d9994 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 19:14:10 +0800 Subject: [PATCH 18/46] Better generated docs --- .../backends/interfaces/__init__.py | 1 + .../backends/interfaces/backend_features.py | 61 +++++++++++++++---- 2 files changed, 50 insertions(+), 12 deletions(-) diff --git a/src/compas_fab/backends/interfaces/__init__.py b/src/compas_fab/backends/interfaces/__init__.py index ce5ef1fbcc..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 diff --git a/src/compas_fab/backends/interfaces/backend_features.py b/src/compas_fab/backends/interfaces/backend_features.py index ae3db3c868..f688cece73 100644 --- a/src/compas_fab/backends/interfaces/backend_features.py +++ b/src/compas_fab/backends/interfaces/backend_features.py @@ -12,7 +12,14 @@ class BackendFeature(object): - """Base class for all backend features that are implemented by a backend client.""" + """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. + + """ def __init__(self, client): # All backend features are assumed to be associated with a backend client. @@ -36,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. @@ -53,6 +60,9 @@ 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(BackendFeature): """Mix-in interface for implementing a planner's inverse kinematics feature.""" @@ -66,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. @@ -82,6 +92,9 @@ 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(BackendFeature): """Mix-in interface for implementing a planner's plan motion feature.""" @@ -93,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 @@ -111,6 +124,9 @@ 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(BackendFeature): """Mix-in interface for implementing a planner's plan cartesian motion feature.""" @@ -124,12 +140,12 @@ def plan_cartesian_motion(self, robot, waypoints, start_configuration=None, grou The robot instance for which the cartesian motion path is being calculated. waypoints : :class:`compas_fab.robots.Waypoints` The waypoints for the robot to follow. - start_configuration: :class:`compas_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 planning group used for calculation. - options: dict, optional + options : dict, optional Dictionary containing kwargs for arguments specific to the client being queried. @@ -140,6 +156,9 @@ def plan_cartesian_motion(self, robot, waypoints, start_configuration=None, grou """ pass + # The implementation code is located in the backend's module: + # "src/compas_fab/backends//backend_features/_plan_cartesian_motion.py" + class GetPlanningScene(BackendFeature): """Mix-in interface for implementing a planner's get planning scene feature.""" @@ -159,6 +178,9 @@ 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(BackendFeature): """Mix-in interface for implementing a planner's reset planning scene feature.""" @@ -178,6 +200,9 @@ 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(BackendFeature): """Mix-in interface for implementing a planner's add collision mesh feature.""" @@ -199,6 +224,9 @@ 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(BackendFeature): """Mix-in interface for implementing a planner's remove collision mesh feature.""" @@ -241,6 +269,9 @@ 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(BackendFeature): """Mix-in interface for implementing a planner's add attached collision mesh feature.""" @@ -262,6 +293,9 @@ 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(BackendFeature): """Mix-in interface for implementing a planner's remove attached collision mesh feature.""" @@ -282,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" From f1064ce343451b6769fa66101155eb305af534c4 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 19:51:48 +0800 Subject: [PATCH 19/46] render compas_fab.backends.ros.backend_features in docs --- docs/developer/backends.rst | 12 +++++++++ .../pybullet/backend_features/__init__.py | 25 +++++++++++++++++-- .../backends/ros/backend_features/__init__.py | 22 ++++++++++++++++ src/compas_fab/backends/ros/planner.py | 1 + 4 files changed, 58 insertions(+), 2 deletions(-) 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/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/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/planner.py b/src/compas_fab/backends/ros/planner.py index 8c30023c4c..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 From 5bfe44753ff6af802106f4020e1801d13dc49609 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 19:58:36 +0800 Subject: [PATCH 20/46] unbreak my heart --- .../backends/ros/backend_features/move_it_plan_motion.py | 2 -- 1 file changed, 2 deletions(-) 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 22ecc027b6..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 @@ -90,8 +90,6 @@ def plan_motion(self, robot, target, start_configuration=None, group=None, optio options["joints"] = {j.name: j.type for j in robot.model.joints} options["ee_link_name"] = robot.get_end_effector_link_name(group) - raise TypeError("This method is intentionally broken.") - return await_callback(self.plan_motion_async, **kwargs) def plan_motion_async(self, callback, errback, target, start_configuration=None, group=None, options=None): From 7d15d9426787e12a0fff93805ffeaf9c96b479c7 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 30 May 2024 04:20:26 +0800 Subject: [PATCH 21/46] Renamed `get_cached_robot` to `get_cached_robot_model` --- CHANGELOG.md | 4 + .../analytical_inverse_kinematics.py | 2 +- .../pybullet_add_attached_collision_mesh.py | 4 +- .../pybullet_forward_kinematics.py | 6 +- .../pybullet_inverse_kinematics.py | 8 +- ...pybullet_remove_attached_collision_mesh.py | 4 +- src/compas_fab/backends/pybullet/client.py | 120 ++++++++++-------- 7 files changed, 80 insertions(+), 68 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 08029bf7f3..1e5e2ec70a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,10 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed +* 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_carteisan_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. 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/pybullet/backend_features/pybullet_add_attached_collision_mesh.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_add_attached_collision_mesh.py index 87fe42d2df..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 @@ -60,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) @@ -68,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_forward_kinematics.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py index dab8262272..730400591e 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 @@ -37,9 +37,9 @@ def forward_kinematics(self, robot, configuration, group=None, options=None): The frame in the world's coordinate system (WCF). """ 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 a3586e95fd..27b9d2445c 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 @@ -74,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] 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 8497331ff9..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 @@ -34,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/client.py b/src/compas_fab/backends/pybullet/client.py index 112dd80144..d8bf706c4e 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" ) @@ -244,9 +249,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 +270,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 +295,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 +308,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 +350,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 +390,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 +413,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 +430,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 +502,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 +525,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 +540,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 +610,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 +660,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 +679,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 From 41c08d7a3357c475a36172b3858c3502ae4a5a69 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 30 May 2024 13:57:09 +0800 Subject: [PATCH 22/46] Found error in PyBullet _accurate_inverse_kinematics when comparing threshold --- CHANGELOG.md | 1 + .../pybullet/backend_features/pybullet_inverse_kinematics.py | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1e5e2ec70a..bffb5c5ab0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed +* 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`. 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 27b9d2445c..ec5bb69f67 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 @@ -202,8 +202,10 @@ def _accurate_inverse_kinematics(self, joints, threshold, max_iter, **kwargs): target_position[1] - new_pose[1], target_position[2] - new_pose[2], ] + # The distance is squared to avoid a sqrt operation distance = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2] - close_enough = distance < threshold + # Therefor, the threshold is squared as well + close_enough = distance < threshold * threshold kwargs["restPoses"] = joint_poses iter += 1 From e82b723931a19077e4d3532823a47db372146693 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 30 May 2024 13:59:47 +0800 Subject: [PATCH 23/46] Fixed error in PyBulletForwardKinematics.forward_kinematics where function would crash --- CHANGELOG.md | 1 + .../pybullet/backend_features/pybullet_forward_kinematics.py | 2 ++ 2 files changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index bffb5c5ab0..5e3b9ee168 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,7 @@ 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`. 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 730400591e..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 @@ -36,6 +36,8 @@ 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_model = self.client.get_cached_robot_model(robot) body_id = self.client.get_uid(cached_robot_model) From 4ed1f6cae477dcc36f2501a5bfdb4161f62b5b2d Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 30 May 2024 14:02:14 +0800 Subject: [PATCH 24/46] Added `PyBulletClient.load_existing_robot` --- CHANGELOG.md | 1 + src/compas_fab/backends/pybullet/client.py | 33 +++++++++++++++++++++- 2 files changed, 33 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5e3b9ee168..dc94daa9ed 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added +* 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` diff --git a/src/compas_fab/backends/pybullet/client.py b/src/compas_fab/backends/pybullet/client.py index d8bf706c4e..ab1d8cad13 100644 --- a/src/compas_fab/backends/pybullet/client.py +++ b/src/compas_fab/backends/pybullet/client.py @@ -218,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 ---------- From 03d0a168436d05b2df82e625157264e0dc08e898 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 30 May 2024 14:03:07 +0800 Subject: [PATCH 25/46] Added notes in docstring of `get_link_names_with_collision_geometry` --- src/compas_fab/robots/robot.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index d8c57b970d..ab4d249583 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -393,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` From 4f83af79ffb3f209c686a19f36951cd7395a8805 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 30 May 2024 14:06:09 +0800 Subject: [PATCH 26/46] Added unit test for `PyBulletClient` and `PyBulletPlanner` --- CHANGELOG.md | 1 + .../backends/pybullet/test_pybullet_client.py | 70 +++++ .../pybullet/test_pybullet_planner.py | 252 ++++++++++++++++++ 3 files changed, 323 insertions(+) create mode 100644 tests/backends/pybullet/test_pybullet_client.py create mode 100644 tests/backends/pybullet/test_pybullet_planner.py diff --git a/CHANGELOG.md b/CHANGELOG.md index dc94daa9ed..66d630b9f0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,7 @@ 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. diff --git a/tests/backends/pybullet/test_pybullet_client.py b/tests/backends/pybullet/test_pybullet_client.py new file mode 100644 index 0000000000..b320380b64 --- /dev/null +++ b/tests/backends/pybullet/test_pybullet_client.py @@ -0,0 +1,70 @@ +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_connection_gui(): + with PyBulletClient(connection_type="gui") 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..123d95b630 --- /dev/null +++ b/tests/backends/pybullet/test_pybullet_planner.py @@ -0,0 +1,252 @@ +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 +TOL = Tolerance(unit="m", absolute=1e-4, relative=1e-4) + + +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_allclose( + planner_result.xaxis, true_result.xaxis + ), f"Planner Result X {planner_result.xaxis} != Known Truth X {true_result.xaxis}" + assert TOL.is_allclose( + planner_result.yaxis, true_result.yaxis + ), f"Planner Result Y {planner_result.yaxis} != 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_allclose( + model_result.xaxis, true_result.xaxis + ), f"Model Result X {model_result.xaxis} != Known Truth X {true_result.xaxis}" + assert TOL.is_allclose( + model_result.yaxis, true_result.yaxis + ), f"Model Result Y {model_result.yaxis} != 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_allclose( + ik_target_frame.xaxis, fk_result_frame.xaxis + ), f"IK Target X {ik_target_frame.xaxis} != FK Result X {fk_result_frame.xaxis}" + assert TOL.is_allclose( + ik_target_frame.yaxis, fk_result_frame.yaxis + ), f"IK Target Y {ik_target_frame.yaxis} != 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 From c1dff0a26b95b0886e5ee9d0259613efbc050ec7 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 31 May 2024 09:59:11 +0200 Subject: [PATCH 27/46] Update docs/examples/02_description_models/03_targets.rst --- docs/examples/02_description_models/03_targets.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/examples/02_description_models/03_targets.rst b/docs/examples/02_description_models/03_targets.rst index 46681d3054..335f7ec89c 100644 --- a/docs/examples/02_description_models/03_targets.rst +++ b/docs/examples/02_description_models/03_targets.rst @@ -55,7 +55,7 @@ It is created by a list of :class:`compas.geometry.Frame` objects or alternative 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 a targets where the rotation +: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 From 847142a0bd95b2f18a48e2281dd644b989860aeb Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 7 Jun 2024 03:27:00 +0200 Subject: [PATCH 28/46] Update CHANGELOG.md Co-authored-by: Gonzalo Casas --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2895423719..219a7fe36b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,7 +17,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed -* `Robot.plan_carteisan_motion()` now accepts `Waypoints` as target. Implementation for `FrameWaypoints` is supported with same functionality as before. Simply wrap `Frame` objects using `FrameWaypoints(frames)`. +* `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. From 3f7706290b0eb00e7c2dcbba5cf8324fb818cf4e Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 7 Jun 2024 03:43:04 +0200 Subject: [PATCH 29/46] Apply suggestions from code review Co-authored-by: Gonzalo Casas --- docs/examples/02_description_models/03_targets.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/examples/02_description_models/03_targets.rst b/docs/examples/02_description_models/03_targets.rst index 335f7ec89c..5abad4bcba 100644 --- a/docs/examples/02_description_models/03_targets.rst +++ b/docs/examples/02_description_models/03_targets.rst @@ -9,7 +9,7 @@ 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 Free Motion Planning with :meth:`compas_fab.robots.Robot.plan_motion`. +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). @@ -42,7 +42,7 @@ constraints. At the moment, only the ROS MoveIt planning backend supports this t Waypoints (Multiple Points / Segments) ------------------------------------------ -The :class:`compas_fab.robots.Waypoints` classes are used to describe a sequence of +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. From 90d07c4a69235c555325aba386e4209b9352c59c Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 6 Jun 2024 21:46:58 +0800 Subject: [PATCH 30/46] fix test_target incorrect test at tolerance_orientation --- tests/robots/test_targets.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/robots/test_targets.py b/tests/robots/test_targets.py index 9beb21df1f..612a63cc00 100644 --- a/tests/robots/test_targets.py +++ b/tests/robots/test_targets.py @@ -172,7 +172,7 @@ def test_target_scale(frame_target): nt = frame_target.scaled(scale_factor) assert nt.target_frame == frame_target.target_frame.scaled(scale_factor) assert nt.tolerance_position == frame_target.tolerance_position * scale_factor - assert nt.tolerance_orientation == frame_target.tolerance_orientation * scale_factor + assert nt.tolerance_orientation == frame_target.tolerance_orientation assert nt.tool_coordinate_frame == frame_target.tool_coordinate_frame.scaled(scale_factor) From 13bd6520693b0e55a8cbd568eb0452a8cdc9bab7 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 6 Jun 2024 21:54:53 +0800 Subject: [PATCH 31/46] Remove type hints from new files --- .../analytical_plan_cartesian_motion.py | 17 ++--------------- 1 file changed, 2 insertions(+), 15 deletions(-) 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 d4672bc8cb..71361378d6 100644 --- a/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py +++ b/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py @@ -11,16 +11,6 @@ from compas_fab.robots import FrameWaypoints from compas_fab.robots import PointAxisWaypoints -from compas_fab.utilities import from_tcf_to_t0cf - -if not compas.IPY: - from typing import TYPE_CHECKING - - if TYPE_CHECKING: - from typing import Optional # noqa: F401 - from compas_fab.robots import Robot # noqa: F401 - from compas_robots import Configuration # noqa: F401 - class AnalyticalPlanCartesianMotion(PlanCartesianMotion): """ """ @@ -69,7 +59,6 @@ def plan_cartesian_motion(self, robot, waypoints, start_configuration=None, grou def _plan_cartesian_motion_with_frame_waypoints( self, robot, waypoints, start_configuration=None, group=None, options=None ): - # type: (Robot, FrameWaypoints, Optional[Configuration], Optional[str], Optional[dict]) -> JointTrajectory """Calculates a cartesian motion path with frame waypoints. Planner behavior: @@ -115,9 +104,8 @@ def _plan_cartesian_motion_with_frame_waypoints( path = paths[idx] path = self.smooth_configurations(path) trajectory = JointTrajectory() - trajectory.fraction = len(path) / len( - frames_RCF - ) # Technically this always be 1.0 because otherwise, the path would be rejected earlier + 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) @@ -126,7 +114,6 @@ def _plan_cartesian_motion_with_frame_waypoints( def _plan_cartesian_motion_with_point_axis_waypoints( self, robot, waypoints, start_configuration=None, group=None, options=None ): - # type: (Robot, PointAxisWaypoints, Optional[Configuration], Optional[str], Optional[dict]) -> JointTrajectory """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." From de28b227eec80c97cce75bfdd88b43d2ee7f8497 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 6 Jun 2024 22:06:00 +0800 Subject: [PATCH 32/46] Rephrased the developer note inside _plan_cartesian_motion_with_frame_waypoints --- .../kinematics/analytical_plan_cartesian_motion.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) 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 71361378d6..f7bde1769d 100644 --- a/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py +++ b/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py @@ -10,6 +10,7 @@ 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): @@ -64,7 +65,7 @@ def _plan_cartesian_motion_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. - - 'stepsize' is not used to sample in between frames (i.e. no interpolation), only the input frames are used. + - 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: @@ -86,8 +87,13 @@ def _plan_cartesian_motion_with_frame_waypoints( configurations = list(robot.iter_inverse_kinematics(frame, options=options)) configurations_along_path.append(configurations) - # There is a maximum of 8 possible paths, corresponding to the 8 possible IK solutions for each frame - # The all() function is used to check if all configurations in a path are present. + # 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): From e7dc608246f44ca98feb78cd94aa79198cdfe90a Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 6 Jun 2024 22:33:58 +0800 Subject: [PATCH 33/46] Rephrased the doc for `waypoint` parameter in `robot.plan_cartesian_motion` --- docs/examples/02_description_models/03_targets.rst | 2 ++ src/compas_fab/robots/robot.py | 5 +++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/docs/examples/02_description_models/03_targets.rst b/docs/examples/02_description_models/03_targets.rst index 5abad4bcba..021c59346c 100644 --- a/docs/examples/02_description_models/03_targets.rst +++ b/docs/examples/02_description_models/03_targets.rst @@ -38,6 +38,8 @@ constraints as a planning target. This is intended for advanced users who want t 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) ------------------------------------------ diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index d8c57b970d..6398540ad0 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -1321,8 +1321,9 @@ def plan_cartesian_motion(self, waypoints, start_configuration=None, group=None, ---------- waypoints : :class:`compas_fab.robots.Waypoints` The waypoints for the robot to follow. - If a tool is attached to the robot, the :meth:`~compas_fab.robots.Waypoints.tool_coordinate_frame` parameter - should be set. + 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 start of the motion. From 34f60be3e1792d453e8c12e17380558b2554f54c Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 6 Jun 2024 22:37:43 +0800 Subject: [PATCH 34/46] Fixed incorrect docs for target and waypoint --- src/compas_fab/robots/targets.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index 70dc1bad9c..22572096b2 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -30,9 +30,9 @@ class Target(Data): pose, configuration, and joint constraints. Dynamic targets such as velocity, acceleration, and jerk are not yet supported. - Waypoints are intended to be used for motion planning with a planning backend by using :meth:`compas_fab.robot.plan_motion`. - 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 ---------- @@ -497,8 +497,8 @@ 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 From a01221b1a6641e75121a56959ddfbaa3ed084d60 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 6 Jun 2024 22:38:58 +0800 Subject: [PATCH 35/46] chore: Refactor tolerance_orientation calculation in FrameTarget --- src/compas_fab/robots/targets.py | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index 22572096b2..6e3cc8ed30 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -5,12 +5,6 @@ from compas.geometry import Transformation from compas_robots.model import Joint -if not compas.IPY: - from typing import TYPE_CHECKING - - if TYPE_CHECKING: - from compas_robots import Configuration # noqa: F401 - __all__ = [ "Target", "FrameTarget", @@ -190,7 +184,7 @@ def scaled(self, factor): """ target_frame = self.target_frame.scaled(factor) tolerance_position = self.tolerance_position * factor - tolerance_orientation = self.tolerance_orientation * factor + tolerance_orientation = self.tolerance_orientation tool_coordinate_frame = self.tool_coordinate_frame.scaled(factor) if self.tool_coordinate_frame else None return FrameTarget(target_frame, tolerance_position, tolerance_orientation, tool_coordinate_frame, self.name) From 88c1ee216adbbc8bcfe65cfb35c13905b034d0bd Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 6 Jun 2024 22:39:46 +0800 Subject: [PATCH 36/46] lint --- .../backends/kinematics/analytical_plan_cartesian_motion.py | 1 - src/compas_fab/robots/targets.py | 2 -- 2 files changed, 3 deletions(-) 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 f7bde1769d..7dccb1d0e4 100644 --- a/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py +++ b/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py @@ -1,6 +1,5 @@ import math -import compas from compas.geometry import argmin from compas_fab.backends.interfaces import PlanCartesianMotion diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index 6e3cc8ed30..63f3de8ec3 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -1,5 +1,3 @@ -import compas - from compas.data import Data from compas.geometry import Frame from compas.geometry import Transformation From 103d9b7b62af4545b42e80760cc274cb2bcb419f Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 7 Jun 2024 03:07:23 +0800 Subject: [PATCH 37/46] Remove pybullet client test with GUI --- tests/backends/pybullet/test_pybullet_client.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/tests/backends/pybullet/test_pybullet_client.py b/tests/backends/pybullet/test_pybullet_client.py index b320380b64..a076404652 100644 --- a/tests/backends/pybullet/test_pybullet_client.py +++ b/tests/backends/pybullet/test_pybullet_client.py @@ -12,11 +12,6 @@ def test_pybullet_client_connection_direct(): assert client.is_connected -def test_pybullet_client_connection_gui(): - with PyBulletClient(connection_type="gui") 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") From e652a7f30e516c9ffbe485d1d971629ad4721f6c Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Wed, 12 Jun 2024 09:35:16 +0800 Subject: [PATCH 38/46] Clean up code in plan_cartesian_motion --- src/compas_fab/robots/robot.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index 6398540ad0..f932e266e6 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -1415,9 +1415,7 @@ def plan_cartesian_motion(self, waypoints, start_configuration=None, group=None, 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 + options["path_constraints"] = path_constraints_WCF_scaled # ===================== # Attached CM and Tools From ba32b907563e560ece30ab74ccba0c20fc179405 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Wed, 12 Jun 2024 09:45:59 +0800 Subject: [PATCH 39/46] Added comment about interpolation in the test for AnalyticalPyBulletClient --- tests/backends/kinematics/test_inverse_kinematics.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tests/backends/kinematics/test_inverse_kinematics.py b/tests/backends/kinematics/test_inverse_kinematics.py index aa92f7b9a2..c3e27fc18a 100644 --- a/tests/backends/kinematics/test_inverse_kinematics.py +++ b/tests/backends/kinematics/test_inverse_kinematics.py @@ -150,4 +150,6 @@ def test_kinematics_cartesian_with_tool_coordinate_frame(frame_waypoints): # 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) From 514d2e9b631edc6f630c9b068eb1f5fec2c66258 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 13 Jun 2024 15:27:46 +0800 Subject: [PATCH 40/46] Changed the default inverse kinematic high_accuracy threshold to 0.1mm (1e-04) --- .../backend_features/pybullet_inverse_kinematics.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 ec5bb69f67..8ecaea7e19 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 @@ -138,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), ) ) @@ -203,9 +203,9 @@ def _accurate_inverse_kinematics(self, joints, threshold, max_iter, **kwargs): target_position[2] - new_pose[2], ] # The distance is squared to avoid a sqrt operation - distance = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2] + distance_squared = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2] # Therefor, the threshold is squared as well - close_enough = distance < threshold * threshold + close_enough = distance_squared < threshold * threshold kwargs["restPoses"] = joint_poses iter += 1 From c0bf89fe028c9d307725ce5460c5d0e1b501c36c Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 13 Jun 2024 15:30:18 +0800 Subject: [PATCH 41/46] lint --- .../pybullet/backend_features/pybullet_inverse_kinematics.py | 1 - 1 file changed, 1 deletion(-) 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 8ecaea7e19..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 @@ -183,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"] From c9758046456fc8a73562ba646d4b1c52584b4d70 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 13 Jun 2024 15:43:40 +0800 Subject: [PATCH 42/46] Lets see what happens if we also import PyBullet stuff in IPY environment --- src/compas_fab/backends/__init__.py | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/src/compas_fab/backends/__init__.py b/src/compas_fab/backends/__init__.py index a635e6a543..74a67525bd 100644 --- a/src/compas_fab/backends/__init__.py +++ b/src/compas_fab/backends/__init__.py @@ -147,15 +147,13 @@ Staubli_TX260LKinematics, ABB_IRB4600_40_255Kinematics, ) - -if not compas.IPY: - from .pybullet import ( - PyBulletClient, - CollisionError, - PyBulletError, - PyBulletPlanner, - AnalyticalPyBulletClient, - ) +from .pybullet import ( + PyBulletClient, + CollisionError, + PyBulletError, + PyBulletPlanner, + AnalyticalPyBulletClient, +) __all__ = [ # Base From 9c62178139c08ec9f7b4c58496b9997ba10c9b29 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 13 Jun 2024 16:12:01 +0800 Subject: [PATCH 43/46] Improve angle comparison at PyBullet Planner Test --- .../pybullet/test_pybullet_planner.py | 40 ++++++++++--------- 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/tests/backends/pybullet/test_pybullet_planner.py b/tests/backends/pybullet/test_pybullet_planner.py index 123d95b630..3372c8034b 100644 --- a/tests/backends/pybullet/test_pybullet_planner.py +++ b/tests/backends/pybullet/test_pybullet_planner.py @@ -8,7 +8,9 @@ from compas_fab.robots import RobotLibrary # The tolerance for the tests are set to 1e-4 meters, equivalent to 0.1 mm -TOL = Tolerance(unit="m", absolute=1e-4, relative=1e-4) +# 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): @@ -22,22 +24,22 @@ def validate_planner_model_fk_with_truth(planner_result, model_result, true_resu 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_allclose( - planner_result.xaxis, true_result.xaxis - ), f"Planner Result X {planner_result.xaxis} != Known Truth X {true_result.xaxis}" - assert TOL.is_allclose( - planner_result.yaxis, true_result.yaxis - ), f"Planner Result Y {planner_result.yaxis} != Known Truth Y {true_result.yaxis}" + assert TOL.is_angle_zero( + planner_result.xaxis.angle(true_result.xaxis) + ), f"Planner Result X {planner_result.xaxis} angle 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 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_allclose( - model_result.xaxis, true_result.xaxis - ), f"Model Result X {model_result.xaxis} != Known Truth X {true_result.xaxis}" - assert TOL.is_allclose( - model_result.yaxis, true_result.yaxis - ), f"Model Result Y {model_result.yaxis} != Known Truth Y {true_result.yaxis}" + assert TOL.is_angle_zero( + model_result.xaxis.angle(true_result.xaxis) + ), f"Model Result X {model_result.xaxis} angle 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 with Known Truth Y {true_result.yaxis}" def validate_ik_with_fk(ik_target_frame, fk_result_frame): @@ -45,12 +47,12 @@ def validate_ik_with_fk(ik_target_frame, 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_allclose( - ik_target_frame.xaxis, fk_result_frame.xaxis - ), f"IK Target X {ik_target_frame.xaxis} != FK Result X {fk_result_frame.xaxis}" - assert TOL.is_allclose( - ik_target_frame.yaxis, fk_result_frame.yaxis - ), f"IK Target Y {ik_target_frame.yaxis} != FK Result Y {fk_result_frame.yaxis}" + assert TOL.is_angle_zero( + ik_target_frame.xaxis.angle(fk_result_frame.xaxis) + ), f"IK Target X {ik_target_frame.xaxis} angle with FK Result X {fk_result_frame.xaxis} > tolerance" + assert TOL.is_angle_zero( + ik_target_frame.yaxis.angle(fk_result_frame.yaxis) + ), f"IK Target Y {ik_target_frame.yaxis} angle with FK Result Y {fk_result_frame.yaxis} > tolerance" def _test_fk_with_pybullet_planner(robot, true_result): From ee08aee6c3477c1d2be647750b447e36be240826 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 13 Jun 2024 17:11:46 +0800 Subject: [PATCH 44/46] Better error message --- .../pybullet/test_pybullet_planner.py | 21 ++++++++++++------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/tests/backends/pybullet/test_pybullet_planner.py b/tests/backends/pybullet/test_pybullet_planner.py index 3372c8034b..a19788bfb0 100644 --- a/tests/backends/pybullet/test_pybullet_planner.py +++ b/tests/backends/pybullet/test_pybullet_planner.py @@ -1,5 +1,9 @@ -from compas_fab.backends import PyBulletClient -from compas_fab.backends import PyBulletPlanner +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 @@ -20,26 +24,27 @@ def validate_planner_model_fk_with_truth(planner_result, model_result, true_resu 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 with Known Truth X {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 with Known Truth Y {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 with Known Truth X {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 with Known Truth Y {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): @@ -49,10 +54,10 @@ def validate_ik_with_fk(ik_target_frame, fk_result_frame): ), 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 with FK Result X {fk_result_frame.xaxis} > tolerance" + ), 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 with FK Result Y {fk_result_frame.yaxis} > tolerance" + ), 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): From 3db9aa7fa05b2c2f1d8775edcddb8771ae66a4ea Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 13 Jun 2024 17:19:05 +0800 Subject: [PATCH 45/46] Exclude PyBullet tests in Iron Python environment --- tests/ipy_test_runner.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) 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) From 20d947f87715ec31ab76e8747060b68de4bd50c0 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 13 Jun 2024 17:20:40 +0800 Subject: [PATCH 46/46] Put the IPY guard back in backends import --- src/compas_fab/backends/__init__.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/compas_fab/backends/__init__.py b/src/compas_fab/backends/__init__.py index 74a67525bd..9501fdb169 100644 --- a/src/compas_fab/backends/__init__.py +++ b/src/compas_fab/backends/__init__.py @@ -147,13 +147,16 @@ Staubli_TX260LKinematics, ABB_IRB4600_40_255Kinematics, ) -from .pybullet import ( - PyBulletClient, - CollisionError, - PyBulletError, - PyBulletPlanner, - AnalyticalPyBulletClient, -) + +if not compas.IPY: + # PyBullet do not work in IronPython + from .pybullet import ( + PyBulletClient, + CollisionError, + PyBulletError, + PyBulletPlanner, + AnalyticalPyBulletClient, + ) __all__ = [ # Base