diff --git a/CHANGELOG.md b/CHANGELOG.md index 8ac23bd39..219a7fe36 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_cartesian_motion()` now accepts `Waypoints` as target. Implementation for `FrameWaypoints` is supported with same functionality as before. Simply wrap `Frame` objects using `FrameWaypoints(frames)`. * Changed `BoundingVolume`, `Constraint`, `JointConstraint`, `OrientationConstraint`, `PositionConstraint` to inherit from `compas.data.Data` class. * Change the signature of `plan_motion()` to use `target` (`Target` class) instead of `goal_constraints`. Only one target is accepted. Users who wish to compose their own constraint sets can still use `ConstraintSetTarget`. * Moved `Robot.orientation_constraint_from_frame()` to `OrientationConstraint.from_frame()`, as constraints are no longer intended for users to use directly. diff --git a/docs/examples/02_description_models/03_targets.rst b/docs/examples/02_description_models/03_targets.rst index de0bf9ce6..021c59346 100644 --- a/docs/examples/02_description_models/03_targets.rst +++ b/docs/examples/02_description_models/03_targets.rst @@ -1,16 +1,15 @@ .. _targets: ******************************************************************************* -Targets +Targets and Waypoints ******************************************************************************* ----------------------- -Single Targets (Static) +Targets (Single Goal) ----------------------- Target classes are used to describe the goal condition (i.e. end condition) of a robot -for motion planning. They can be used for both Free Motion Planning (FMP) and Cartesian -Motion Planning (CMP). +for motion planning. They can be used for Free-space Motion Planning with :meth:`compas_fab.robots.Robot.plan_motion`. The :class:`compas_fab.robots.FrameTarget` is the most common target for motion planning. It defines the complete pose of the end-effector (or the robot flange, if no tool is attached). @@ -38,3 +37,29 @@ The :class:`compas_fab.robots.ConstraintSetTarget` class is used to specify a li constraints as a planning target. This is intended for advanced users who want to create custom combination of constraints. See :class:`compas_fab.robots.Constraint` for available constraints. At the moment, only the ROS MoveIt planning backend supports this target type. + +.. _waypoints: + +------------------------------------------ +Waypoints (Multiple Points / Segments) +------------------------------------------ + +Waypoints classes are used to describe a sequence of +waypoints that the robot should pass through in a planned motion. They are similar to Targets classes +but contain a list of targets instead of a single target, which is useful for tasks such as +drawing, welding or 3D printing. +They can be used for Cartesian Motion Planning with :meth:`compas_fab.robots.Robot.plan_cartesian_motion`. + +The :class:`compas_fab.robots.FrameWaypoints` is the most common waypoint for Cartesian motion planning. +It defines a list of complete pose for the end-effector (or the robot flange, if no tool is attached). +It is created by a list of :class:`compas.geometry.Frame` objects or alternatively from a list of +:class:`compas.geometry.Transformation` objects. + +The :class:`compas_fab.robots.PointAxisWaypoints` class is used for specifying a list of waypoints based on +the Point-Axis concept used in the :class:`compas_fab.robots.PointAxisTarget`. Compared to +:class:`~compas_fab.robots.FrameWaypoints`, this class allows for specifying targets where the rotation +around the Z-axis is not fixed. This is useful for example when the robot is using a cylindrical tool +to perform a task, for example 3D printing, welding or drilling. The freely rotating axis is defined relative +to the Z-axis of the tool coordinate frame (TCF). Note that the orientation of the tool +at the end of the motion is not determined until after the motion is planned. + diff --git a/docs/examples/03_backends_ros/files/04_plan_cartesian_motion.py b/docs/examples/03_backends_ros/files/04_plan_cartesian_motion.py index 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] 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. 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..7dccb1d0e 100644 --- a/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py +++ b/src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py @@ -7,6 +7,9 @@ from compas_fab.backends.kinematics.utils import smallest_joint_angles from compas_fab.robots import JointTrajectory from compas_fab.robots import JointTrajectoryPoint +from compas_fab.robots import FrameWaypoints +from compas_fab.robots import PointAxisWaypoints +from compas_fab.utilities import from_tcf_to_t0cf class AnalyticalPlanCartesianMotion(PlanCartesianMotion): @@ -15,23 +18,22 @@ class AnalyticalPlanCartesianMotion(PlanCartesianMotion): def __init__(self, client=None): self.client = client - def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, group=None, options=None): + def plan_cartesian_motion(self, robot, waypoints, start_configuration=None, group=None, options=None): """Calculates a cartesian motion path (linear in tool space). Parameters ---------- robot : :class:`compas_fab.robots.Robot` The robot instance for which the cartesian motion path is being calculated. - frames_WCF : list of :class:`compas.geometry.Frame` - The frames through which the path is defined. + waypoints : :class:`compas_fab.robots.Waypoints` + The waypoints for the robot to follow. start_configuration : :class:`compas_robots.Configuration`, optional The robot's full configuration, i.e. values for all configurable joints of the entire robot, at the starting position. group : str, optional The planning group used for calculation. options : dict, optional - Dictionary containing kwargs for arguments specific to - the client being queried. + Dictionary containing the key-value pairs that are passed to :func:`compas_fab.robots.Robot.iter_inverse_kinematics` Returns ------- @@ -42,22 +44,55 @@ def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, gro ----- This will only work with robots that have 6 revolute joints. """ - # what is the expected behaviour of that function? - # - Return all possible paths or select only the one that is closest to the start_configuration? - # - Do we use a stepsize to sample in between frames or use only the input frames? + + if isinstance(waypoints, FrameWaypoints): + return self._plan_cartesian_motion_with_frame_waypoints( + robot, waypoints, start_configuration, group, options + ) + elif isinstance(waypoints, PointAxisWaypoints): + return self._plan_cartesian_motion_with_point_axis_waypoints( + robot, waypoints, start_configuration, group, options + ) + else: + raise TypeError("Unsupported waypoints type {}".format(type(waypoints))) + + def _plan_cartesian_motion_with_frame_waypoints( + self, robot, waypoints, start_configuration=None, group=None, options=None + ): + """Calculates a cartesian motion path with frame waypoints. + + Planner behavior: + - If multiple paths are possible (i.e. due to multiple IK results), only the one that is closest to the start_configuration is returned. + - The path is checked to ensure that the joint values are continuous and that revolution values are the smallest possible. + - There is no interpolation in between frames (i.e. 'max_step' parameter is not supported), only the input frames are used. + """ + # convert the target frames to the robot's base frame + if waypoints.tool_coordinate_frame is not None: + frames_WCF = [from_tcf_to_t0cf(frame, waypoints.tool_coordinate_frame) for frame in waypoints.target_frames] + else: + frames_WCF = waypoints.target_frames # convert the frame WCF to RCF base_frame = robot.get_base_frame(group=group, full_configuration=start_configuration) frames_RCF = [base_frame.to_local_coordinates(frame_WCF) for frame_WCF in frames_WCF] + # 'keep_order' is set to True, so that iter_inverse_kinematics will return the configurations in the same order across all frames options = options or {} options.update({"keep_order": True}) + # iterate over all input frames and calculate the inverse kinematics, no interpolation in between frames configurations_along_path = [] for frame in frames_RCF: configurations = list(robot.iter_inverse_kinematics(frame, options=options)) configurations_along_path.append(configurations) + # Analytical backend only supports robots with finite IK solutions + # For 6R articulated robots, there is a maximum of 8 possible paths, corresponding to the 8 possible IK solutions for each frame + # The `options.update({"keep_order": True})` ensures that the order of the configurations is the same across all frames + # but this also cause some configurations to be None, if no solution was found. + + # The `all(configurations)` below is used to check if all configurations in a path are present. + # indicating that a complete trajectory was found. paths = [] for configurations in zip(*configurations_along_path): if all(configurations): @@ -75,11 +110,20 @@ def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, gro path = self.smooth_configurations(path) trajectory = JointTrajectory() trajectory.fraction = len(path) / len(frames_RCF) + # Technically trajectory.fraction should always be 1.0 because otherwise, the path would be rejected earlier trajectory.joint_names = path[0].joint_names trajectory.points = [JointTrajectoryPoint(config.joint_values, config.joint_types) for config in path] trajectory.start_configuration = robot.merge_group_with_full_configuration(path[0], start_configuration, group) return trajectory + def _plan_cartesian_motion_with_point_axis_waypoints( + self, robot, waypoints, start_configuration=None, group=None, options=None + ): + """Planning Cartesian motion with PointAxisWaypoints is not yet implemented in the Analytical backend.""" + raise NotImplementedError( + "Planning Cartesian motion with PointAxisWaypoints is not yet implemented in the Analytical backend." + ) + def smooth_configurations(self, configurations): joint_values_corrected = [] prev = smallest_joint_angles(configurations[0].joint_values) diff --git a/src/compas_fab/backends/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") 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( diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index 8cc68350f..f932e266e 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,23 @@ def forward_kinematics(self, configuration, group=None, use_attached_tool_frame= return frame_WCF - def plan_cartesian_motion( - self, frames_WCF, start_configuration=None, group=None, use_attached_tool_frame=True, options=None - ): + def plan_cartesian_motion(self, waypoints, start_configuration=None, group=None, options=None): """Calculate a cartesian motion path (linear in tool space). Parameters ---------- - frames_WCF : :obj:`list` of :class:`compas.geometry.Frame` - The frames through which the path is defined. + waypoints : :class:`compas_fab.robots.Waypoints` + The waypoints for the robot to follow. + For more information on how to define waypoints, see :ref:`waypoints`. + In addition, note that not all planning backends support all waypoint types, + check documentation of the backend in use for more details. start_configuration : :class:`compas_robots.Configuration`, optional The robot's full configuration, i.e. values for all configurable - joints of the entire robot, at the starting position. Defaults to - the all-zero configuration. + joints of the entire robot, at the start of the motion. + Defaults to the all-zero configuration. group : :obj:`str`, optional - The planning group used for calculation. Defaults to the robot's - main planning group. - use_attached_tool_frame : :obj:`bool`, optional - If ``True`` and there is a tool attached to the planning group, it will use its TCF - instead of the T0CF to calculate cartesian paths. Defaults to ``True``. + The name of the planning group used for motion planning. + Defaults to the robot's main planning group. options : :obj:`dict`, optional Dictionary containing the following key-value pairs: @@ -1360,42 +1359,52 @@ def plan_cartesian_motion( >>> with RosClient() as client: # doctest: +SKIP #: This doctest can pass locally but persistently fails on CI in GitHub. "roslibpy.core.RosTimeoutError: Failed to connect to ROS" ... robot = client.load_robot() - ... frames = [Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]),\ + ... target_frames = [Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]),\ Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0])] + ... waypoints = FrameWaypoints(target_frames) ... start_configuration = Configuration.from_revolute_values([-0.042, 0.033, -2.174, 5.282, -1.528, 0.000]) ... group = robot.main_group_name ... options = {'max_step': 0.01,\ 'jump_threshold': 1.57,\ 'avoid_collisions': True} - ... trajectory = robot.plan_cartesian_motion(frames,\ + ... trajectory = robot.plan_cartesian_motion(waypoints,\ start_configuration,\ group=group,\ options=options) ... len(trajectory.points) > 1 True """ + # The plan_cartesian_motion method in the Robot class is a wrapper around planing backend's + # plan_cartesian_motion method. This method is responsible for scaling the waypoints, start_configuration and the planned trajectory. + # Some options may also need scaling, like max_step. This should be removed in the future. + # TODO: Discuss if we can have all options passed with a fixed unit to avoid scaling. + # Attached tools are also managed by this function. + options = options or {} - max_step = options.get("max_step") - path_constraints = options.get("path_constraints") - attached_collision_meshes = options.get("attached_collision_meshes") or [] self.ensure_client() if not group: group = self.main_group_name # ensure semantics + # ======= + # Scaling + # ======= + need_scaling = self.scale_factor != 1.0 + + if need_scaling: + waypoints = waypoints.scaled(1.0 / self.scale_factor) + + max_step = options.get("max_step") + if need_scaling and max_step: + options["max_step"] = max_step / self.scale_factor + # NOTE: start_configuration has to be a full robot configuration, such # that all configurable joints of the whole robot are defined for planning. start_configuration, start_configuration_scaled = self._check_full_configuration_and_scale(start_configuration) - attached_tool = self.attached_tools.get(group) - if use_attached_tool_frame and attached_tool: - frames_WCF = self.from_tcf_to_t0cf(frames_WCF, group) - - frames_WCF_scaled = [] - for frame in frames_WCF: - frames_WCF_scaled.append(Frame(frame.point * 1.0 / self.scale_factor, frame.xaxis, frame.yaxis)) - - if path_constraints: + # Path constraints are only relevant to ROS Backend + path_constraints = options.get("path_constraints") + if need_scaling and path_constraints: path_constraints_WCF_scaled = [] for c in path_constraints: cp = c.copy() @@ -1406,26 +1415,36 @@ def plan_cartesian_motion( else: cp.scale(1.0 / self.scale_factor) path_constraints_WCF_scaled.append(cp) - else: - path_constraints_WCF_scaled = None + options["path_constraints"] = path_constraints_WCF_scaled + # ===================== + # Attached CM and Tools + # ===================== + + attached_collision_meshes = options.get("attached_collision_meshes") or [] + # Collect attached collision meshes from attached tools + # TODO: Discuss why scaling is not happening here? for _, tool in self.attached_tools.items(): if tool: attached_collision_meshes.extend(tool.attached_collision_meshes) - options["attached_collision_meshes"] = attached_collision_meshes - options["path_constraints"] = path_constraints - if max_step: - options["max_step"] = max_step / self.scale_factor + + # ======== + # Planning + # ======== trajectory = self.client.plan_cartesian_motion( robot=self, - frames_WCF=frames_WCF_scaled, + waypoints=waypoints, start_configuration=start_configuration_scaled, group=group, options=options, ) + # ========================= + # Scaling result trajectory + # ========================= + # Scale everything back to robot's scale for pt in trajectory.points: pt.scale(self.scale_factor) diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index 64ff1b3c6..fc08393ee 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -22,9 +22,9 @@ class Target(Data): pose, configuration, and joint constraints. Dynamic targets such as velocity, acceleration, and jerk are not yet supported. - Targets are intended to be used as arguments for the Backend's motion - planning methods. Different backends might support different types of - targets. + Targets are intended to be used for motion planning with a planning backend + by using :meth:`compas_fab.robot.plan_motion`. + Note that different backends support different types of targets. Attributes ---------- @@ -490,14 +490,16 @@ class Waypoints(Data): Waypoints are useful for tasks like painting, welding, or 3D printing, where the programmer wants to define the waypoints the robot should pass through. - Waypoints are intended to be used for motion planning with a planning backend by using :meth:`compas_fab.robot.plan_motion_with_waypoints`. - Different backends might support different types of waypoints. + Waypoints are intended to be used for motion planning with a planning backend by using :meth:`compas_fab.robot.plan_cartesian_motion`. + Note that different backends support different types of waypoints. The method of interpolation between the waypoints is controlled by the motion planner backend. Attributes ---------- name : str , optional, default = 'target' A human-readable name for identifying the target. + tool_coordinate_frame : :class:`compas.geometry.Frame` or :class:`compas.geometry.Transformation`, optional + The tool tip coordinate frame relative to the flange of the robot. See Also -------- @@ -505,13 +507,13 @@ class Waypoints(Data): :class:`FrameWaypoints` """ - def __init__(self, name="Generic Waypoints"): + def __init__(self, tool_coordinate_frame, name="Generic Waypoints"): super(Waypoints, self).__init__() self.name = name - - @property - def __data__(self): - raise NotImplementedError + # If the user provides a transformation, convert it to a Frame + if isinstance(tool_coordinate_frame, Transformation): + tool_coordinate_frame = Frame.from_transformation(tool_coordinate_frame) + self.tool_coordinate_frame = tool_coordinate_frame def scaled(self, factor): """Returns a scaled copy of the waypoints. @@ -568,13 +570,10 @@ def __init__( tool_coordinate_frame=None, name="Frame Waypoints", ): - super(FrameWaypoints, self).__init__(name=name) + super(FrameWaypoints, self).__init__(tool_coordinate_frame=tool_coordinate_frame, name=name) self.target_frames = target_frames self.tolerance_position = tolerance_position self.tolerance_orientation = tolerance_orientation - if isinstance(tool_coordinate_frame, Transformation): - tool_coordinate_frame = Frame.from_transformation(tool_coordinate_frame) - self.tool_coordinate_frame = tool_coordinate_frame @property def __data__(self): @@ -668,7 +667,7 @@ class PointAxisWaypoints(Waypoints): tolerance_position : float, optional The tolerance for the position of the target point. If not specified, the default value from the planner is used. - tool_coordinate_frame : :class:`compas.geometry.Frame`, optional + tool_coordinate_frame : :class:`compas.geometry.Frame` or :class:`compas.geometry.Transformation`, optional The tool tip coordinate frame relative to the flange coordinate frame of the robot. If not specified, the target point is relative to the robot's flange (T0CF) and the Z axis of the flange can rotate around the target axis. @@ -685,10 +684,9 @@ def __init__( tool_coordinate_frame=None, name="Point-Axis Waypoints", ): - super(PointAxisWaypoints, self).__init__(name=name) + super(PointAxisWaypoints, self).__init__(tool_coordinate_frame=tool_coordinate_frame, name=name) self.target_points_and_axes = target_points_and_axes self.tolerance_position = tolerance_position - self.tool_coordinate_frame = tool_coordinate_frame @property def __data__(self): diff --git a/tests/backends/kinematics/test_inverse_kinematics.py b/tests/backends/kinematics/test_inverse_kinematics.py index abfee5b2a..c3e27fc18 100644 --- a/tests/backends/kinematics/test_inverse_kinematics.py +++ b/tests/backends/kinematics/test_inverse_kinematics.py @@ -1,3 +1,5 @@ +import pytest + import compas from compas.geometry import Frame from compas_robots import Configuration @@ -6,6 +8,7 @@ from compas_fab.backends import AnalyticalInverseKinematics from compas_fab.robots import Tool from compas_fab.robots import RobotLibrary +from compas_fab.robots import FrameWaypoints if not compas.IPY: from compas_fab.backends import AnalyticalPyBulletClient @@ -95,10 +98,10 @@ def test_kinematics_client_with_attached_tool(): assert len(solutions) == 6 -def test_kinematics_cartesian(): - if compas.IPY: - return - frames_WCF = [ +@pytest.fixture +def frames_WCF(): + """A list of frames in the world coordinate frame for planning tests""" + return [ Frame((0.407, 0.073, 0.320), (0.922, 0.000, 0.388), (0.113, 0.956, -0.269)), Frame((0.404, 0.057, 0.324), (0.919, 0.000, 0.394), (0.090, 0.974, -0.210)), Frame((0.390, 0.064, 0.315), (0.891, 0.000, 0.454), (0.116, 0.967, -0.228)), @@ -106,11 +109,47 @@ def test_kinematics_cartesian(): Frame((0.376, 0.087, 0.299), (0.850, 0.000, 0.528), (0.184, 0.937, -0.296)), ] + +@pytest.fixture +def frame_waypoints(frames_WCF): + """A FrameWaypoints Object for planning tests""" + return FrameWaypoints(frames_WCF) + + +def test_kinematics_cartesian(frame_waypoints): + if compas.IPY: + return + + with AnalyticalPyBulletClient(connection_type="direct") as client: + robot = client.load_robot(urdf_filename) + client.load_semantics(robot, srdf_filename) + + options = {"solver": "ur5", "check_collision": True} + + trajectory = robot.plan_cartesian_motion(frame_waypoints, options=options) + + # Assert that the trajectory is complete + assert trajectory.fraction == 1.0 + # Assert that the trajectory has the correct number of points + assert len(trajectory.points) == len(frame_waypoints.target_frames) + + +def test_kinematics_cartesian_with_tool_coordinate_frame(frame_waypoints): + if compas.IPY: + return + frame_waypoints.tool_coordinate_frame = Frame([0.01, 0.02, -0.03], [1, 0, 0], [0, 1, 0]) + with AnalyticalPyBulletClient(connection_type="direct") as client: robot = client.load_robot(urdf_filename) client.load_semantics(robot, srdf_filename) options = {"solver": "ur5", "check_collision": True} - start_configuration = list(robot.iter_inverse_kinematics(frames_WCF[0], options=options))[-1] - trajectory = robot.plan_cartesian_motion(frames_WCF, start_configuration=start_configuration, options=options) + + trajectory = robot.plan_cartesian_motion(frame_waypoints, options=options) + + # Assert that the trajectory is complete assert trajectory.fraction == 1.0 + # Assert that the trajectory has the correct number of points + # NOTE: At the moment the AnalyticalPyBulletClient does not perform any interpolation between frames + # NOTE: if future implementation fixes this, the following test will not be valid anymore + assert len(trajectory.points) == len(frame_waypoints.target_frames)