From bb1bd4dea5c6c03776f44c039f163e2fbd1708ac Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 3 May 2024 15:45:27 +0800 Subject: [PATCH 01/27] 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 0f13b20ff..027e2bca3 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/27] 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 027e2bca3..81ab7c7ce 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/27] 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 8cc68350f..b108a69b2 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/27] 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 388f3d93a..af947b375 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/27] 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 28ea26e12..e978eff60 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/27] 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 b108a69b2..d8c57b970 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/27] 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 70a017fb0..3de0693a5 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/27] 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 e978eff60..38a2f7f34 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 305f4c382..2d0c8811f 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/27] 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 38a2f7f34..58ba487a6 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/27] 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 81ab7c7ce..4b63d7491 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/27] 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 abfee5b2a..aa92f7b9a 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/27] 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 69b1be796..7b74fcd9f 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 1a72566fd..89446644e 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 42d339bb9..9e311a70f 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/27] 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 8ac23bd39..289542371 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 de0bf9ce6..338f20cb1 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/27] 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 58ba487a6..d4672bc8c 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 4b63d7491..70dc1bad9 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/27] 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 338f20cb1..46681d305 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 c1dff0a26b95b0886e5ee9d0259613efbc050ec7 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 31 May 2024 09:59:11 +0200 Subject: [PATCH 16/27] 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 46681d305..335f7ec89 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 17/27] 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 289542371..219a7fe36 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 18/27] 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 335f7ec89..5abad4bcb 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 19/27] 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 9beb21df1..612a63cc0 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 20/27] 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 d4672bc8c..71361378d 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 21/27] 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 71361378d..f7bde1769 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 22/27] 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 5abad4bcb..021c59346 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 d8c57b970..6398540ad 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 23/27] 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 70dc1bad9..22572096b 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 24/27] 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 22572096b..6e3cc8ed3 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 25/27] 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 f7bde1769..7dccb1d0e 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 6e3cc8ed3..63f3de8ec 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 e652a7f30e516c9ffbe485d1d971629ad4721f6c Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Wed, 12 Jun 2024 09:35:16 +0800 Subject: [PATCH 26/27] 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 6398540ad..f932e266e 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 27/27] 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 aa92f7b9a..c3e27fc18 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)