From 20a3f4fc4d93a925fec5b215966bbf971118d9c4 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 22 Feb 2024 14:07:29 +0800 Subject: [PATCH 01/25] Introduce `compas_fab.robot.Target` class --- AUTHORS.rst | 1 - CHANGELOG.md | 3 + src/compas_fab/robots/__init__.py | 25 +++++ src/compas_fab/robots/targets.py | 159 ++++++++++++++++++++++++++++++ 4 files changed, 187 insertions(+), 1 deletion(-) create mode 100644 src/compas_fab/robots/targets.py diff --git a/AUTHORS.rst b/AUTHORS.rst index d1178d796..2eb6b7ac0 100644 --- a/AUTHORS.rst +++ b/AUTHORS.rst @@ -50,4 +50,3 @@ Authors * Chen Kasirer `@chenkasirer `_ * Edvard Bruun `@ebruun `_ * Victor Pok Yin Leung `@yck011522 `_ - diff --git a/CHANGELOG.md b/CHANGELOG.md index 21d3dd76b..b0557dbfd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,9 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added +* Added `compas_fab.robot.Target` class to represent a motion planning target. +* Added also child classes `FrameTarget`, `PointAxisTarget`, `ConfigurationTarget`, `ConstraintSetTarget` + ### Changed ### Removed diff --git a/src/compas_fab/robots/__init__.py b/src/compas_fab/robots/__init__.py index 457b2b924..ced9e8726 100644 --- a/src/compas_fab/robots/__init__.py +++ b/src/compas_fab/robots/__init__.py @@ -46,6 +46,19 @@ CollisionMesh PlanningScene +Targets +------- + +.. autosummary:: + :toctree: generated/ + :nosignatures: + + Target + FrameTarget + PointAxisTarget + ConfigurationTarget + ConstraintSetTarget + Constraints ----------- @@ -101,6 +114,13 @@ from .semantics import ( RobotSemantics, ) +from .targets import ( + Target, + FrameTarget, + PointAxisTarget, + ConfigurationTarget, + ConstraintSetTarget, +) from .time_ import ( Duration, ) @@ -123,20 +143,25 @@ "AttachedCollisionMesh", "BoundingVolume", "CollisionMesh", + "ConfigurationTarget", "Constraint", + "ConstraintSetTarget", "Duration", + "FrameTarget", "Inertia", "JointConstraint", "JointTrajectory", "JointTrajectoryPoint", "OrientationConstraint", "PlanningScene", + "PointAxisTarget", "PositionConstraint", "ReachabilityMap", "DeviationVectorsGenerator", "OrthonormalVectorsFromAxisGenerator", "Robot", "RobotSemantics", + "Target", "Tool", "Trajectory", "Wrench", diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py new file mode 100644 index 000000000..f3e94400b --- /dev/null +++ b/src/compas_fab/robots/targets.py @@ -0,0 +1,159 @@ +from compas.data import Data + + +class Target(Data): + """Represents a kinematic target for motion planning. + + The current implementation supports only static target constraints such as + 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. + + Attributes + ---------- + name : str , optional, default = 'target' + A human-readable name for identifying the target. + + See Also + -------- + :class:`PointAxisTarget` + :class:`FrameTarget` + :class:`ConfigurationTarget` + :class:`ConstraintSetTarget` + """ + + def __init__(self, name="Generic Target"): + super(Target, self).__init__() + self.name = name + + +class FrameTarget(Target): + """Represents a fully constrained pose target for the robot's end-effector using a :class:`compas.geometry.Frame`. + + The end-effector has no translational or rotational freedom. + + Given a FrameTarget, the planner aims to find a path moving + the robot's flange frame (or tool frame) to the specified `FrameTarget.target_frame`. + The default target frame corresponds to the Flange Coordinate Frame (RfCF) + of the robot. When using a tool, the target frame can be specified + relative to the tool tip coordinate frame (TTCF). + The planner will then move the tool tip towards the target frame. + + For robots with multiple end effector attachment points, the FCF depends on + the planning group setting in the planning request, as defined in an SRDF file or + :class:`compas_fab.robots.RobotSemantics`. + Attributes + ---------- + target_frame : :class:`compas.geometry.Frame` + The target frame. + F_TtCF : :class:`compas.geometry.Frame`, optional + The tool tip coordinate frame relative to the flange of the robot. + If not specified, the target frame is relative to the robot's flange. + + """ + + def __init__(self, target_frame, F_TtCF=None, name="Frame Target"): + super(FrameTarget, self).__init__(name=name) + self.F_TtCF = F_TtCF + self.target_frame = target_frame + + +class PointAxisTarget(Target): + """ + Represents a point and axis target for the robot's end-effector motion planning. + + This target allows one degree of rotational freedom, enabling the end-effector + to rotate around the target axis. + Given a PointAxisTarget, the planner seeks a path to move the robot's end-effector + tool tip to the target point and align the tool tip's Z axis with the specified target axis. + + PointAxisTarget is suitable for tasks like drilling, milling, and 3D printing, + where aligning the end-effector with a target axis is crucial, + but the orientation around the axis is not important. + + The user must define (1) the target point of which the tool tip will reach + and (2) the target axis where the tool tip coordinate frame (TCF)'s Z axis + can rotate around. The target point and axis are defined relative to the robot's + base coordinate frame (RCF). + + In addition, it's necessary to define the tool tip coordinate frame (TtCF) + relative to the robot's flange frame (FCF). This is labeled as F_TtCF. + If F_TtCF is unspecified, the target point and axis will be matched with the robot's flange frame. + + For robots with multiple end effector attachment points, the FCF depends on + the planning group setting in the planning request, as defined in an SRDF file or + :class:`compas_fab.robots.RobotSemantics`. + + Attributes + ---------- + target_point : :class:`compas.geometry.Point` + The target point defined in the robot's base coordinate frame (RCF). + target_z_vector : :class:`compas.geometry.Vector` + The target axis defined in the robot's base coordinate frame (RCF). + F_TtCF : :class:`compas.geometry.Frame`, 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. + + """ + + def __init__(self, target_point, target_z_vector, F_TtCF=None, name="Point-Axis Target"): + super(PointAxisTarget, self).__init__(name=name) + self.F_TtCF = F_TtCF # type: Frame + self.target_point = target_point # type: Point + self.target_z_vector = target_z_vector # type: Vector + + +class ConfigurationTarget(Target): + """Represents a configuration target for the robot's end-effector motion planning. + + The configuration target is a joint configuration of the robot's joints. + Given a ConfigurationTarget, the planner aims to find a path moving + the robot's joint positions to match with `ConfigurationTarget.target_configuration`. + + ConfigurationTarget is suitable for targets whose joint configuration is known, + such as a home position, or a repetitive position that has been calibrated + in the actual robot cell, such as a tool changing position. + + The number of joints in the target configuration should match the number of joints + in the robot's planning group. Otherwise the behaviour of the bankend planner may + be undefined. See tutorial :ref:`configuration-target` for more details. + + Attributes + ---------- + target_configuration : :class:`compas_robots.Configuration` + The target configuration. joint_names and joint_values must be specified. + """ + + def __init__(self, target_configuration, name="Configuration Target"): + super(ConfigurationTarget, self).__init__(name=name) + self.target_configuration = target_configuration # type: Configuration + + +class ConstraintSetTarget(Target): + """Represents a list of Constraint as the target for motion planning. + + Given a ConstraintSetTarget, the planner aims to find a path moving + the robot's end-effector to satisfy ALL the constraints in the constraint set. + This target cannot be translated into a single pose or configuration target + except in trivial cases. Therefore the ending pose or configuration of the + planned path can only be determined after performing the motion planning. + + ConstraintSetTarget cannot be used as a target for Cartesian motion planning. + + ConstraintSetTarget is suitable for advanced users who want to specify + custom constraints for the robot motion planning. + Different planner backends may support differnt types of Constraints. + See tutorial :ref:`constraint-set-target` for more details. + + Attributes + ---------- + constraint_set : :class:`compas_fab.robots.Constraint` + A list of constraints to be satisfied. + """ + + def __init__(self, constraint_set, name="Constraint Set Target"): + super(ConstraintSetTarget, self).__init__(name=name) + self.constraint_set = constraint_set # type: List[Constraint] From 6cc358ae4588c32f9cb10036ec34cc55c36eae60 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 8 Mar 2024 21:46:11 +0800 Subject: [PATCH 02/25] Introduce Targets for plan_motion() --- CHANGELOG.md | 14 +- .../02_description_models/03_targets.rst | 48 +++ .../03_backends_ros/files/04_plan_motion.py | 15 +- .../backends/interfaces/backend_features.py | 14 +- .../backends/ros/backend_features/helpers.py | 55 +++ .../backend_features/move_it_plan_motion.py | 22 +- .../components/Cf_ConfigurationTarget/code.py | 30 ++ .../icon.png | Bin .../metadata.json | 18 +- .../Cf_ConstraintsFromConfiguration/code.py | 31 -- .../Cf_ConstraintsFromPlane/code.py | 30 -- .../Cf_FrameTargetFromPlane/code.py | 38 ++ .../icon.png | Bin .../metadata.json | 27 +- .../ghpython/components/Cf_PlanMotion/code.py | 6 +- .../components/Cf_PointAxisTarget/code.py | 30 ++ .../components/Cf_PointAxisTarget/icon.png | Bin 0 -> 620 bytes .../Cf_PointAxisTarget/metadata.json | 37 ++ src/compas_fab/robots/constraints.py | 215 ++++++++++ src/compas_fab/robots/robot.py | 399 ++---------------- src/compas_fab/robots/targets.py | 272 +++++++++++- src/compas_fab/utilities/__init__.py | 15 +- src/compas_fab/utilities/transformation.py | 31 ++ 23 files changed, 838 insertions(+), 509 deletions(-) create mode 100644 docs/examples/02_description_models/03_targets.rst create mode 100644 src/compas_fab/ghpython/components/Cf_ConfigurationTarget/code.py rename src/compas_fab/ghpython/components/{Cf_ConstraintsFromConfiguration => Cf_ConfigurationTarget}/icon.png (100%) rename src/compas_fab/ghpython/components/{Cf_ConstraintsFromConfiguration => Cf_ConfigurationTarget}/metadata.json (69%) delete mode 100644 src/compas_fab/ghpython/components/Cf_ConstraintsFromConfiguration/code.py delete mode 100644 src/compas_fab/ghpython/components/Cf_ConstraintsFromPlane/code.py create mode 100644 src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py rename src/compas_fab/ghpython/components/{Cf_ConstraintsFromPlane => Cf_FrameTargetFromPlane}/icon.png (100%) rename src/compas_fab/ghpython/components/{Cf_ConstraintsFromPlane => Cf_FrameTargetFromPlane}/metadata.json (66%) create mode 100644 src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py create mode 100644 src/compas_fab/ghpython/components/Cf_PointAxisTarget/icon.png create mode 100644 src/compas_fab/ghpython/components/Cf_PointAxisTarget/metadata.json create mode 100644 src/compas_fab/utilities/transformation.py diff --git a/CHANGELOG.md b/CHANGELOG.md index b0557dbfd..a0e10d325 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,11 +11,23 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 * Added `compas_fab.robot.Target` class to represent a motion planning target. * Added also child classes `FrameTarget`, `PointAxisTarget`, `ConfigurationTarget`, `ConstraintSetTarget` +* Unlike previous constraints, `Targets` do not contain `group` as parameter. Instead, group parameter is passed to the planning call. +* Target scaling function is now embeded in the code for Targets. `scaled()` should be called by the `plan_motion` class and a copy if the target is returned. +* FrameTarget uses `tolerance_orientation` to specify a orientation tolerance, the values are adopted to become `tolerance_axes` when being converted to `OrientationConstraint`. ### Changed -### Removed +* 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. +* Moved `Robot.position_constraint_from_frame()` to `PositionConstraint.from_frame()`, as constraints are no longer intended for users to use directly. +* Moved `Robot.constraints_from_frame()` to ros.backend_features and is handled by `convert_target_to_goal_constraints()`. Users who wish to use a frame as target should use a `FrameTarget` instead. + +* Changed GH Component `ConstraintsFromPlane` to `FrameTargetFromPlane` +* Changed GH Component `ConstraintsFromTargetConfiguration` +### Removed +* Removed `plan_cartesian_motion_deprecated` and `plan_motion_deprecated` methods from `Robot` class +* Removed `forward_kinematics_deprecated` and `inverse_kinematics_deprecated` method from `Robot` class ## [1.0.2] 2024-02-22 diff --git a/docs/examples/02_description_models/03_targets.rst b/docs/examples/02_description_models/03_targets.rst new file mode 100644 index 000000000..d3385d40f --- /dev/null +++ b/docs/examples/02_description_models/03_targets.rst @@ -0,0 +1,48 @@ +.. _targets: + +******************************************************************************* +Targets +******************************************************************************* + +----------------------- +Single Targets (Static) +----------------------- + +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). + +The :class:`compas_fab.robot.FrameTarget` is the most common target for motion planning. +It defined the complete pose of the end-effector (or the robot flange, if no tool is attached). +The FrameTarget is commonly created from a Frame object, or alternatively from a a Transformation object. + +The :class:`compas_fab.robot.PointAxisTarget` class is used for specifying a target +based on a point and a Z-Vector. This is useful for example when the robot is using a +cylinrical tool to perform a task, for example 3D printing,welding or drilling. +The point and the vector is defined relative to the Z-axis of the tool coordinate +frame (TCF). The goal is for the tool's TCF point to coincide with the `target_point`, +and the TCF's Z-axis is parallel to the Z vector. Note that the exact orientation +of the TCF is not determined until after the target is planned. + +The :class:`compas_fab.robot.ConfigurationTarget` class is used to specify a target +based on a robot configuration. This is useful for example when the robot is +required to move to a specific joint configuration. This is useful for example +when the position of the robot is aquired by teaching (i.e. jogging). Typically, +the ConfigurationTarget should have the same number of joints as the planning group +of the robot. However, it is possible to specify a subset of the joints, in which +case the remaining joints are left unspecified. Note that ConfigurationTarget does +not take into account whether a tool is attached or not. +The pose of the end-effector and the robot flange is determined by forward kinematics. + +The :class:`compas_fab.robot.ConstraintSetTarget` class is used to specify a list of +ROS MoveIt constraints. This is intended for advanced users who want to create custom +combination of constraints. See :class:`compas_fab.robots.Constraint` for available +constraints. + +---------------------------- +Multiple Tragets (Waypoints) +---------------------------- + + + + diff --git a/docs/examples/03_backends_ros/files/04_plan_motion.py b/docs/examples/03_backends_ros/files/04_plan_motion.py index 406c5786f..79097d20e 100644 --- a/docs/examples/03_backends_ros/files/04_plan_motion.py +++ b/docs/examples/03_backends_ros/files/04_plan_motion.py @@ -3,6 +3,7 @@ from compas.geometry import Frame from compas_fab.backends import RosClient +from compas_fab.robots import FrameTarget with RosClient() as client: robot = client.load_robot() @@ -10,19 +11,19 @@ frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) tolerance_position = 0.001 - tolerance_axes = [math.radians(1)] * 3 + tolerance_orientation = math.radians(1) start_configuration = robot.zero_configuration() start_configuration.joint_values = (-3.530, 3.830, -0.580, -3.330, 4.760, 0.000) group = robot.main_group_name - # create goal constraints from frame - goal_constraints = robot.constraints_from_frame(frame, - tolerance_position, - tolerance_axes, - group) + # create target from frame + target = FrameTarget(frame, + tolerance_position, + tolerance_orientation, + ) - trajectory = robot.plan_motion(goal_constraints, + trajectory = robot.plan_motion(target, start_configuration, group, options=dict( diff --git a/src/compas_fab/backends/interfaces/backend_features.py b/src/compas_fab/backends/interfaces/backend_features.py index c25f69ae3..e0ef5bc05 100644 --- a/src/compas_fab/backends/interfaces/backend_features.py +++ b/src/compas_fab/backends/interfaces/backend_features.py @@ -87,22 +87,18 @@ class PlanMotion(object): . """ - def __call__(self, robot, goal_constraints, start_configuration=None, group=None, options=None): - return self.plan_motion(robot, goal_constraints, start_configuration, group, options) + def __call__(self, robot, target, start_configuration=None, group=None, options=None): + return self.plan_motion(robot, target, start_configuration, group, options) - def plan_motion(self, robot, goal_constraints, start_configuration=None, group=None, options=None): + def plan_motion(self, robot, target, start_configuration=None, group=None, options=None): """Calculates a motion path. Parameters ---------- robot : :class:`compas_fab.robots.Robot` The robot instance for which the motion path is being calculated. - goal_constraints: list of :class:`compas_fab.robots.Constraint` - The goal to be achieved, defined in a set of constraints. - Constraints can be very specific, for example defining value domains - for each joint, such that the goal configuration is included, - or defining a volume in space, to which a specific robot link (e.g. - the end-effector) is required to move to. + target: :class:`compas_fab.robots.Target` + The goal for the robot to achieve. start_configuration: :class:`compas_fab.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/ros/backend_features/helpers.py b/src/compas_fab/backends/ros/backend_features/helpers.py index 9fdd211b7..81284c671 100644 --- a/src/compas_fab/backends/ros/backend_features/helpers.py +++ b/src/compas_fab/backends/ros/backend_features/helpers.py @@ -12,9 +12,16 @@ from compas_fab.backends.ros.messages import MoveItErrorCodes from compas_fab.backends.ros.messages import OrientationConstraint from compas_fab.backends.ros.messages import PositionConstraint +from compas_fab.robots import ConfigurationTarget +from compas_fab.robots import ConstraintSetTarget from compas_fab.robots import Duration +from compas_fab.robots import FrameTarget from compas_fab.robots import JointTrajectory from compas_fab.robots import JointTrajectoryPoint +from compas_fab.robots import PointAxisTarget +from compas_fab.robots.constraints import JointConstraint as CF_JointConstraint +from compas_fab.robots.constraints import PositionConstraint as CF_PositionConstraint +from compas_fab.robots.constraints import OrientationConstraint as CF_OrientationConstraint def validate_response(response): @@ -23,6 +30,54 @@ def validate_response(response): raise RosError(response.error_code.human_readable, int(response.error_code)) +def convert_target_to_goal_constraints(target, ee_link_name): + """Convert COMPAS FAB `Target` objects into `Constraint` objects for passing it to MoveIt backend. + This function is intended to be called only by MoveIt backend when handeling different Target types. + + Parameters + ---------- + target: :class:`compas_fab.robots.Target` + The goal for the robot to achieve. + + Returns + ------- + list of :class:`Constraint` + Set of Constraint classes + """ + + if type(target) == ConstraintSetTarget: + return target.constraint_set + + elif type(target) == ConfigurationTarget: + configuration = target.target_configuration + tolerance_above = target.tolerance_above + tolerance_below = target.tolerance_below + return CF_JointConstraint.joint_constraints_from_configuration(configuration, tolerance_above, tolerance_below) + + elif type(target) == FrameTarget: + tcf_frame_in_wcf = target.target_frame + tool_coordinate_frame = target.tool_coordinate_frame + pc = CF_PositionConstraint.from_frame( + tcf_frame_in_wcf, target.tolerance_position, ee_link_name, tool_coordinate_frame) + oc = CF_OrientationConstraint.from_frame( + tcf_frame_in_wcf, [target.tolerance_orientation] * 3, ee_link_name, tool_coordinate_frame) + return [pc, oc] + + elif type(target) == PointAxisTarget: + tcf_point_in_wcf = target.target_point + tool_coordinate_frame = target.tool_coordinate_frame + + if tool_coordinate_frame: + raise NotImplementedError( + "Tool coordinate frame is not yet supported when converting PointAxisTarget to ConstraintSetTarget.") + + pc = CF_PositionConstraint.from_point( + tcf_point_in_wcf, target.tolerance_position, ee_link_name, tool_coordinate_frame) + oc = CF_OrientationConstraint.from_frame( + tcf_frame_in_wcf, [6.35, 6.35, 0.01], ee_link_name, tool_coordinate_frame) + return [pc, oc] + + def convert_constraints_to_rosmsg(constraints, header): """Convert COMPAS FAB constraints into ROS Messages.""" if not constraints: diff --git a/src/compas_fab/backends/ros/backend_features/move_it_plan_motion.py b/src/compas_fab/backends/ros/backend_features/move_it_plan_motion.py index d7933c891..427b072a6 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_plan_motion.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_plan_motion.py @@ -6,6 +6,7 @@ from compas_fab.backends.interfaces import PlanMotion from compas_fab.backends.ros.backend_features.helpers import convert_constraints_to_rosmsg +from compas_fab.backends.ros.backend_features.helpers import convert_target_to_goal_constraints from compas_fab.backends.ros.backend_features.helpers import convert_trajectory from compas_fab.backends.ros.backend_features.helpers import validate_response from compas_fab.backends.ros.messages import AttachedCollisionObject @@ -22,7 +23,7 @@ class MoveItPlanMotion(PlanMotion): - """Callable to find a path plan to move the selected robot from its current position within the `goal_constraints`.""" + """Callable to find a path in joint space for the robot to move from its `start_configuration` to the `target`.""" GET_MOTION_PLAN = ServiceDescription( "/plan_kinematic_path", "GetMotionPlan", MotionPlanRequest, MotionPlanResponse, validate_response @@ -31,19 +32,15 @@ class MoveItPlanMotion(PlanMotion): def __init__(self, ros_client): self.ros_client = ros_client - def plan_motion(self, robot, goal_constraints, start_configuration=None, group=None, options=None): + def plan_motion(self, robot, target, start_configuration=None, group=None, options=None): """Calculates a motion path. Parameters ---------- robot : :class:`compas_fab.robots.Robot` The robot instance for which the motion plan is being calculated. - goal_constraints: list of :class:`compas_fab.robots.Constraint` - The goal to be achieved, defined in a set of constraints. - Constraints can be very specific, for example defining value domains - for each joint, such that the goal configuration is included, - or defining a volume in space, to which a specific robot link (e.g. - the end-effector) is required to move to. + target: list of :class:`compas_fab.robots.Target` + The goal for the robot to achieve. start_configuration: :class:`compas_fab.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 +82,7 @@ def plan_motion(self, robot, goal_constraints, start_configuration=None, group=N """ options = options or {} kwargs = {} - kwargs["goal_constraints"] = goal_constraints + kwargs["target"] = target kwargs["start_configuration"] = start_configuration kwargs["group"] = group kwargs["options"] = options @@ -94,12 +91,11 @@ def plan_motion(self, robot, goal_constraints, start_configuration=None, group=N # Use base_link or fallback to model's root link options["base_link"] = options.get("base_link", robot.model.root.name) options["joints"] = {j.name: j.type for j in robot.model.joints} + options["ee_link_name"] = robot.get_end_effector_link_name(group) return await_callback(self.plan_motion_async, **kwargs) - def plan_motion_async( - self, callback, errback, goal_constraints, start_configuration=None, group=None, options=None - ): + def plan_motion_async(self, callback, errback, target, start_configuration=None, group=None, options=None): """Asynchronous handler of MoveIt motion planner service.""" # http://docs.ros.org/jade/api/moveit_core/html/utils_8cpp_source.html # TODO: if list of frames (goals) => receive multiple solutions? @@ -120,6 +116,8 @@ def plan_motion_async( start_state.filter_fields_for_distro(self.ros_client.ros_distro) # convert constraints + ee_link_name = options["ee_link_name"] + goal_constraints = convert_target_to_goal_constraints(target, ee_link_name) goal_constraints = [convert_constraints_to_rosmsg(goal_constraints, header)] path_constraints = convert_constraints_to_rosmsg(options.get("path_constraints"), header) trajectory_constraints = options.get("trajectory_constraints") diff --git a/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/code.py b/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/code.py new file mode 100644 index 000000000..79f907ca6 --- /dev/null +++ b/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/code.py @@ -0,0 +1,30 @@ +""" +Create joint constraints for each of the robot's configurable joints based on a given target configuration. + +COMPAS FAB v1.0.2 +""" + +import math + +from ghpythonlib.componentbase import executingcomponent as component + +from compas_fab.robots import ConfigurationTarget + + +class ConfigurationTargetComponent(component): + DEFAULT_TOLERANCE_METERS = 0.001 + DEFAULT_TOLERANCE_RADIANS = math.radians(1) + + def RunScript(self, robot, target_configuration, tolerances_above, tolerances_below): + if robot and target_configuration: + default_tolerances_above, default_tolerances_below = ConfigurationTarget.generate_default_tolerances( + target_configuration, self.DEFAULT_TOLERANCE_METERS, self.DEFAULT_TOLERANCE_RADIANS + ) + + target = ConfigurationTarget( + target_configuration=target_configuration, + tolerances_above=tolerances_above or default_tolerances_above, + tolerances_below=tolerances_below or default_tolerances_below, + ) + + return target diff --git a/src/compas_fab/ghpython/components/Cf_ConstraintsFromConfiguration/icon.png b/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/icon.png similarity index 100% rename from src/compas_fab/ghpython/components/Cf_ConstraintsFromConfiguration/icon.png rename to src/compas_fab/ghpython/components/Cf_ConfigurationTarget/icon.png diff --git a/src/compas_fab/ghpython/components/Cf_ConstraintsFromConfiguration/metadata.json b/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/metadata.json similarity index 69% rename from src/compas_fab/ghpython/components/Cf_ConstraintsFromConfiguration/metadata.json rename to src/compas_fab/ghpython/components/Cf_ConfigurationTarget/metadata.json index 1835341b2..b5b3dfd3e 100644 --- a/src/compas_fab/ghpython/components/Cf_ConstraintsFromConfiguration/metadata.json +++ b/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/metadata.json @@ -1,11 +1,10 @@ { - "name": "Constraints From Target Configuration", - "nickname": "Constraints From Configuration", + "name": "ConfigurationTarget From Configuration", + "nickname": "ConfigurationTarget", "category": "COMPAS FAB", "subcategory": "Planning", "description": "Create joint constraints for each of the robot's configurable joints based on a given target configuration.", "exposure": 8, - "ghpython": { "isAdvancedMode": true, "iconDisplay": 2, @@ -17,7 +16,7 @@ }, { "name": "target_configuration", - "description": "The target configuration." + "description": "The target configuration, in radians/meters." }, { "name": "tolerance_above", @@ -28,18 +27,13 @@ "name": "tolerance_below", "description": "The tolerances below the targeted joint value of each configurable joint, defining the lower bound in radians/meters to be achieved.", "typeHintID": "float" - }, - { - "name": "group", - "description": "The planning group for which we specify the constraint. Defaults to the robot's main planning group.", - "typeHintID": "str" } ], "outputParameters": [ { - "name": "constraints", - "description": "A list joint constraints in radians/meters." + "name": "target", + "description": "A ConfigurationTarget" } ] } -} +} \ No newline at end of file diff --git a/src/compas_fab/ghpython/components/Cf_ConstraintsFromConfiguration/code.py b/src/compas_fab/ghpython/components/Cf_ConstraintsFromConfiguration/code.py deleted file mode 100644 index 9c31653a3..000000000 --- a/src/compas_fab/ghpython/components/Cf_ConstraintsFromConfiguration/code.py +++ /dev/null @@ -1,31 +0,0 @@ -""" -Create joint constraints for each of the robot's configurable joints based on a given target configuration. - -COMPAS FAB v1.0.2 -""" - -import math - -from ghpythonlib.componentbase import executingcomponent as component - - -class ConstraintsFromTargetConfiguration(component): - DEFAULT_TOLERANCE_METERS = 0.001 - DEFAULT_TOLERANCE_RADIANS = math.radians(1) - - def RunScript(self, robot, target_configuration, tolerance_above, tolerance_below, group): - if robot and target_configuration: - tolerance_above = tolerance_above or self._generate_default_tolerances(robot.get_configurable_joints(group)) - tolerance_below = tolerance_below or self._generate_default_tolerances(robot.get_configurable_joints(group)) - - constraints = robot.constraints_from_configuration( - configuration=target_configuration, - tolerances_above=tolerance_above, - tolerances_below=tolerance_below, - group=group, - ) - - return constraints - - def _generate_default_tolerances(self, joints): - return [self.DEFAULT_TOLERANCE_METERS if j.is_scalable() else self.DEFAULT_TOLERANCE_RADIANS for j in joints] diff --git a/src/compas_fab/ghpython/components/Cf_ConstraintsFromPlane/code.py b/src/compas_fab/ghpython/components/Cf_ConstraintsFromPlane/code.py deleted file mode 100644 index 4c58d804f..000000000 --- a/src/compas_fab/ghpython/components/Cf_ConstraintsFromPlane/code.py +++ /dev/null @@ -1,30 +0,0 @@ -""" -Create a position and an orientation constraint from a plane calculated for the group's end-effector link. - -COMPAS FAB v1.0.2 -""" - -import math - -from compas_rhino.conversions import plane_to_compas_frame -from ghpythonlib.componentbase import executingcomponent as component - - -class ConstraintsFromPlane(component): - def RunScript(self, robot, plane, group, tolerance_position, tolerance_xaxis, tolerance_yaxis, tolerance_zaxis): - goal_constraints = None - if robot and plane: - tolerance_position = tolerance_position or 0.001 - tolerance_xaxis = tolerance_xaxis or 1.0 - tolerance_yaxis = tolerance_yaxis or 1.0 - tolerance_zaxis = tolerance_zaxis or 1.0 - - frame = plane_to_compas_frame(plane) - tolerances_axes = [ - math.radians(tolerance_xaxis), - math.radians(tolerance_yaxis), - math.radians(tolerance_zaxis), - ] - goal_constraints = robot.constraints_from_frame(frame, tolerance_position, tolerances_axes, group) - - return goal_constraints diff --git a/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py new file mode 100644 index 000000000..155212da0 --- /dev/null +++ b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py @@ -0,0 +1,38 @@ +""" +Create a position and an orientation constraint from a plane calculated for the group's end-effector link. + +COMPAS FAB v1.0.2 +""" + +import math + +from compas_rhino.conversions import plane_to_compas_frame +from ghpythonlib.componentbase import executingcomponent as component + +from compas.geometry import Frame +from compas_fab.robots import FrameTarget + + +class FrameTargetFromPlaneComponent(component): + def RunScript(self, plane, tolerance_position, tolerance_xaxis, tolerance_yaxis, tolerance_zaxis, tool_coordinate_frame): + target = None + if plane: + # Convert Rhino geometry to COMPAS geometry + frame = plane if isinstance(plane, Frame) else plane_to_compas_frame(plane) + tool_coordinate_frame = tool_coordinate_frame if isinstance( + tool_coordinate_frame, Frame) else plane_to_compas_frame(tool_coordinate_frame) + + # Tolerance values + tolerance_position = tolerance_position or 0.001 + tolerance_xaxis = tolerance_xaxis or 1.0 + tolerance_yaxis = tolerance_yaxis or 1.0 + tolerance_zaxis = tolerance_zaxis or 1.0 + tolerance_orientation = [ + math.radians(tolerance_xaxis), + math.radians(tolerance_yaxis), + math.radians(tolerance_zaxis), + ] + + target = FrameTarget(frame, tolerance_position, tolerance_orientation, tool_coordinate_frame) + + return target diff --git a/src/compas_fab/ghpython/components/Cf_ConstraintsFromPlane/icon.png b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/icon.png similarity index 100% rename from src/compas_fab/ghpython/components/Cf_ConstraintsFromPlane/icon.png rename to src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/icon.png diff --git a/src/compas_fab/ghpython/components/Cf_ConstraintsFromPlane/metadata.json b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/metadata.json similarity index 66% rename from src/compas_fab/ghpython/components/Cf_ConstraintsFromPlane/metadata.json rename to src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/metadata.json index 2e4473c5d..9b4949595 100644 --- a/src/compas_fab/ghpython/components/Cf_ConstraintsFromPlane/metadata.json +++ b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/metadata.json @@ -1,28 +1,17 @@ { - "name": "Constraints From Plane", - "nickname": "Constraints From Plane", + "name": "FrameTarget From Plane", + "nickname": "FrameTarget", "category": "COMPAS FAB", "subcategory": "Planning", "description": "Create a position and an orientation constraint from a plane calculated for the group's end-effector link.", "exposure": 8, - "ghpython": { "isAdvancedMode": true, "iconDisplay": 2, "inputParameters": [ - { - "name": "robot", - "description": "The robot.", - "wireDisplay": "hidden" - }, { "name": "plane", - "description": "The plane or frame from which we create position and orientation constraints." - }, - { - "name": "group", - "description": "The planning group for which we specify the constraint. Defaults to the robot's main planning group.", - "typeHintID": "str" + "description": "The Rhino or Grasshopper Plane or COMPAS Frame from which we create position and orientation constraints." }, { "name": "tolerance_position", @@ -43,13 +32,17 @@ "name": "tolerance_zaxis", "description": "The allowed tolerance of the frame's Z-axis in degrees.", "typeHintID": "float" + }, + { + "name": "tool_coordinate_frame", + "description": "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). Accepts Rhino / Grasshopper Frame or COMPAS Frame." } ], "outputParameters": [ { - "name": "constraints", - "description": "A list containing a position and an orientation constraint." + "name": "target", + "description": "A FrameTarget." } ] } -} +} \ No newline at end of file diff --git a/src/compas_fab/ghpython/components/Cf_PlanMotion/code.py b/src/compas_fab/ghpython/components/Cf_PlanMotion/code.py index c25f0fd25..4f81882db 100644 --- a/src/compas_fab/ghpython/components/Cf_PlanMotion/code.py +++ b/src/compas_fab/ghpython/components/Cf_PlanMotion/code.py @@ -14,7 +14,7 @@ class PlanMotion(component): def RunScript( self, robot, - goal_constraints, + target, start_configuration, group, attached_collision_meshes, @@ -33,11 +33,11 @@ def RunScript( and robot.client and robot.client.is_connected and start_configuration - and goal_constraints + and target and compute ): st[key] = robot.plan_motion( - goal_constraints, + target, start_configuration=start_configuration, group=group, options=dict( diff --git a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py new file mode 100644 index 000000000..c64ab1e9c --- /dev/null +++ b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py @@ -0,0 +1,30 @@ +""" +Create a position and an orientation constraint from a plane calculated for the group's end-effector link. + +COMPAS FAB v1.0.2 +""" + +import math + +from compas_rhino.conversions import point_to_compas +from compas_rhino.conversions import vector_to_compas +from ghpythonlib.componentbase import executingcomponent as component + +from compas.geometry import Point +from compas.geometry import Vector +from compas_fab.robots import PointAxisTarget + + +class PointAxisTargetComponent(component): + def RunScript(self, point, target_z_vector, tolerance_position, tool_coordinate_frame): + target = None + if point: + + # Convert Rhino geometry to COMPAS geometry + point = point if isinstance(point, Point) else point_to_compas(point) + target_z_vector = target_z_vector if isinstance( + target_z_vector, Vector) else vector_to_compas(target_z_vector) + + target = PointAxisTarget(point, target_z_vector, tolerance_position, tool_coordinate_frame) + + return target diff --git a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/icon.png b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..0613d36bb28ba43396ababe26de3bb32bbe036c7 GIT binary patch literal 620 zcmV-y0+aoTP)2 zFCMhL^w{+x2tDLnMC+wz^`^OO4*~5d^j289Lwl?QKT;1iX3AS8vW9HjEat(m`*z>F z-~0N_kWvc0l)fmwiArN}W4IT9s3ZW`Jp%xc*Q;j$ivVWgmMLlmh?_-60HV??fV5T@ z9~_`1>W9yuC2B)YRCmHe>$HF)8#r?AvkP01h!!T6FbkG2omeF*6ZthC!QWIeaW%~E zv_*N^qTsl4$#R_l+MY#aq5^;&0Gr!WbF15#mnp2+y8z|^R7G+6Q=*o5!~D=&&H?N( z<6Y^=jsOAYP1$|*5#Oq5Z1BiV9Aq&1eFPxAxyzhm!{~X-h7vh{F*$1%1sjFpm#t~)?$4~2{H`k{N zRoo0Vo?Oh<3eEkl^c;OC&RD{{9dExxWg`Btrt9cDSE>?>!V+fl!21B;r2e>me4Y6x z`Y-#RJ^C{+31D5DrU1ML@Rj|z(z7F7IF>MN?Yt{-rAJ+nCTobZtRb9$Zf6al0(zeH ztRdE#-?}9lH!#Np3;AEv&%HZ6h$@yanPl7_L)V@4RXssP&5O6L^op?nbU&EAekLuy zc=DKNx6We$1c3OiU}p^EkFzU19_PCf_mm7Y)ef~$3jYB=lVSJb+G6kk0000>> from compas_fab.robots.ur5 import Robot + >>> robot = Robot() + >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) + >>> tolerances_orientation = [math.radians(1)] * 3 + >>> group = robot.main_group_name + >>> ee_link_name = robot.get_end_effector_link_name(group) + >>> OrientationConstraint.from_frame(frame, tolerances_orientation, ee_link_name) + OrientationConstraint('ee_link', [0.5, 0.5, 0.5, 0.5], [0.017453292519943295, 0.017453292519943295, 0.017453292519943295], 1.0) + """ + + if tool_coordinate_frame: + frame_WCF = from_tcf_to_t0cf(frame_WCF, tool_coordinate_frame) + + tolerances_orientation = list(tolerances_orientation) + if len(tolerances_orientation) == 1: + tolerances_orientation *= 3 + elif len(tolerances_orientation) != 3: + raise ValueError("`tolerances_orientation` must be a list with either 1 or 3 values") + + return cls(link_name, frame_WCF.quaternion, tolerances_orientation, weight) + class PositionConstraint(Constraint): """Constrains a link to be within a certain bounding volume. @@ -447,6 +588,55 @@ def __init__(self, link_name, bounding_volume, weight=1.0): self.bounding_volume = bounding_volume self.weight = weight + @classmethod + def from_frame(cls, frame_WCF, tolerance_position, link_name, tool_coordinate_frame=None, weight=1.0): + """Create a PositionConstraint from a frame on the group's end-effector link. + Only the position of the frame is considered for the constraint. + + Parameters + ---------- + frame_WCF : :class:`compas.geometry.Frame` + The frame from which we create position and orientation constraints. + tolerance_position : :obj:`float` + The allowed tolerance to the frame's position (defined in the + robot's units). + link_name : :obj:`str` + The name of the end-effector link. Necessary for creating the position constraint. + tool_coordinate_frame : :class:`compas.geometry.Frame`, optional + The tool tip coordinate frame relative to the flange of the robot. + If not specified, the target frame is relative to the robot's flange. + weight : :obj:`float` + A weighting factor for this constraint. Denotes relative importance to + other constraints. Closer to zero means less important. Defaults to ``1``. + + Returns + ------- + :class:`PositionConstraint` + + See Also + -------- + :meth:`PositionConstraint.from_box` + :meth:`PositionConstraint.from_mesh` + :meth:`PositionConstraint.from_sphere` + + Examples + -------- + >>> from compas_fab.robots.ur5 import Robot + >>> robot = Robot() + >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) + >>> tolerance_position = 0.001 + >>> group = robot.main_group_name + >>> ee_link_name = robot.get_end_effector_link_name(group) + >>> PositionConstraint.from_frame(frame, tolerance_position, ee_link_name) # doctest: +SKIP + PositionConstraint('ee_link', BoundingVolume(2, Sphere(Point(0.400, 0.300, 0.400), 0.001)), 1.0) # doctest: +SKIP + """ + + if tool_coordinate_frame: + frame_WCF = from_tcf_to_t0cf(frame_WCF, tool_coordinate_frame) + + sphere = Sphere(radius=tolerance_position, point=frame_WCF.point) + return cls.from_sphere(link_name, sphere, weight) + @classmethod def from_box(cls, link_name, box, weight=1.0): """Create a :class:`PositionConstraint` from a :class:`compas.geometry.Box`. @@ -503,6 +693,31 @@ def from_sphere(cls, link_name, sphere, weight=1.0): bounding_volume = BoundingVolume.from_sphere(sphere) return cls(link_name, bounding_volume, weight) + @classmethod + def from_point(cls, link_name, point, tolerance_position, weight=1.0): + """Create a :class:`PositionConstraint` from a point. + + Parameters + ---------- + link_name : :obj:`str` + The name of the link this contraint refers to. + point : :class:`compas.geometry.Point` + Point defining the bounding volume this constraint refers to. + tolerance_position : :obj:`float` + The allowed tolerance to the point's position (defined in the + robot's units). + weight : :obj:`float` + A weighting factor for this constraint. Denotes relative importance to + other constraints. Closer to zero means less important. Defaults to ``1``. + + Returns + ------- + :class:`PositionConstraint` + + """ + sphere = Sphere(radius=tolerance_position, point=point) + return cls.from_sphere(link_name, sphere, weight) + @classmethod def from_mesh(cls, link_name, mesh, weight=1.0): """Create a class:`PositionConstraint` from a :class:`compas.datastructures.Mesh`. diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index 68152d951..68c20ebd1 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -669,19 +669,20 @@ def _check_full_configuration_and_scale(self, full_configuration=None): return configuration, configuration.scaled(1.0 / self.scale_factor) def get_configuration_from_group_state(self, group, group_state): - """Get a ``Configuration`` from a group's group state. + """Get the Configuration specified by a planning group's group state. Parameters ---------- group : :obj:`str` The name of the planning group. - group_state : :obj:`str - The name of the ``group_state``. + group_state : :obj:`str` + The name of the `group_state`. Returns ------- - :class:`Configuration` - The configuration specified by the ``group_state``. + :class:`compas_robots.Configuration` + The configuration of the group. + """ joint_dict = self.group_states[group][group_state] group_joint_names = self.get_configurable_joint_names(group) @@ -982,264 +983,6 @@ def ensure_geometry(self): """ self.model.ensure_geometry() - # ========================================================================== - # constraints - # ========================================================================== - - def orientation_constraint_from_frame(self, frame_WCF, tolerances_axes, group=None, use_attached_tool_frame=True): - r"""Create an orientation constraint from a frame on the group's end-effector link. - - Parameters - ---------- - frame_WCF: :class:`compas.geometry.Frame` - The frame from which we create the orientation constraint. - tolerances_axes: :obj:`list` of :obj:`float` - Error tolerances t\ :sub:`i` for each of the frame's axes in - radians. If only one value is passed it will be uses for all 3 axes. - group: :obj:`str`, optional - The planning group for which we specify the constraint. 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 create the constraints. Defaults to ``True``. - - Returns - ------- - :class:`OrientationConstraint` - - Raises - ------ - :exc:`ValueError` - If tolerance axes given are not one or three values. - - Notes - ----- - The rotation tolerance for an axis is defined by the other vector - component values for rotation around corresponding axis. - If you specify the tolerances_axes vector with ``[0.01, 0.01, 6.3]``, it - means that the frame's x-axis and y-axis are allowed to rotate about the - z-axis by an angle of 6.3 radians, whereas the z-axis would only rotate - by 0.01. - - - Examples - -------- - >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) - >>> tolerances_axes = [math.radians(1)] * 3 - >>> group = robot.main_group_name - >>> robot.orientation_constraint_from_frame(frame, tolerances_axes, group=group) - OrientationConstraint('ee_link', [0.5, 0.5, 0.5, 0.5], [0.017453292519943295, 0.017453292519943295, 0.017453292519943295], 1.0) - """ - - attached_tool = self.attached_tools.get(group) - if use_attached_tool_frame and attached_tool: - frame_WCF = self.from_tcf_to_t0cf([frame_WCF], group)[0] - - ee_link = self.get_end_effector_link_name(group) - - tolerances_axes = list(tolerances_axes) - if len(tolerances_axes) == 1: - tolerances_axes *= 3 - elif len(tolerances_axes) != 3: - raise ValueError("Must give either one or 3 values") - - return OrientationConstraint(ee_link, frame_WCF.quaternion, tolerances_axes) - - def position_constraint_from_frame(self, frame_WCF, tolerance_position, group=None, use_attached_tool_frame=True): - """Create a position constraint from a frame on the group's end-effector link. - - Parameters - ---------- - frame_WCF : :class:`compas.geometry.Frame` - The frame from which we create position and orientation constraints. - tolerance_position : :obj:`float` - The allowed tolerance to the frame's position (defined in the - robot's units). - group: :obj:`str`, optional - The planning group for which we specify the constraint. 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 create the constraints. Defaults to ``True``. - - Returns - ------- - :class:`PositionConstraint` - - See Also - -------- - :meth:`PositionConstraint.from_box` - :meth:`PositionConstraint.from_mesh` - :meth:`PositionConstraint.from_sphere` - - Notes - ----- - The rotation tolerance for an axis is defined by the other vector - component values for rotation around corresponding axis. - If you specify the tolerances_axes vector with ``[0.01, 0.01, 6.3]``, it - means that the frame's x-axis and y-axis are allowed to rotate about the - z-axis by an angle of 6.3 radians, whereas the z-axis would only rotate - by 0.01. - - Examples - -------- - >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) - >>> tolerance_position = 0.001 - >>> robot.position_constraint_from_frame(frame, tolerance_position) # doctest: +SKIP - PositionConstraint('ee_link', BoundingVolume(2, Sphere(Point(0.400, 0.300, 0.400), 0.001)), 1.0) # doctest: +SKIP - """ - - attached_tool = self.attached_tools.get(group) - if use_attached_tool_frame and attached_tool: - frame_WCF = self.from_tcf_to_t0cf([frame_WCF], group)[0] - - ee_link = self.get_end_effector_link_name(group) - sphere = Sphere(radius=tolerance_position, point=frame_WCF.point) - return PositionConstraint.from_sphere(ee_link, sphere) - - def constraints_from_frame( - self, frame_WCF, tolerance_position, tolerances_axes, group=None, use_attached_tool_frame=True - ): - r"""Create a position and an orientation constraint from a frame calculated for the group's end-effector link. - - Parameters - ---------- - frame_WCF: :class:`compas.geometry.Frame` - The frame from which we create position and orientation constraints. - tolerance_position: :obj:`float` - The allowed tolerance to the frame's position (defined in the - robot's units). - tolerances_axes: :obj:`list` of :obj:`float` - Error tolerances t\ :sub:`i` for each of the frame's axes in - radians. If only one value is passed it will be uses for all 3 axes. - group: :obj:`str`, optional - The planning group for which we specify the constraint. 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 create the constraints. Defaults to ``True``. - - Returns - ------- - :obj:`list` of :class:`Constraint` - - See Also - -------- - :meth:`PositionConstraint.from_box` - :meth:`PositionConstraint.from_mesh` - :meth:`PositionConstraint.from_sphere` - :meth:`orientation_constraint_from_frame` - - Notes - ----- - The rotation tolerance for an axis is defined by the other vector - component values for rotation around corresponding axis. - If you specify the tolerances_axes vector with ``[0.01, 0.01, 6.3]``, it - means that the frame's x-axis and y-axis are allowed to rotate about the - z-axis by an angle of 6.3 radians, whereas the z-axis would only rotate - by 0.01. - - - Examples - -------- - >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) - >>> tolerance_position = 0.001 - >>> tolerances_axes = [math.radians(1)] - >>> group = robot.main_group_name - >>> robot.constraints_from_frame(frame, tolerance_position, tolerances_axes, group) # doctest: +SKIP - [PositionConstraint('ee_link', BoundingVolume(2, Sphere(Point(0.400, 0.300, 0.400), 0.001)), 1.0), # doctest: +SKIP - OrientationConstraint('ee_link', [0.5, 0.5, 0.5, 0.5], [0.017453292519943295, 0.017453292519943295, 0.017453292519943295], 1.0)] # doctest: +SKIP - """ - pc = self.position_constraint_from_frame(frame_WCF, tolerance_position, group, use_attached_tool_frame) - oc = self.orientation_constraint_from_frame(frame_WCF, tolerances_axes, group, use_attached_tool_frame) - return [pc, oc] - - def constraints_from_configuration(self, configuration, tolerances_above, tolerances_below, group=None): - """Create joint constraints for all joints of the configuration. - - Parameters - ---------- - configuration: :class:`Configuration` - The target configuration. - tolerances_above: :obj:`list` of :obj:`float` - The tolerances above the targeted configuration's joint value on - each of the joints, defining the upper bound in radians to be - achieved. If only one value is passed, it will be used to create - upper bounds for all joint constraints. - tolerances_below: :obj:`list` of :obj:`float` - The tolerances below the targeted configuration's joint value on - each of the joints, defining the upper bound in radians to be - achieved. If only one value is passed, it will be used to create - lower bounds for all joint constraints. - group: :obj:`str`, optional - The planning group for which we specify the constraint. Defaults to - the robot's main planning group. - - Returns - ------- - :obj:`list` of :class:`JointConstraint` - - Raises - ------ - :exc:`ValueError` - If the passed configuration does not correspond to the group. - :exc:`ValueError` - If the passed list of tolerance values have a different length than - the configuration. - - Notes - ----- - Make sure that you are using the correct tolerance units if your robot - has different joint types defined. - - Examples - -------- - >>> configuration = Configuration.from_revolute_values([-0.042, 4.295, -4.110, -3.327, 4.755, 0.]) - >>> tolerances_above = [math.radians(1)] * 6 - >>> tolerances_below = [math.radians(1)] * 6 - >>> group = robot.main_group_name - >>> robot.constraints_from_configuration(configuration, tolerances_above, tolerances_below, group) - [JointConstraint('shoulder_pan_joint', -0.042, 0.017453292519943295, 0.017453292519943295, 1.0), \ - JointConstraint('shoulder_lift_joint', 4.295, 0.017453292519943295, 0.017453292519943295, 1.0), \ - JointConstraint('elbow_joint', -4.11, 0.017453292519943295, 0.017453292519943295, 1.0), \ - JointConstraint('wrist_1_joint', -3.327, 0.017453292519943295, 0.017453292519943295, 1.0), \ - JointConstraint('wrist_2_joint', 4.755, 0.017453292519943295, 0.017453292519943295, 1.0), \ - JointConstraint('wrist_3_joint', 0.0, 0.017453292519943295, 0.017453292519943295, 1.0)] - """ - if not group: - group = self.main_group_name - - joint_names = self.get_configurable_joint_names(group) - if len(joint_names) != len(configuration.joint_values): - raise ValueError( - "The passed configuration has {} joint_values, the group {} needs however: {}".format( - len(configuration.joint_values), group, len(joint_names) - ) - ) - if len(tolerances_above) == 1: - tolerances_above = tolerances_above * len(joint_names) - elif len(tolerances_above) != len(configuration.joint_values): - raise ValueError( - "The passed configuration has {} joint_values, the tolerances_above however: {}".format( - len(configuration.joint_values), len(tolerances_above) - ) - ) - if len(tolerances_below) == 1: - tolerances_below = tolerances_below * len(joint_names) - elif len(tolerances_below) != len(configuration.joint_values): - raise ValueError( - "The passed configuration has {} joint_values, the tolerances_below however: {}".format( - len(configuration.joint_values), len(tolerances_below) - ) - ) - - constraints = [] - for name, value, tolerance_above, tolerance_below in zip( - joint_names, configuration.joint_values, tolerances_above, tolerances_below - ): - constraints.append(JointConstraint(name, value, tolerance_above, tolerance_below)) - return constraints - # ========================================================================== # services # ========================================================================== @@ -1423,30 +1166,6 @@ def _build_configuration(self, joint_positions, joint_names, group, return_full_ return configuration.scaled(self.scale_factor) - def inverse_kinematics_deprecated( - self, - frame_WCF, - start_configuration=None, - group=None, - avoid_collisions=True, - constraints=None, - attempts=8, - attached_collision_meshes=None, - return_full_configuration=False, - ): - return self.inverse_kinematics( - frame_WCF, - start_configuration, - group, - return_full_configuration, - options=dict( - avoid_collisions=avoid_collisions, - constraints=constraints, - attempts=attempts, - attached_collision_meshes=attached_collision_meshes, - ), - ) - def forward_kinematics(self, configuration, group=None, use_attached_tool_frame=True, options=None): """Calculate the robot's forward kinematic. @@ -1537,9 +1256,6 @@ def forward_kinematics(self, configuration, group=None, use_attached_tool_frame= return frame_WCF - def forward_kinematics_deprecated(self, configuration, group=None, backend=None, ee_link=None): - return self.forward_kinematics(configuration, group, options=dict(solver=backend, link=ee_link)) - def plan_cartesian_motion( self, frames_WCF, start_configuration=None, group=None, use_attached_tool_frame=True, options=None ): @@ -1662,41 +1378,14 @@ def plan_cartesian_motion( return trajectory - def plan_cartesian_motion_deprecated( - self, - frames_WCF, - start_configuration=None, - max_step=0.01, - jump_threshold=1.57, - avoid_collisions=True, - group=None, - path_constraints=None, - attached_collision_meshes=None, - ): - return self.plan_cartesian_motion( - frames_WCF, - start_configuration=start_configuration, - group=group, - options=dict( - max_step=max_step, - jump_threshold=jump_threshold, - avoid_collisions=avoid_collisions, - path_constraints=path_constraints, - attached_collision_meshes=attached_collision_meshes, - ), - ) - - def plan_motion(self, goal_constraints, start_configuration=None, group=None, options=None): + def plan_motion(self, target, start_configuration=None, group=None, options=None): """Calculate a motion path. Parameters ---------- - goal_constraints : list of :class:`Constraint` - The goal to be achieved, defined in a set of constraints. - Constraints can be very specific, for example defining value domains - for each joint, such that the goal configuration is included, - or defining a volume in space, to which a specific robot link (e.g. - the end-effector) is required to move to. + target : class:`Target` + The goal to be achieved by the robot at the end of the motion. + See :ref: start_configuration : :class:`.Configuration`, optional The robot's full configuration, i.e. values for all configurable joints of the entire robot, at the starting position. Defaults to @@ -1728,16 +1417,17 @@ def plan_motion(self, goal_constraints, start_configuration=None, group=None, op Using position and orientation constraints: + >>> from compas_fab.robots import FrameTarget >>> ros = RosClient() >>> ros.run() >>> robot = ros.load_robot() >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) >>> tolerance_position = 0.001 - >>> tolerances_axes = [math.radians(1)] * 3 + >>> tolerance_orientation = math.radians(1) >>> start_configuration = Configuration.from_revolute_values([-0.042, 4.295, 0, -3.327, 4.755, 0.]) >>> group = robot.main_group_name - >>> goal_constraints = robot.constraints_from_frame(frame, tolerance_position, tolerances_axes, group) - >>> trajectory = robot.plan_motion(goal_constraints, start_configuration, group, {'planner_id': 'RRTConnect'}) + >>> target = FrameTarget(frame, tolerance_position, tolerance_orientation) + >>> trajectory = robot.plan_motion(target, start_configuration, group, {'planner_id': 'RRTConnect'}) >>> trajectory.fraction 1.0 >>> len(trajectory.points) > 1 @@ -1746,15 +1436,17 @@ def plan_motion(self, goal_constraints, start_configuration=None, group=None, op Using joint constraints (to the UP configuration): + >>> from compas_fab.robots import ConfigurationTarget >>> ros = RosClient() >>> ros.run() >>> robot = ros.load_robot() - >>> configuration = Configuration.from_revolute_values([0.0, -1.5707, 0.0, -1.5707, 0.0, 0.0]) + >>> joint_names = robot.get_configurable_joint_names() + >>> configuration = Configuration.from_revolute_values([0.0, -1.5707, 0.0, -1.5707, 0.0, 0.0], joint_names) >>> tolerances_above = [math.radians(5)] * len(configuration.joint_values) >>> tolerances_below = [math.radians(5)] * len(configuration.joint_values) >>> group = robot.main_group_name - >>> goal_constraints = robot.constraints_from_configuration(configuration, tolerances_above, tolerances_below, group) - >>> trajectory = robot.plan_motion(goal_constraints, start_configuration, group, {'planner_id': 'RRTConnect'}) + >>> target = ConfigurationTarget(configuration, tolerances_above, tolerances_below) + >>> trajectory = robot.plan_motion(target, start_configuration, group, {'planner_id': 'RRTConnect'}) >>> trajectory.fraction 1.0 >>> len(trajectory.points) > 1 @@ -1780,16 +1472,19 @@ def plan_motion(self, goal_constraints, start_configuration=None, group=None, op # 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) - goal_constraints_WCF_scaled = [] - for c in goal_constraints: - cp = c.copy() - if c.type == Constraint.JOINT: - joint = self.get_joint_by_name(c.joint_name) - if joint.is_scalable(): - cp.scale(1.0 / self.scale_factor) - else: - cp.scale(1.0 / self.scale_factor) - goal_constraints_WCF_scaled.append(cp) + # goal_constraints_WCF_scaled = [] + # for c in goal_constraints: + # cp = c.copy() + # if c.type == Constraint.JOINT: + # joint = self.get_joint_by_name(c.joint_name) + # if joint.is_scalable(): + # cp.scale(1.0 / self.scale_factor) + # else: + # cp.scale(1.0 / self.scale_factor) + # goal_constraints_WCF_scaled.append(cp) + + # Scale Target Definitions + target = target.scaled(1.0 / self.scale_factor) # Transform path constraints to RCF and scale if path_constraints: @@ -1815,7 +1510,7 @@ def plan_motion(self, goal_constraints, start_configuration=None, group=None, op trajectory = self.client.plan_motion( robot=self, - goal_constraints=goal_constraints_WCF_scaled, + target=target, start_configuration=start_configuration_scaled, group=group, options=options, @@ -1829,34 +1524,6 @@ def plan_motion(self, goal_constraints, start_configuration=None, group=None, op return trajectory - def plan_motion_deprecated( - self, - goal_constraints, - start_configuration=None, - group=None, - path_constraints=None, - planner_id="RRTConnect", - num_planning_attempts=1, - allowed_planning_time=2.0, - max_velocity_scaling_factor=1.0, - max_acceleration_scaling_factor=1.0, - attached_collision_meshes=None, - ): - return self.plan_motion( - goal_constraints, - start_configuration, - group, - options=dict( - path_constraints=path_constraints, - planner_id=planner_id, - num_planning_attempts=num_planning_attempts, - allowed_planning_time=allowed_planning_time, - max_velocity_scaling_factor=max_velocity_scaling_factor, - max_acceleration_scaling_factor=max_acceleration_scaling_factor, - attached_collision_meshes=attached_collision_meshes, - ), - ) - def transformed_frames(self, configuration, group=None): """Get the robot's transformed frames. diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index f3e94400b..731bec64d 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -1,4 +1,11 @@ from compas.data import Data +from compas.geometry import Frame +from compas_robots import Configuration +from compas_robots.model import Joint + +from compas_fab.robots import JointConstraint +from compas_fab.robots import OrientationConstraint +from compas_fab.robots import PositionConstraint class Target(Data): @@ -29,6 +36,21 @@ def __init__(self, name="Generic Target"): super(Target, self).__init__() self.name = name + def scaled(self, factor): + """Returns a scaled copy of the target. + + Parameters + ---------- + factor : float + The scaling factor. + + Returns + ------- + :class:`Target` + The scaled target. + """ + raise NotImplementedError + class FrameTarget(Target): """Represents a fully constrained pose target for the robot's end-effector using a :class:`compas.geometry.Frame`. @@ -45,20 +67,84 @@ class FrameTarget(Target): For robots with multiple end effector attachment points, the FCF depends on the planning group setting in the planning request, as defined in an SRDF file or :class:`compas_fab.robots.RobotSemantics`. + Attributes ---------- target_frame : :class:`compas.geometry.Frame` The target frame. - F_TtCF : :class:`compas.geometry.Frame`, optional + tolerance_position : float, optional + The tolerance for the position. + If not specified, the default value from the planner is used. + tolerance_orientation : float, optional + The tolerance for the orientation. + If not specified, the default value from the planner is used. + tool_coordinate_frame : :class:`compas.geometry.Frame`, optional The tool tip coordinate frame relative to the flange of the robot. If not specified, the target frame is relative to the robot's flange. + name : str, optional + The name of the target. + Defaults to 'Frame Target'. """ - def __init__(self, target_frame, F_TtCF=None, name="Frame Target"): + def __init__(self, target_frame, tolerance_position=None, tolerance_orientation=None, tool_coordinate_frame=None, name="Frame Target"): super(FrameTarget, self).__init__(name=name) - self.F_TtCF = F_TtCF self.target_frame = target_frame + self.tolerance_position = tolerance_position + self.tolerance_orientation = tolerance_orientation + self.tool_coordinate_frame = tool_coordinate_frame + + @classmethod + def from_transformation(cls, transformation, tolerance_position=None, tolerance_orientation=None, tool_coordinate_frame=None, name="Frame Target"): + """Creates a FrameTarget from a transformation matrix. + + Parameters + ---------- + transformation : :class + The transformation matrix. + tolerance_position : float, optional + The tolerance for the position. + if not specified, the default value from the planner is used. + tolerance_orientation : float, optional + The tolerance for the orientation. + if not specified, the default value from the planner is used. + tool_coordinate_frame : :class:`compas.geometry.Frame`, optional + The tool tip coordinate frame relative to the flange of the robot. + If not specified, the target frame is relative to the robot's flange. + name : str, optional + The name of the target. + Defaults to 'Frame Target'. + + Returns + ------- + :class:`FrameTarget` + The frame target. + """ + frame = Frame.from_transformation(transformation) + return cls(frame, tolerance_position, tolerance_orientation, tool_coordinate_frame, name) + + def scaled(self, factor): + """Returns a copy of the target where the target frame and tolerances are scaled. + + By convention, compas_fab robots use meters as the default unit of measure. + If user model is created in millimeters, the FrameTarget should be scaled by a factor + of 0.001 before passing to the planner. + + Parameters + ---------- + factor : float + The scaling factor. + + Returns + ------- + :class:`FrameTarget` + The scaled frame target. + """ + target_frame = self.target_frame.scaled(factor) + tolerance_position = self.tolerance_position * factor + tolerance_orientation = self.tolerance_orientation * factor + 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) class PointAxisTarget(Target): @@ -80,8 +166,8 @@ class PointAxisTarget(Target): base coordinate frame (RCF). In addition, it's necessary to define the tool tip coordinate frame (TtCF) - relative to the robot's flange frame (FCF). This is labeled as F_TtCF. - If F_TtCF is unspecified, the target point and axis will be matched with the robot's flange frame. + relative to the robot's flange frame (FCF). This is labeled as tool_coordinate_frame. + If tool_coordinate_frame is unspecified, the target point and axis will be matched with the robot's flange frame. For robots with multiple end effector attachment points, the FCF depends on the planning group setting in the planning request, as defined in an SRDF file or @@ -90,20 +176,45 @@ class PointAxisTarget(Target): Attributes ---------- target_point : :class:`compas.geometry.Point` - The target point defined in the robot's base coordinate frame (RCF). + The target point defined relative to the robot's base coordinate frame (RCF). target_z_vector : :class:`compas.geometry.Vector` - The target axis defined in the robot's base coordinate frame (RCF). - F_TtCF : :class:`compas.geometry.Frame`, optional + The target axis is defined by the target_point and pointing towards this vector. + The tool tip coordinate frame (TCF)'s Z axis can rotate around this axis. + 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 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. + 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. """ - def __init__(self, target_point, target_z_vector, F_TtCF=None, name="Point-Axis Target"): + def __init__(self, target_point, target_z_vector, tolerance_position=None, tool_coordinate_frame=None, name="Point-Axis Target"): super(PointAxisTarget, self).__init__(name=name) - self.F_TtCF = F_TtCF # type: Frame - self.target_point = target_point # type: Point - self.target_z_vector = target_z_vector # type: Vector + self.target_point = target_point + self.target_z_vector = target_z_vector + self.tolerance_position = tolerance_position + self.tool_coordinate_frame = tool_coordinate_frame + + def scaled(self, factor): + """Returns a copy of the target where the target point and tolerances are scaled. + + Parameters + ---------- + factor : float + The scaling factor. + + Returns + ------- + :class:`PointAxisTarget` + The scaled point-axis target. + """ + target_point = self.target_point.scaled(factor) + tolerance_position = self.tolerance_position * factor if self.tolerance_position else None + target_z_vector = self.target_z_vector.scaled # Vector is unitized and is not scaled + tool_coordinate_frame = self.tool_coordinate_frame.scaled(factor) if self.tool_coordinate_frame else None + return PointAxisTarget(target_point, target_z_vector, tool_coordinate_frame, tolerance_position, self.name) class ConfigurationTarget(Target): @@ -119,17 +230,122 @@ class ConfigurationTarget(Target): The number of joints in the target configuration should match the number of joints in the robot's planning group. Otherwise the behaviour of the bankend planner may - be undefined. See tutorial :ref:`configuration-target` for more details. + be undefined. See tutorial :ref:`targets` for more details. Attributes ---------- target_configuration : :class:`compas_robots.Configuration` The target configuration. joint_names and joint_values must be specified. + Defaults unit is radians for revolute and continuous joints, and meters for prismatic joints. + tolerance_above : :obj:`list` of :obj:`float`, optional + Acceptable deviation above the targeted configurations. One for each joint. + Always use positive values. + If not specified, the default value from the planner is used. + tolerance_below : :obj:`list` of :obj:`float`, optional + Acceptable deviation below the targeted configurations. One for each joint. + Always use positive values. + If not specified, the default value from the planner is used. """ + SUPPORTED_JOINT_TYPES = [Joint.PRISMATIC, Joint.REVOLUTE, Joint.CONTINUOUS] + + def __init__(self, target_configuration, tolerance_above=None, tolerance_below=None, name="Configuration Target"): - def __init__(self, target_configuration, name="Configuration Target"): super(ConfigurationTarget, self).__init__(name=name) self.target_configuration = target_configuration # type: Configuration + self.tolerance_above = tolerance_above + self.tolerance_below = tolerance_below + + # Check to make sure the joint types are supported + for joint_type in target_configuration.joint_types: + assert joint_type in self.SUPPORTED_JOINT_TYPES, "Unsupported joint type: {}".format(joint_type) + + @classmethod + def generate_default_tolerances(cls, configuration, tolerance_prismatic, tolerance_revolute): + """ Generates tolerances values for the target configuration based on the joint types. + + The parameters `tolerance_prismatic` and `tolerance_revolute` are used to generate the + list of values for `tolerances_above`, `tolerances_below`. The length of the list is equal to the + number of joints in the target configuration. + + This function will not override the existing `tolerance_above` and `tolerance_below` values, + users should set the values explicitly. + + Parameters + ---------- + tolerance_prismatic : obj:`float` + The default tolerance applied to prismatic joints. + Defaults unit is meters. + tolerance_revolute : obj:`float` + The default tolerance applied to revolute and continuous joints. + Defaults unit is radians. + + Returns + ------- + :obj:`tuple` of :obj:`list` of :obj:`float` + The tolerances_above and tolerances_below lists. + + Examples + -------- + >>> from compas_robots import Configuration + >>> from compas_fab.robots import ConfigurationTarget + >>> from compas_robots.model import Joint + >>> configuration = Configuration.from_revolute_values([0, 3.14, 0, 0, 3.14, 0]) + >>> tolerance_prismatic = 0.001 + >>> tolerance_revolute = math.radians(1) + >>> tolerances_above, tolerances_below = ConfigurationTarget.generate_default_tolerances(configuration, tolerance_prismatic, tolerance_revolute) + >>> target = ConfigurationTarget(configuration, tolerances_above, tolerances_below) + + """ + tolerances_above = [] + tolerances_below = [] + for joint_type in configuration.joint_types: + if joint_type in [Joint.PRISMATIC]: + tolerances_above.append(tolerance_prismatic) + tolerances_below.append(tolerance_prismatic) + elif joint_type in [Joint.REVOLUTE, Joint.CONTINUOUS]: + tolerances_above.append(tolerance_revolute) + tolerances_below.append(tolerance_revolute) + else: + raise NotImplementedError("Unsupported joint type: {}".format(joint_type)) + return tolerances_above, tolerances_below + + def scaled(self, factor): + """Returns copy of the target where the target configuration and tolerances are scaled. + + This function should only be needed if the ConfigurationTarget was created + with a distance unit other than meters. + + Only the values for prismatic and planar joints are scaled. The values for revolute + and continuous joints are not scaled, as they must be in radians. + + Parameters + ---------- + factor : float + The scaling factor. + + Returns + ------- + :class:`ConfigurationTarget` + The scaled configuration target. + """ + target_configuration = self.target_configuration.scaled(factor) + + def scale_tolerance(tolerance, joint_types): + scaled_tolerance = [] + for t, joint_type in zip(tolerance, joint_types): + if joint_type in (Joint.PLANAR, Joint.PRISMATIC): + t *= factor + scaled_tolerance.append(t) + return scaled_tolerance + + # We scale only the tolerances for prismatic and planar joints, + # similar to the Configuration.scale() method + tolerance_above = scale_tolerance( + self.tolerance_above, target_configuration.joint_types) if self.tolerance_above else None + tolerance_below = scale_tolerance( + self.tolerance_below, target_configuration.joint_types) if self.tolerance_below else None + + return ConfigurationTarget(target_configuration, tolerance_above, tolerance_below, self.name) class ConstraintSetTarget(Target): @@ -137,23 +353,39 @@ class ConstraintSetTarget(Target): Given a ConstraintSetTarget, the planner aims to find a path moving the robot's end-effector to satisfy ALL the constraints in the constraint set. + Constraints can be very specific, for example defining value domains + for each joint, such that the goal configuration is included, + or defining a volume in space, to which a specific robot link (e.g. + the end-effector) is required to move to. + This target cannot be translated into a single pose or configuration target except in trivial cases. Therefore the ending pose or configuration of the planned path can only be determined after performing the motion planning. - ConstraintSetTarget cannot be used as a target for Cartesian motion planning. - ConstraintSetTarget is suitable for advanced users who want to specify custom constraints for the robot motion planning. Different planner backends may support differnt types of Constraints. - See tutorial :ref:`constraint-set-target` for more details. + See tutorial :ref:`targets` for more details. + + ConstraintSetTarget is only supported by Free motion planning (using the MoveIt Planner), + Inverse Kinematics and Cartesian motion planning do not support this target type. Attributes ---------- - constraint_set : :class:`compas_fab.robots.Constraint` + constraint_set : :obj:`list` of :class:`compas_fab.robots.Constraint` A list of constraints to be satisfied. """ def __init__(self, constraint_set, name="Constraint Set Target"): super(ConstraintSetTarget, self).__init__(name=name) - self.constraint_set = constraint_set # type: List[Constraint] + self.constraint_set = constraint_set + + def scaled(self, factor): + """Returns a scaled copy of the target. + + Raises + ------ + NotImplementedError + This target type does not support scaling. + """ + raise NotImplementedError diff --git a/src/compas_fab/utilities/__init__.py b/src/compas_fab/utilities/__init__.py index 3817c2178..bfdde0118 100644 --- a/src/compas_fab/utilities/__init__.py +++ b/src/compas_fab/utilities/__init__.py @@ -19,6 +19,15 @@ read_data_from_pickle write_data_to_pickle +Transformation functions +======================== + +.. autosummary:: + :toctree: generated/ + :nosignatures: + + from_tcf_to_t0cf + Numerical functions =================== @@ -73,7 +82,9 @@ range_geometric_row, sign, ) - +from .transformation import ( + from_tcf_to_t0cf, +) __all__ = [ # file_io "read_csv_to_dictionary", @@ -96,4 +107,6 @@ "map_range", "range_geometric_row", "sign", + # transformation + "from_tcf_to_t0cf", ] diff --git a/src/compas_fab/utilities/transformation.py b/src/compas_fab/utilities/transformation.py new file mode 100644 index 000000000..a0d2c6665 --- /dev/null +++ b/src/compas_fab/utilities/transformation.py @@ -0,0 +1,31 @@ +from compas.geometry import Transformation +from compas.geometry import Frame + + +def from_tcf_to_t0cf(tcf_frame_in_wcf, tool_coordinate_frame): + """Converts a frame describing the robot's tool tip (tcf frame) relative to WCF + to a frame describing the robot's flange (tool0 frame), relative to WCF. + + Let: W_TCF = tcf_frame_in_wcf + Let: T0CF_TCF = tool_coordinate_frame + And: TCF_T0CF = (T0CF_TCF)^-1 + Let: frames_t0cf = W_T0CF + Then: W_T0CF = W_TCF * TCF_T0CF + + Parameters + ---------- + tcf_frame_in_wcf : :class:`~compas.geometry.Frame` + Frame (in WCF) of the robot's tool tip (tcf). + tool_coordinate_frame : :class:`~compas.geometry.Frame` + Tool tip coordinate frame relative to the flange of the robot. + + Returns + ------- + :class:`~compas.geometry.Frame` + Frame (in WCF) of the robot's flange (tool0). + + """ + T0CF_TCF = tool_coordinate_frame + TCF_T0CF = Transformation.from_frame(T0CF_TCF).inverse() + W_TCF = Transformation.from_frame(tcf_frame_in_wcf) + return Frame.from_transformation(W_TCF * TCF_T0CF) From 555055a849174b8cbb6e6077cfd4328cb5e92e29 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 8 Mar 2024 21:47:02 +0800 Subject: [PATCH 03/25] Black src --- .../backends/ros/backend_features/helpers.py | 15 ++++--- .../Cf_FrameTargetFromPlane/code.py | 11 +++-- .../ghpython/components/Cf_PlanMotion/code.py | 9 +---- .../components/Cf_PointAxisTarget/code.py | 5 ++- src/compas_fab/robots/targets.py | 40 +++++++++++++++---- src/compas_fab/utilities/__init__.py | 1 + 6 files changed, 55 insertions(+), 26 deletions(-) diff --git a/src/compas_fab/backends/ros/backend_features/helpers.py b/src/compas_fab/backends/ros/backend_features/helpers.py index 81284c671..d68497cca 100644 --- a/src/compas_fab/backends/ros/backend_features/helpers.py +++ b/src/compas_fab/backends/ros/backend_features/helpers.py @@ -58,9 +58,11 @@ def convert_target_to_goal_constraints(target, ee_link_name): tcf_frame_in_wcf = target.target_frame tool_coordinate_frame = target.tool_coordinate_frame pc = CF_PositionConstraint.from_frame( - tcf_frame_in_wcf, target.tolerance_position, ee_link_name, tool_coordinate_frame) + tcf_frame_in_wcf, target.tolerance_position, ee_link_name, tool_coordinate_frame + ) oc = CF_OrientationConstraint.from_frame( - tcf_frame_in_wcf, [target.tolerance_orientation] * 3, ee_link_name, tool_coordinate_frame) + tcf_frame_in_wcf, [target.tolerance_orientation] * 3, ee_link_name, tool_coordinate_frame + ) return [pc, oc] elif type(target) == PointAxisTarget: @@ -69,12 +71,15 @@ def convert_target_to_goal_constraints(target, ee_link_name): if tool_coordinate_frame: raise NotImplementedError( - "Tool coordinate frame is not yet supported when converting PointAxisTarget to ConstraintSetTarget.") + "Tool coordinate frame is not yet supported when converting PointAxisTarget to ConstraintSetTarget." + ) pc = CF_PositionConstraint.from_point( - tcf_point_in_wcf, target.tolerance_position, ee_link_name, tool_coordinate_frame) + tcf_point_in_wcf, target.tolerance_position, ee_link_name, tool_coordinate_frame + ) oc = CF_OrientationConstraint.from_frame( - tcf_frame_in_wcf, [6.35, 6.35, 0.01], ee_link_name, tool_coordinate_frame) + tcf_frame_in_wcf, [6.35, 6.35, 0.01], ee_link_name, tool_coordinate_frame + ) return [pc, oc] diff --git a/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py index 155212da0..0b1772be6 100644 --- a/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py +++ b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py @@ -14,13 +14,18 @@ class FrameTargetFromPlaneComponent(component): - def RunScript(self, plane, tolerance_position, tolerance_xaxis, tolerance_yaxis, tolerance_zaxis, tool_coordinate_frame): + def RunScript( + self, plane, tolerance_position, tolerance_xaxis, tolerance_yaxis, tolerance_zaxis, tool_coordinate_frame + ): target = None if plane: # Convert Rhino geometry to COMPAS geometry frame = plane if isinstance(plane, Frame) else plane_to_compas_frame(plane) - tool_coordinate_frame = tool_coordinate_frame if isinstance( - tool_coordinate_frame, Frame) else plane_to_compas_frame(tool_coordinate_frame) + tool_coordinate_frame = ( + tool_coordinate_frame + if isinstance(tool_coordinate_frame, Frame) + else plane_to_compas_frame(tool_coordinate_frame) + ) # Tolerance values tolerance_position = tolerance_position or 0.001 diff --git a/src/compas_fab/ghpython/components/Cf_PlanMotion/code.py b/src/compas_fab/ghpython/components/Cf_PlanMotion/code.py index 4f81882db..7aa6dc4f3 100644 --- a/src/compas_fab/ghpython/components/Cf_PlanMotion/code.py +++ b/src/compas_fab/ghpython/components/Cf_PlanMotion/code.py @@ -28,14 +28,7 @@ def RunScript( attached_collision_meshes = list(attached_collision_meshes) if attached_collision_meshes else None planner_id = str(planner_id) if planner_id else "RRTConnect" - if ( - robot - and robot.client - and robot.client.is_connected - and start_configuration - and target - and compute - ): + if robot and robot.client and robot.client.is_connected and start_configuration and target and compute: st[key] = robot.plan_motion( target, start_configuration=start_configuration, diff --git a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py index c64ab1e9c..862228748 100644 --- a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py +++ b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py @@ -22,8 +22,9 @@ def RunScript(self, point, target_z_vector, tolerance_position, tool_coordinate_ # Convert Rhino geometry to COMPAS geometry point = point if isinstance(point, Point) else point_to_compas(point) - target_z_vector = target_z_vector if isinstance( - target_z_vector, Vector) else vector_to_compas(target_z_vector) + target_z_vector = ( + target_z_vector if isinstance(target_z_vector, Vector) else vector_to_compas(target_z_vector) + ) target = PointAxisTarget(point, target_z_vector, tolerance_position, tool_coordinate_frame) diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index 731bec64d..0692e9a87 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -87,7 +87,14 @@ class FrameTarget(Target): """ - def __init__(self, target_frame, tolerance_position=None, tolerance_orientation=None, tool_coordinate_frame=None, name="Frame Target"): + def __init__( + self, + target_frame, + tolerance_position=None, + tolerance_orientation=None, + tool_coordinate_frame=None, + name="Frame Target", + ): super(FrameTarget, self).__init__(name=name) self.target_frame = target_frame self.tolerance_position = tolerance_position @@ -95,7 +102,14 @@ def __init__(self, target_frame, tolerance_position=None, tolerance_orientation= self.tool_coordinate_frame = tool_coordinate_frame @classmethod - def from_transformation(cls, transformation, tolerance_position=None, tolerance_orientation=None, tool_coordinate_frame=None, name="Frame Target"): + def from_transformation( + cls, + transformation, + tolerance_position=None, + tolerance_orientation=None, + tool_coordinate_frame=None, + name="Frame Target", + ): """Creates a FrameTarget from a transformation matrix. Parameters @@ -190,7 +204,14 @@ class PointAxisTarget(Target): """ - def __init__(self, target_point, target_z_vector, tolerance_position=None, tool_coordinate_frame=None, name="Point-Axis Target"): + def __init__( + self, + target_point, + target_z_vector, + tolerance_position=None, + tool_coordinate_frame=None, + name="Point-Axis Target", + ): super(PointAxisTarget, self).__init__(name=name) self.target_point = target_point self.target_z_vector = target_z_vector @@ -246,6 +267,7 @@ class ConfigurationTarget(Target): Always use positive values. If not specified, the default value from the planner is used. """ + SUPPORTED_JOINT_TYPES = [Joint.PRISMATIC, Joint.REVOLUTE, Joint.CONTINUOUS] def __init__(self, target_configuration, tolerance_above=None, tolerance_below=None, name="Configuration Target"): @@ -261,7 +283,7 @@ def __init__(self, target_configuration, tolerance_above=None, tolerance_below=N @classmethod def generate_default_tolerances(cls, configuration, tolerance_prismatic, tolerance_revolute): - """ Generates tolerances values for the target configuration based on the joint types. + """Generates tolerances values for the target configuration based on the joint types. The parameters `tolerance_prismatic` and `tolerance_revolute` are used to generate the list of values for `tolerances_above`, `tolerances_below`. The length of the list is equal to the @@ -340,10 +362,12 @@ def scale_tolerance(tolerance, joint_types): # We scale only the tolerances for prismatic and planar joints, # similar to the Configuration.scale() method - tolerance_above = scale_tolerance( - self.tolerance_above, target_configuration.joint_types) if self.tolerance_above else None - tolerance_below = scale_tolerance( - self.tolerance_below, target_configuration.joint_types) if self.tolerance_below else None + tolerance_above = ( + scale_tolerance(self.tolerance_above, target_configuration.joint_types) if self.tolerance_above else None + ) + tolerance_below = ( + scale_tolerance(self.tolerance_below, target_configuration.joint_types) if self.tolerance_below else None + ) return ConfigurationTarget(target_configuration, tolerance_above, tolerance_below, self.name) diff --git a/src/compas_fab/utilities/__init__.py b/src/compas_fab/utilities/__init__.py index bfdde0118..0403ab03c 100644 --- a/src/compas_fab/utilities/__init__.py +++ b/src/compas_fab/utilities/__init__.py @@ -85,6 +85,7 @@ from .transformation import ( from_tcf_to_t0cf, ) + __all__ = [ # file_io "read_csv_to_dictionary", From a2a259d18598879e6e040c74d52cae54a2f1a390 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 8 Mar 2024 22:03:23 +0800 Subject: [PATCH 04/25] Lint --- .../backends/ros/backend_features/helpers.py | 11 +++++++---- .../ghpython/components/Cf_PointAxisTarget/code.py | 2 -- src/compas_fab/robots/constraints.py | 2 +- src/compas_fab/robots/robot.py | 7 ++----- src/compas_fab/robots/targets.py | 11 +++++++---- 5 files changed, 17 insertions(+), 16 deletions(-) diff --git a/src/compas_fab/backends/ros/backend_features/helpers.py b/src/compas_fab/backends/ros/backend_features/helpers.py index d68497cca..e6050208d 100644 --- a/src/compas_fab/backends/ros/backend_features/helpers.py +++ b/src/compas_fab/backends/ros/backend_features/helpers.py @@ -45,16 +45,16 @@ def convert_target_to_goal_constraints(target, ee_link_name): Set of Constraint classes """ - if type(target) == ConstraintSetTarget: + if isinstance(target, ConstraintSetTarget): return target.constraint_set - elif type(target) == ConfigurationTarget: + elif isinstance(target, ConfigurationTarget): configuration = target.target_configuration tolerance_above = target.tolerance_above tolerance_below = target.tolerance_below return CF_JointConstraint.joint_constraints_from_configuration(configuration, tolerance_above, tolerance_below) - elif type(target) == FrameTarget: + elif isinstance(target, FrameTarget): tcf_frame_in_wcf = target.target_frame tool_coordinate_frame = target.tool_coordinate_frame pc = CF_PositionConstraint.from_frame( @@ -65,7 +65,7 @@ def convert_target_to_goal_constraints(target, ee_link_name): ) return [pc, oc] - elif type(target) == PointAxisTarget: + elif isinstance(target, PointAxisTarget): tcf_point_in_wcf = target.target_point tool_coordinate_frame = target.tool_coordinate_frame @@ -82,6 +82,9 @@ def convert_target_to_goal_constraints(target, ee_link_name): ) return [pc, oc] + else: + raise NotImplementedError("Target type {} not supported by ROS planning backend.".format(type(target))) + def convert_constraints_to_rosmsg(constraints, header): """Convert COMPAS FAB constraints into ROS Messages.""" diff --git a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py index 862228748..588faf1d6 100644 --- a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py +++ b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py @@ -4,8 +4,6 @@ COMPAS FAB v1.0.2 """ -import math - from compas_rhino.conversions import point_to_compas from compas_rhino.conversions import vector_to_compas from ghpythonlib.componentbase import executingcomponent as component diff --git a/src/compas_fab/robots/constraints.py b/src/compas_fab/robots/constraints.py index 9a1b2593a..f198581ae 100644 --- a/src/compas_fab/robots/constraints.py +++ b/src/compas_fab/robots/constraints.py @@ -368,7 +368,7 @@ def joint_constraints_from_configuration(cls, configuration, tolerances_above, t if len(joint_names) != len(configuration.joint_values): raise ValueError( - "The passed configuration has {} joint_names but {} joint_values: {}".format( + "The passed configuration has {} joint_names but {} joint_values".format( len(joint_names), len(joint_values) ) ) diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index 68c20ebd1..82a02c987 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -6,16 +6,12 @@ from compas.data import Data from compas.geometry import Frame -from compas.geometry import Sphere from compas.geometry import Transformation from compas_robots import Configuration from compas_robots import RobotModel from compas_robots.model import Joint from compas_fab.robots.constraints import Constraint -from compas_fab.robots.constraints import JointConstraint -from compas_fab.robots.constraints import OrientationConstraint -from compas_fab.robots.constraints import PositionConstraint __all__ = [ "Robot", @@ -1484,7 +1480,8 @@ def plan_motion(self, target, start_configuration=None, group=None, options=None # goal_constraints_WCF_scaled.append(cp) # Scale Target Definitions - target = target.scaled(1.0 / self.scale_factor) + if self.scale_factor != 1.0: + target = target.scaled(1.0 / self.scale_factor) # Transform path constraints to RCF and scale if path_constraints: diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index 0692e9a87..b10c290f7 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -1,11 +1,14 @@ from compas.data import Data from compas.geometry import Frame -from compas_robots import Configuration from compas_robots.model import Joint -from compas_fab.robots import JointConstraint -from compas_fab.robots import OrientationConstraint -from compas_fab.robots import PositionConstraint +__all__ = [ + "Target", + "FrameTarget", + "PointAxisTarget", + "ConfigurationTarget", + "ConstraintSetTarget", +] class Target(Data): From 56c5e5f5f7f8eee34f8afec2e2bad36e983e98d1 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 22 Mar 2024 15:59:35 +0800 Subject: [PATCH 05/25] Make Targets and Constraints serializable. --- CHANGELOG.md | 1 + src/compas_fab/robots/constraints.py | 41 ++++++++++++++++++++++++++-- src/compas_fab/robots/targets.py | 31 +++++++++++++++++++++ 3 files changed, 71 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a0e10d325..d5eb89e51 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 +* 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. * Moved `Robot.position_constraint_from_frame()` to `PositionConstraint.from_frame()`, as constraints are no longer intended for users to use directly. diff --git a/src/compas_fab/robots/constraints.py b/src/compas_fab/robots/constraints.py index f198581ae..0ebb9a73a 100644 --- a/src/compas_fab/robots/constraints.py +++ b/src/compas_fab/robots/constraints.py @@ -2,6 +2,7 @@ from __future__ import division from __future__ import print_function +from compas.data import Data from compas.geometry import Rotation from compas.geometry import Scale from compas.geometry import Sphere @@ -11,7 +12,7 @@ __all__ = ["BoundingVolume", "Constraint", "JointConstraint", "OrientationConstraint", "PositionConstraint"] -class BoundingVolume(object): +class BoundingVolume(Data): """A container for describing a bounding volume. Parameters @@ -60,6 +61,12 @@ def __init__(self, volume_type, volume): self.type = volume_type self.volume = volume + def __data__(self): + return { + "volume_type": self.type, + "volume": self.volume, + } + @classmethod def from_box(cls, box): """Create a :class:`BoundingVolume` from a :class:`compas.geometry.Box`. @@ -171,7 +178,7 @@ def copy(self): return cls(self.type, self.volume.copy()) -class Constraint(object): +class Constraint(Data): """Base class for robot constraints. Parameters @@ -219,6 +226,12 @@ def __init__(self, constraint_type, weight=1.0): self.type = constraint_type self.weight = weight + def __data__(self): + return { + "constraint_type": self.type, + "weight": self.weight, + } + def transform(self, transformation): """Transform the :class:`Constraint`.""" pass @@ -295,6 +308,15 @@ def __init__(self, joint_name, value, tolerance_above=0.0, tolerance_below=0.0, self.tolerance_above = abs(tolerance_above) self.tolerance_below = abs(tolerance_below) + def __data__(self): + return { + "joint_name": self.joint_name, + "value": self.value, + "tolerance_above": self.tolerance_above, + "tolerance_below": self.tolerance_below, + "weight": self.weight, + } + def scale(self, scale_factor): """Scale (multiply) the constraint with a factor. @@ -455,6 +477,14 @@ def __init__(self, link_name, quaternion, tolerances=None, weight=1.0): self.quaternion = [float(a) for a in list(quaternion)] self.tolerances = [float(a) for a in list(tolerances)] if tolerances else [0.01] * 3 + def __data__(self): + return { + "link_name": self.link_name, + "quaternion": self.quaternion, + "tolerances": self.tolerances, + "weight": self.weight, + } + def transform(self, transformation): """Transform the volume using a :class:`compas.geometry.Transformation`. @@ -588,6 +618,13 @@ def __init__(self, link_name, bounding_volume, weight=1.0): self.bounding_volume = bounding_volume self.weight = weight + def __data__(self): + return { + "link_name": self.link_name, + "bounding_volume": self.bounding_volume, + "weight": self.weight, + } + @classmethod def from_frame(cls, frame_WCF, tolerance_position, link_name, tool_coordinate_frame=None, weight=1.0): """Create a PositionConstraint from a frame on the group's end-effector link. diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index b10c290f7..abd12373c 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -39,6 +39,10 @@ def __init__(self, name="Generic Target"): super(Target, self).__init__() self.name = name + @property + def __data__(self): + raise NotImplementedError + def scaled(self, factor): """Returns a scaled copy of the target. @@ -104,6 +108,15 @@ def __init__( self.tolerance_orientation = tolerance_orientation self.tool_coordinate_frame = tool_coordinate_frame + @property + def __data__(self): + return { + "target_frame": self.target_frame, + "tolerance_position": self.tolerance_position, + "tolerance_orientation": self.tolerance_orientation, + "tool_coordinate_frame": self.tool_coordinate_frame, + } + @classmethod def from_transformation( cls, @@ -221,6 +234,14 @@ def __init__( self.tolerance_position = tolerance_position self.tool_coordinate_frame = tool_coordinate_frame + def __data__(self): + return { + "target_point": self.target_point, + "target_z_vector": self.target_z_vector, + "tolerance_position": self.tolerance_position, + "tool_coordinate_frame": self.tool_coordinate_frame, + } + def scaled(self, factor): """Returns a copy of the target where the target point and tolerances are scaled. @@ -284,6 +305,13 @@ def __init__(self, target_configuration, tolerance_above=None, tolerance_below=N for joint_type in target_configuration.joint_types: assert joint_type in self.SUPPORTED_JOINT_TYPES, "Unsupported joint type: {}".format(joint_type) + def __data__(self): + return { + "target_configuration": self.target_configuration, + "tolerance_above": self.tolerance_above, + "tolerance_below": self.tolerance_below, + } + @classmethod def generate_default_tolerances(cls, configuration, tolerance_prismatic, tolerance_revolute): """Generates tolerances values for the target configuration based on the joint types. @@ -407,6 +435,9 @@ def __init__(self, constraint_set, name="Constraint Set Target"): super(ConstraintSetTarget, self).__init__(name=name) self.constraint_set = constraint_set + def __data__(self): + return {"constraint_set": self.constraint_set} + def scaled(self, factor): """Returns a scaled copy of the target. From 6da310412b51bb1f4e49cc10270980786c8138a6 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 5 Apr 2024 04:59:28 +0200 Subject: [PATCH 06/25] Apply suggestions from code review (Docstrings only) Co-authored-by: Gonzalo Casas --- CHANGELOG.md | 10 +++++----- .../02_description_models/03_targets.rst | 17 +++++++++-------- .../backends/ros/backend_features/helpers.py | 6 +++--- .../ros/backend_features/move_it_plan_motion.py | 2 +- .../Cf_ConfigurationTarget/metadata.json | 6 +++--- .../components/Cf_FrameTargetFromPlane/code.py | 11 +++++------ .../Cf_FrameTargetFromPlane/metadata.json | 8 ++++---- 7 files changed, 30 insertions(+), 30 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d5eb89e51..27e62dbf1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,22 +9,22 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added -* Added `compas_fab.robot.Target` class to represent a motion planning target. +* Added `compas_fab.robots.Target` class to represent a motion planning target. * Added also child classes `FrameTarget`, `PointAxisTarget`, `ConfigurationTarget`, `ConstraintSetTarget` * Unlike previous constraints, `Targets` do not contain `group` as parameter. Instead, group parameter is passed to the planning call. -* Target scaling function is now embeded in the code for Targets. `scaled()` should be called by the `plan_motion` class and a copy if the target is returned. +* Target scaling function is now embeded in the code for Targets. `scaled()` should be called by the `plan_motion` class and a copy of the target is returned. * FrameTarget uses `tolerance_orientation` to specify a orientation tolerance, the values are adopted to become `tolerance_axes` when being converted to `OrientationConstraint`. ### Changed * 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 +* 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. * Moved `Robot.position_constraint_from_frame()` to `PositionConstraint.from_frame()`, as constraints are no longer intended for users to use directly. * Moved `Robot.constraints_from_frame()` to ros.backend_features and is handled by `convert_target_to_goal_constraints()`. Users who wish to use a frame as target should use a `FrameTarget` instead. -* Changed GH Component `ConstraintsFromPlane` to `FrameTargetFromPlane` -* Changed GH Component `ConstraintsFromTargetConfiguration` +* Changed GH Component `ConstraintsFromPlane` to `FrameTargetFromPlane`. +* Changed GH Component `ConstraintsFromTargetConfiguration` to `ConfigurationTarget`. ### Removed * Removed `plan_cartesian_motion_deprecated` and `plan_motion_deprecated` methods from `Robot` class diff --git a/docs/examples/02_description_models/03_targets.rst b/docs/examples/02_description_models/03_targets.rst index d3385d40f..8b2a65c0c 100644 --- a/docs/examples/02_description_models/03_targets.rst +++ b/docs/examples/02_description_models/03_targets.rst @@ -12,19 +12,20 @@ Target classes are used to describe the goal condition (i.e. end condition) of a for motion planning. They can be used for both Free Motion Planning (FMP) and Cartesian Motion Planning (CMP). -The :class:`compas_fab.robot.FrameTarget` is the most common target for motion planning. -It defined the complete pose of the end-effector (or the robot flange, if no tool is attached). -The FrameTarget is commonly created from a Frame object, or alternatively from a a Transformation object. +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). +A frame target is commonly created from a :class:`compas.geometry.Frame` object, or alternatively from a :class:`compas.geometry.Transformation` object. -The :class:`compas_fab.robot.PointAxisTarget` class is used for specifying a target +The :class:`compas_fab.robots.PointAxisTarget` class is used for specifying a target based on a point and a Z-Vector. This is useful for example when the robot is using a -cylinrical tool to perform a task, for example 3D printing,welding or drilling. +cylindrical tool to perform a task, for example 3D printing, welding or drilling, +or more generically, any tool for which a rotation around its own Z axis is not relevant. The point and the vector is defined relative to the Z-axis of the tool coordinate frame (TCF). The goal is for the tool's TCF point to coincide with the `target_point`, and the TCF's Z-axis is parallel to the Z vector. Note that the exact orientation of the TCF is not determined until after the target is planned. -The :class:`compas_fab.robot.ConfigurationTarget` class is used to specify a target +The :class:`compas_fab.robots.ConfigurationTarget` class is used to specify a target based on a robot configuration. This is useful for example when the robot is required to move to a specific joint configuration. This is useful for example when the position of the robot is aquired by teaching (i.e. jogging). Typically, @@ -34,13 +35,13 @@ case the remaining joints are left unspecified. Note that ConfigurationTarget do not take into account whether a tool is attached or not. The pose of the end-effector and the robot flange is determined by forward kinematics. -The :class:`compas_fab.robot.ConstraintSetTarget` class is used to specify a list of +The :class:`compas_fab.robots.ConstraintSetTarget` class is used to specify a list of ROS MoveIt constraints. This is intended for advanced users who want to create custom combination of constraints. See :class:`compas_fab.robots.Constraint` for available constraints. ---------------------------- -Multiple Tragets (Waypoints) +Multiple Targets (Waypoints) ---------------------------- diff --git a/src/compas_fab/backends/ros/backend_features/helpers.py b/src/compas_fab/backends/ros/backend_features/helpers.py index e6050208d..fbf5aa965 100644 --- a/src/compas_fab/backends/ros/backend_features/helpers.py +++ b/src/compas_fab/backends/ros/backend_features/helpers.py @@ -32,17 +32,17 @@ def validate_response(response): def convert_target_to_goal_constraints(target, ee_link_name): """Convert COMPAS FAB `Target` objects into `Constraint` objects for passing it to MoveIt backend. - This function is intended to be called only by MoveIt backend when handeling different Target types. + This function is intended to be called only by MoveIt backend when handling different Target types. Parameters ---------- target: :class:`compas_fab.robots.Target` - The goal for the robot to achieve. + The target for the robot to reach. Returns ------- list of :class:`Constraint` - Set of Constraint classes + List of constraint objects. """ if isinstance(target, ConstraintSetTarget): diff --git a/src/compas_fab/backends/ros/backend_features/move_it_plan_motion.py b/src/compas_fab/backends/ros/backend_features/move_it_plan_motion.py index 427b072a6..e4a5ea3b6 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_plan_motion.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_plan_motion.py @@ -40,7 +40,7 @@ def plan_motion(self, robot, target, start_configuration=None, group=None, optio robot : :class:`compas_fab.robots.Robot` The robot instance for which the motion plan is being calculated. target: list of :class:`compas_fab.robots.Target` - The goal for the robot to achieve. + The target for the robot to reach. start_configuration: :class:`compas_fab.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 diff --git a/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/metadata.json b/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/metadata.json index b5b3dfd3e..d2935656d 100644 --- a/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/metadata.json +++ b/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/metadata.json @@ -1,6 +1,6 @@ { - "name": "ConfigurationTarget From Configuration", - "nickname": "ConfigurationTarget", + "name": "Target from a configuration", + "nickname": "Configuration target", "category": "COMPAS FAB", "subcategory": "Planning", "description": "Create joint constraints for each of the robot's configurable joints based on a given target configuration.", @@ -32,7 +32,7 @@ "outputParameters": [ { "name": "target", - "description": "A ConfigurationTarget" + "description": "A configuration target" } ] } diff --git a/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py index 0b1772be6..b8ade139c 100644 --- a/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py +++ b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py @@ -20,12 +20,11 @@ def RunScript( target = None if plane: # Convert Rhino geometry to COMPAS geometry - frame = plane if isinstance(plane, Frame) else plane_to_compas_frame(plane) - tool_coordinate_frame = ( - tool_coordinate_frame - if isinstance(tool_coordinate_frame, Frame) - else plane_to_compas_frame(tool_coordinate_frame) - ) + frame = plane + if not isinstance(frame, Frame): + frame = plane_to_compas_frame(frame) + if not isinstance(tool_coordinate_frame, Frame): + tool_coordinate_frame = plane_to_compas_frame(tool_coordinate_frame) # Tolerance values tolerance_position = tolerance_position or 0.001 diff --git a/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/metadata.json b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/metadata.json index 9b4949595..b203ef31c 100644 --- a/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/metadata.json +++ b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/metadata.json @@ -1,6 +1,6 @@ { - "name": "FrameTarget From Plane", - "nickname": "FrameTarget", + "name": "Target from a plane/frame", + "nickname": "Frame target", "category": "COMPAS FAB", "subcategory": "Planning", "description": "Create a position and an orientation constraint from a plane calculated for the group's end-effector link.", @@ -11,7 +11,7 @@ "inputParameters": [ { "name": "plane", - "description": "The Rhino or Grasshopper Plane or COMPAS Frame from which we create position and orientation constraints." + "description": "The Rhino plane or COMPAS frame from which we create position and orientation constraints." }, { "name": "tolerance_position", @@ -41,7 +41,7 @@ "outputParameters": [ { "name": "target", - "description": "A FrameTarget." + "description": "A frame target." } ] } From 5b3ea4d4a2431fe3f4fc3fcb97fde497a896be7d Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 5 Apr 2024 11:12:26 +0800 Subject: [PATCH 07/25] Lint --- .../ghpython/components/Cf_FrameTargetFromPlane/code.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py index b8ade139c..396808f27 100644 --- a/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py +++ b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py @@ -22,9 +22,9 @@ def RunScript( # Convert Rhino geometry to COMPAS geometry frame = plane if not isinstance(frame, Frame): - frame = plane_to_compas_frame(frame) + frame = plane_to_compas_frame(frame) if not isinstance(tool_coordinate_frame, Frame): - tool_coordinate_frame = plane_to_compas_frame(tool_coordinate_frame) + tool_coordinate_frame = plane_to_compas_frame(tool_coordinate_frame) # Tolerance values tolerance_position = tolerance_position or 0.001 From e88d2482d97a07d26a8c5ef22165ebf792d2437f Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 5 Apr 2024 11:12:43 +0800 Subject: [PATCH 08/25] Modified Changelog --- CHANGELOG.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 27e62dbf1..e52d4d6a7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,8 +12,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 * Added `compas_fab.robots.Target` class to represent a motion planning target. * Added also child classes `FrameTarget`, `PointAxisTarget`, `ConfigurationTarget`, `ConstraintSetTarget` * Unlike previous constraints, `Targets` do not contain `group` as parameter. Instead, group parameter is passed to the planning call. -* Target scaling function is now embeded in the code for Targets. `scaled()` should be called by the `plan_motion` class and a copy of the target is returned. -* FrameTarget uses `tolerance_orientation` to specify a orientation tolerance, the values are adopted to become `tolerance_axes` when being converted to `OrientationConstraint`. +* Target scaling function is now embeded in the code for Targets. `scaled()` should be called by the user before passing the target to the `plan_motion` function. ### Changed From 81f48d0cdc908562688a3f44e721432dc8288d01 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 5 Apr 2024 11:38:02 +0800 Subject: [PATCH 09/25] update documentation --- .../02_description_models/03_targets.rst | 39 +++++++------------ 1 file changed, 15 insertions(+), 24 deletions(-) diff --git a/docs/examples/02_description_models/03_targets.rst b/docs/examples/02_description_models/03_targets.rst index 8b2a65c0c..2e813e5f7 100644 --- a/docs/examples/02_description_models/03_targets.rst +++ b/docs/examples/02_description_models/03_targets.rst @@ -17,33 +17,24 @@ It defines the complete pose of the end-effector (or the robot flange, if no too A frame target is commonly created from a :class:`compas.geometry.Frame` object, or alternatively from a :class:`compas.geometry.Transformation` object. The :class:`compas_fab.robots.PointAxisTarget` class is used for specifying a target -based on a point and a Z-Vector. This is useful for example when the robot is using a -cylindrical tool to perform a task, for example 3D printing, welding or drilling, -or more generically, any tool for which a rotation around its own Z axis is not relevant. -The point and the vector is defined relative to the Z-axis of the tool coordinate -frame (TCF). The goal is for the tool's TCF point to coincide with the `target_point`, -and the TCF's Z-axis is parallel to the Z vector. Note that the exact orientation -of the TCF is not determined until after the target is planned. +based on a point (`target_point`) and a Z-Axis (`target_z_axis`). +This is useful for example when the robot is using a cylindrical tool to perform a task, +for example 3D printing, welding or drilling. +In a more general case, it can be used for any tools for which the rotation +around its own Z axis is acceptable during use. +The point and the Z-Axis are defined relative to the tool coordinate frame (TCF). +The goal is (1) for the tool's TCF point to coincide with the `target_point`, +and (2) for the TCF's Z-axis to become parallel to the `target_z_axis`. +Note that the exact orientatio of the TCF is not determined until after the target is planned. The :class:`compas_fab.robots.ConfigurationTarget` class is used to specify a target -based on a robot configuration. This is useful for example when the robot is -required to move to a specific joint configuration. This is useful for example -when the position of the robot is aquired by teaching (i.e. jogging). Typically, -the ConfigurationTarget should have the same number of joints as the planning group +based on a specific robot configuration (joint values). +For example, it can be used to move the robot to a taught position aquired by jogging. +Typically, the ConfigurationTarget should have the same number of joints as the planning group of the robot. However, it is possible to specify a subset of the joints, in which -case the remaining joints are left unspecified. Note that ConfigurationTarget does -not take into account whether a tool is attached or not. -The pose of the end-effector and the robot flange is determined by forward kinematics. +case the remaining joints are left unspecified. The :class:`compas_fab.robots.ConstraintSetTarget` class is used to specify a list of -ROS MoveIt constraints. This is intended for advanced users who want to create custom +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. - ----------------------------- -Multiple Targets (Waypoints) ----------------------------- - - - - +constraints. At the moment, only the ROS MoveIt planning backend supports this target type. From 6252afe3f45b494521a83074e968efed247a57bd Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 5 Apr 2024 11:39:58 +0800 Subject: [PATCH 10/25] Renaming target_z_vector to target_z_axis --- .../ghpython/components/Cf_PointAxisTarget/code.py | 8 ++++---- src/compas_fab/robots/targets.py | 12 ++++++------ 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py index 588faf1d6..200b4c6be 100644 --- a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py +++ b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py @@ -14,16 +14,16 @@ class PointAxisTargetComponent(component): - def RunScript(self, point, target_z_vector, tolerance_position, tool_coordinate_frame): + def RunScript(self, point, target_z_axis, tolerance_position, tool_coordinate_frame): target = None if point: # Convert Rhino geometry to COMPAS geometry point = point if isinstance(point, Point) else point_to_compas(point) - target_z_vector = ( - target_z_vector if isinstance(target_z_vector, Vector) else vector_to_compas(target_z_vector) + target_z_axis = ( + target_z_axis if isinstance(target_z_axis, Vector) else vector_to_compas(target_z_axis) ) - target = PointAxisTarget(point, target_z_vector, tolerance_position, tool_coordinate_frame) + target = PointAxisTarget(point, target_z_axis, tolerance_position, tool_coordinate_frame) return target diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index abd12373c..b3ed93734 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -207,7 +207,7 @@ class PointAxisTarget(Target): ---------- target_point : :class:`compas.geometry.Point` The target point defined relative to the robot's base coordinate frame (RCF). - target_z_vector : :class:`compas.geometry.Vector` + target_z_axis : :class:`compas.geometry.Vector` The target axis is defined by the target_point and pointing towards this vector. The tool tip coordinate frame (TCF)'s Z axis can rotate around this axis. tolerance_position : float, optional @@ -223,21 +223,21 @@ class PointAxisTarget(Target): def __init__( self, target_point, - target_z_vector, + target_z_axis, tolerance_position=None, tool_coordinate_frame=None, name="Point-Axis Target", ): super(PointAxisTarget, self).__init__(name=name) self.target_point = target_point - self.target_z_vector = target_z_vector + self.target_z_axis = target_z_axis self.tolerance_position = tolerance_position self.tool_coordinate_frame = tool_coordinate_frame def __data__(self): return { "target_point": self.target_point, - "target_z_vector": self.target_z_vector, + "target_z_axis": self.target_z_axis, "tolerance_position": self.tolerance_position, "tool_coordinate_frame": self.tool_coordinate_frame, } @@ -257,9 +257,9 @@ def scaled(self, factor): """ target_point = self.target_point.scaled(factor) tolerance_position = self.tolerance_position * factor if self.tolerance_position else None - target_z_vector = self.target_z_vector.scaled # Vector is unitized and is not scaled + target_z_axis = self.target_z_axis.scaled # Vector is unitized and is not scaled tool_coordinate_frame = self.tool_coordinate_frame.scaled(factor) if self.tool_coordinate_frame else None - return PointAxisTarget(target_point, target_z_vector, tool_coordinate_frame, tolerance_position, self.name) + return PointAxisTarget(target_point, target_z_axis, tool_coordinate_frame, tolerance_position, self.name) class ConfigurationTarget(Target): From ed3de2951bc88d9818aead36019dd374d48d267c Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 5 Apr 2024 12:09:06 +0800 Subject: [PATCH 11/25] format to a one liner --- src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py index 200b4c6be..ccccc8d27 100644 --- a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py +++ b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py @@ -20,9 +20,7 @@ def RunScript(self, point, target_z_axis, tolerance_position, tool_coordinate_fr # Convert Rhino geometry to COMPAS geometry point = point if isinstance(point, Point) else point_to_compas(point) - target_z_axis = ( - target_z_axis if isinstance(target_z_axis, Vector) else vector_to_compas(target_z_axis) - ) + target_z_axis = target_z_axis if isinstance(target_z_axis, Vector) else vector_to_compas(target_z_axis) target = PointAxisTarget(point, target_z_axis, tolerance_position, tool_coordinate_frame) From 6500c203411c7eeb3321ca9bbfd87b2b3bd8551f Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 5 Apr 2024 06:18:20 +0200 Subject: [PATCH 12/25] Update src/compas_fab/robots/targets.py Co-authored-by: Gonzalo Casas --- src/compas_fab/robots/targets.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index b3ed93734..bd2c21139 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -274,7 +274,7 @@ class ConfigurationTarget(Target): in the actual robot cell, such as a tool changing position. The number of joints in the target configuration should match the number of joints - in the robot's planning group. Otherwise the behaviour of the bankend planner may + in the robot's planning group. Otherwise the behavior of the backend planner may be undefined. See tutorial :ref:`targets` for more details. Attributes From b5f8b13953fc7e990ba2c456f9222485c475c63d Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 12 Apr 2024 15:17:20 +0800 Subject: [PATCH 13/25] Add doc tutorial page reference --- docs/examples/01_fundamentals/01_frame_and_transformation.rst | 2 ++ docs/examples/01_fundamentals/02_coordinate_frames.rst | 2 ++ docs/examples/02_description_models/01_kinematic_model.rst | 2 ++ docs/examples/02_description_models/02_robot.rst | 2 ++ 4 files changed, 8 insertions(+) diff --git a/docs/examples/01_fundamentals/01_frame_and_transformation.rst b/docs/examples/01_fundamentals/01_frame_and_transformation.rst index e61373b57..f9c4c65ce 100644 --- a/docs/examples/01_fundamentals/01_frame_and_transformation.rst +++ b/docs/examples/01_fundamentals/01_frame_and_transformation.rst @@ -1,3 +1,5 @@ +.. _frame_and_transformation: + ******************************************************************************* Frame and Transformation ******************************************************************************* diff --git a/docs/examples/01_fundamentals/02_coordinate_frames.rst b/docs/examples/01_fundamentals/02_coordinate_frames.rst index 6350c1041..722d58d01 100644 --- a/docs/examples/01_fundamentals/02_coordinate_frames.rst +++ b/docs/examples/01_fundamentals/02_coordinate_frames.rst @@ -1,3 +1,5 @@ +.. _coordinate_frames: + ******************************************************************************* Coordinate frames ******************************************************************************* diff --git a/docs/examples/02_description_models/01_kinematic_model.rst b/docs/examples/02_description_models/01_kinematic_model.rst index 308be6ad8..4901f0a1c 100644 --- a/docs/examples/02_description_models/01_kinematic_model.rst +++ b/docs/examples/02_description_models/01_kinematic_model.rst @@ -1,3 +1,5 @@ +.. _kinematic_model: + ******************************************************************************* Kinematic model ******************************************************************************* diff --git a/docs/examples/02_description_models/02_robot.rst b/docs/examples/02_description_models/02_robot.rst index 901b7aabe..60baa6fb7 100644 --- a/docs/examples/02_description_models/02_robot.rst +++ b/docs/examples/02_description_models/02_robot.rst @@ -1,3 +1,5 @@ +.. _robot_model: + ******************************************************************************* Robot models ******************************************************************************* From f63300ca7de460adbe33424ffeb6ee8e5bc60caa Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 12 Apr 2024 15:20:47 +0800 Subject: [PATCH 14/25] Fix Docstring --- src/compas_fab/robots/constraints.py | 14 +++++++------- src/compas_fab/robots/robot.py | 14 ++++++++------ 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/compas_fab/robots/constraints.py b/src/compas_fab/robots/constraints.py index 0ebb9a73a..784b8a21b 100644 --- a/src/compas_fab/robots/constraints.py +++ b/src/compas_fab/robots/constraints.py @@ -420,7 +420,7 @@ def joint_constraints_from_configuration(cls, configuration, tolerances_above, t class OrientationConstraint(Constraint): - r"""Constrains a link to be within a certain orientation. + """Constrains a link to be within a certain orientation. Parameters ---------- @@ -430,7 +430,7 @@ class OrientationConstraint(Constraint): The desired orientation of the link specified by a quaternion in the order of ``[w, x, y, z]``. tolerances : :obj:`list` of :obj:`float`, optional - Error tolerances t\ :sub:`i` for each of the frame's axes. If only one + Error tolerances :math:`t_{i}` for each of the frame's axes. If only one value is passed it will be used for all 3 axes. The respective bound to be achieved is :math:`(a_{i} - t_{i}, a_{i} + t_{i})`. Defaults to ``[0.01, 0.01, 0.01]``. @@ -447,7 +447,7 @@ class OrientationConstraint(Constraint): The desired orientation of the link specified by a quaternion in the order of ``[w, x, y, z]``. tolerances : :obj:`list` of :obj:`float` - Error tolerances t\ :sub:`i` for each of the frame's axes. If only one + Error tolerances :math:`t_{i}` for each of the frame's axes. If only one value is passed it will be used for all 3 axes. The respective bound to be achieved is :math:`(a_{i} - t_{i}, a_{i} + t_{i})`. weight : :obj:`float` @@ -515,7 +515,7 @@ def copy(self): @classmethod def from_frame(cls, frame_WCF, tolerances_orientation, link_name, tool_coordinate_frame=None, weight=1.0): - r"""Create an OrientationConstraint from a frame on the group's end-effector link. + """Create an :class:`OrientationConstraint` from a frame on the group's end-effector link. Only the orientation of the frame is considered for the constraint, expressed as a quaternion. @@ -524,7 +524,7 @@ def from_frame(cls, frame_WCF, tolerances_orientation, link_name, tool_coordinat frame_WCF: :class:`compas.geometry.Frame` The frame from which we create the orientation constraint. tolerances_orientation: :obj:`list` of :obj:`float` - Error tolerances t\ :sub:`i` for each of the frame's axes in + Error tolerances :math:`t_{i}` for each of the frame's axes in radians. If only one value is passed in the list it will be uses for all 3 axes. link_name : :obj:`str` The name of the end-effector link. Necessary for creating the position constraint. @@ -627,7 +627,7 @@ def __data__(self): @classmethod def from_frame(cls, frame_WCF, tolerance_position, link_name, tool_coordinate_frame=None, weight=1.0): - """Create a PositionConstraint from a frame on the group's end-effector link. + """Create a :class:`PositionConstraint` from a frame on the group's end-effector link. Only the position of the frame is considered for the constraint. Parameters @@ -757,7 +757,7 @@ def from_point(cls, link_name, point, tolerance_position, weight=1.0): @classmethod def from_mesh(cls, link_name, mesh, weight=1.0): - """Create a class:`PositionConstraint` from a :class:`compas.datastructures.Mesh`. + """Create a :class:`PositionConstraint` from a :class:`compas.datastructures.Mesh`. Parameters ---------- diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index 82a02c987..a667e1745 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -1379,16 +1379,18 @@ def plan_motion(self, target, start_configuration=None, group=None, options=None Parameters ---------- - target : class:`Target` - The goal to be achieved by the robot at the end of the motion. - See :ref: - start_configuration : :class:`.Configuration`, optional + target : :class:`compas_fab.robots.Target` + The target to be achieved by the robot at the end of the motion. + See :ref:`targets`. + 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. group : :obj:`str`, optional - The name of the group to plan for. Defaults to the robot's main - planning group. + The name of the planning group used to define the movable joints and the Planner Coordinate Frame (PCF). + See :ref:`coordinate_frames` for more details about the PCF. + The planning group must match with one of the groups defined in the robot's semantics. + Defaults to the robot's main planning group. options : :obj:`dict`, optional Dictionary containing the following key-value pairs: From 4e19ab3d1e3fecb0aeab57f8bb3466f01114ee2b Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 12 Apr 2024 15:58:25 +0800 Subject: [PATCH 15/25] tool_coordinate_frame accepts a Transformation as input --- src/compas_fab/robots/targets.py | 27 ++++++++++++++++----------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index bd2c21139..048786910 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -1,5 +1,6 @@ from compas.data import Data from compas.geometry import Frame +from compas.geometry import Transformation from compas_robots.model import Joint __all__ = [ @@ -62,18 +63,20 @@ def scaled(self, factor): class FrameTarget(Target): """Represents a fully constrained pose target for the robot's end-effector using a :class:`compas.geometry.Frame`. - The end-effector has no translational or rotational freedom. + When using a FrameTarget, the end-effector has no translational or rotational freedom. + In another words, the pose of the end-effector is fully defined (constrained). Given a FrameTarget, the planner aims to find a path moving - the robot's flange frame (or tool frame) to the specified `FrameTarget.target_frame`. - The default target frame corresponds to the Flange Coordinate Frame (RfCF) - of the robot. When using a tool, the target frame can be specified - relative to the tool tip coordinate frame (TTCF). - The planner will then move the tool tip towards the target frame. + the robot's planner coordinate frame (PCF) to the specified `FrameTarget.target_frame`. + In many case, the PCF corresponds to the Tool0 Coordinate Frame (T0CF) on the robot controller. + See :ref:`coordinate_frames` for more details about the PCF, + or if the planning result does not match the pose displayed on the robot controller. - For robots with multiple end effector attachment points, the FCF depends on - the planning group setting in the planning request, as defined in an SRDF file or - :class:`compas_fab.robots.RobotSemantics`. + If the user wants to plan with a tool (such that the Tool Coordinate Frame (TCF) is matched with the target), + the `tool_coordinate_frame` parameter should be provided to define the TCF relative to the PCF of the robot. + + For robots with multiple end effector attachment points (such as the RFL Robot), the PCF depends on + the planning group used in the planning request, see :meth:`compas_fab.robots.Robot.plan_motion`. Attributes ---------- @@ -85,7 +88,7 @@ class FrameTarget(Target): tolerance_orientation : float, optional The tolerance for the orientation. 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 of the robot. If not specified, the target frame is relative to the robot's flange. name : str, optional @@ -106,6 +109,8 @@ def __init__( self.target_frame = target_frame 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 @@ -138,7 +143,7 @@ def from_transformation( tolerance_orientation : float, optional The tolerance for the orientation. 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 of the robot. If not specified, the target frame is relative to the robot's flange. name : str, optional From 2c515b582071552d90904d2730ee9228f3c90218 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 19 Apr 2024 07:24:31 +0200 Subject: [PATCH 16/25] Update src/compas_fab/robots/targets.py Co-authored-by: Gonzalo Casas --- src/compas_fab/robots/targets.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index 048786910..cc6719da5 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -427,8 +427,8 @@ class ConstraintSetTarget(Target): Different planner backends may support differnt types of Constraints. See tutorial :ref:`targets` for more details. - ConstraintSetTarget is only supported by Free motion planning (using the MoveIt Planner), - Inverse Kinematics and Cartesian motion planning do not support this target type. + ConstraintSetTarget is only supported by Free motion planning, + Cartesian motion planning do not support this target type. Attributes ---------- From 0f1e117283918f31d41efa1fe49835c08245793c Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 19 Apr 2024 13:50:45 +0800 Subject: [PATCH 17/25] remove commented out code --- src/compas_fab/robots/robot.py | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index 684ad3d79..ff86879ed 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -1485,17 +1485,6 @@ def plan_motion(self, target, start_configuration=None, group=None, options=None # 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) - # goal_constraints_WCF_scaled = [] - # for c in goal_constraints: - # cp = c.copy() - # if c.type == Constraint.JOINT: - # joint = self.get_joint_by_name(c.joint_name) - # if joint.is_scalable(): - # cp.scale(1.0 / self.scale_factor) - # else: - # cp.scale(1.0 / self.scale_factor) - # goal_constraints_WCF_scaled.append(cp) - # Scale Target Definitions if self.scale_factor != 1.0: target = target.scaled(1.0 / self.scale_factor) From c85d8b2f32c6f5b2d3e6182e740069b5561af484 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 19 Apr 2024 13:51:13 +0800 Subject: [PATCH 18/25] Update example in Constraints to use new RobotLibrary --- src/compas_fab/robots/constraints.py | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/compas_fab/robots/constraints.py b/src/compas_fab/robots/constraints.py index 784b8a21b..45f088a36 100644 --- a/src/compas_fab/robots/constraints.py +++ b/src/compas_fab/robots/constraints.py @@ -557,14 +557,13 @@ def from_frame(cls, frame_WCF, tolerances_orientation, link_name, tool_coordinat Examples -------- - >>> from compas_fab.robots.ur5 import Robot - >>> robot = Robot() + >>> robot = RobotLibrary.ur5() >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) >>> tolerances_orientation = [math.radians(1)] * 3 >>> group = robot.main_group_name - >>> ee_link_name = robot.get_end_effector_link_name(group) - >>> OrientationConstraint.from_frame(frame, tolerances_orientation, ee_link_name) - OrientationConstraint('ee_link', [0.5, 0.5, 0.5, 0.5], [0.017453292519943295, 0.017453292519943295, 0.017453292519943295], 1.0) + >>> end_effector_link_name = robot.get_end_effector_link_name(group) + >>> OrientationConstraint.from_frame(frame, tolerances_orientation, end_effector_link_name) + OrientationConstraint('tool0', [0.5, 0.5, 0.5, 0.5], [0.017453292519943295, 0.017453292519943295, 0.017453292519943295], 1.0) """ if tool_coordinate_frame: @@ -658,14 +657,13 @@ def from_frame(cls, frame_WCF, tolerance_position, link_name, tool_coordinate_fr Examples -------- - >>> from compas_fab.robots.ur5 import Robot - >>> robot = Robot() + >>> robot = RobotLibrary.ur5() >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) >>> tolerance_position = 0.001 >>> group = robot.main_group_name - >>> ee_link_name = robot.get_end_effector_link_name(group) - >>> PositionConstraint.from_frame(frame, tolerance_position, ee_link_name) # doctest: +SKIP - PositionConstraint('ee_link', BoundingVolume(2, Sphere(Point(0.400, 0.300, 0.400), 0.001)), 1.0) # doctest: +SKIP + >>> end_effector_link_name = robot.get_end_effector_link_name(group) + >>> PositionConstraint.from_frame(frame, tolerance_position, end_effector_link_name) # doctest: +SKIP + PositionConstraint('tool0', BoundingVolume(2, Sphere(Point(0.400, 0.300, 0.400), 0.001)), 1.0) # doctest: +SKIP """ if tool_coordinate_frame: From 3e2970fa712d05e0773a353ea9a203fba901755f Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 19 Apr 2024 14:07:00 +0800 Subject: [PATCH 19/25] clean up some mention of new Frames. --- src/compas_fab/robots/targets.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index cc6719da5..376975b6e 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -198,10 +198,10 @@ class PointAxisTarget(Target): The user must define (1) the target point of which the tool tip will reach and (2) the target axis where the tool tip coordinate frame (TCF)'s Z axis can rotate around. The target point and axis are defined relative to the robot's - base coordinate frame (RCF). + world coordinate frame (WCF). - In addition, it's necessary to define the tool tip coordinate frame (TtCF) - relative to the robot's flange frame (FCF). This is labeled as tool_coordinate_frame. + In addition, it's necessary to define the tool tip coordinate frame (TCF) + relative to the robot's flange frame. This is labeled as tool_coordinate_frame. If tool_coordinate_frame is unspecified, the target point and axis will be matched with the robot's flange frame. For robots with multiple end effector attachment points, the FCF depends on @@ -211,7 +211,7 @@ class PointAxisTarget(Target): Attributes ---------- target_point : :class:`compas.geometry.Point` - The target point defined relative to the robot's base coordinate frame (RCF). + The target point defined relative to the robot's world coordinate frame (WCF). target_z_axis : :class:`compas.geometry.Vector` The target axis is defined by the target_point and pointing towards this vector. The tool tip coordinate frame (TCF)'s Z axis can rotate around this axis. From ab5e0c0633d8fa0ab34f845988753babcefa9425 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Wed, 24 Apr 2024 19:01:48 +0200 Subject: [PATCH 20/25] Update docs/examples/02_description_models/03_targets.rst Co-authored-by: Gonzalo Casas --- 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 2e813e5f7..de0bf9ce6 100644 --- a/docs/examples/02_description_models/03_targets.rst +++ b/docs/examples/02_description_models/03_targets.rst @@ -29,7 +29,7 @@ Note that the exact orientatio of the TCF is not determined until after the targ The :class:`compas_fab.robots.ConfigurationTarget` class is used to specify a target based on a specific robot configuration (joint values). -For example, it can be used to move the robot to a taught position aquired by jogging. +For example, it can be used to move the robot to a taught position acquired by jogging. Typically, the ConfigurationTarget should have the same number of joints as the planning group of the robot. However, it is possible to specify a subset of the joints, in which case the remaining joints are left unspecified. From d11dec5085df983bdd20b98fcd041d979c95f850 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 26 Apr 2024 13:46:07 +0800 Subject: [PATCH 21/25] Removes mention of PCF --- src/compas_fab/robots/robot.py | 3 +-- src/compas_fab/robots/targets.py | 11 ++++------- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index ff86879ed..00adb3e26 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -1407,8 +1407,7 @@ def plan_motion(self, target, start_configuration=None, group=None, options=None joints of the entire robot, at the starting position. Defaults to the all-zero configuration. group : :obj:`str`, optional - The name of the planning group used to define the movable joints and the Planner Coordinate Frame (PCF). - See :ref:`coordinate_frames` for more details about the PCF. + The name of the planning group used to define the movable joints. The planning group must match with one of the groups defined in the robot's semantics. Defaults to the robot's main planning group. options : :obj:`dict`, optional diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index 376975b6e..7652e61f4 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -67,16 +67,13 @@ class FrameTarget(Target): In another words, the pose of the end-effector is fully defined (constrained). Given a FrameTarget, the planner aims to find a path moving - the robot's planner coordinate frame (PCF) to the specified `FrameTarget.target_frame`. - In many case, the PCF corresponds to the Tool0 Coordinate Frame (T0CF) on the robot controller. - See :ref:`coordinate_frames` for more details about the PCF, - or if the planning result does not match the pose displayed on the robot controller. + the robot's Tool0 Coordinate Frame (T0CF) to the specified `FrameTarget.target_frame`. If the user wants to plan with a tool (such that the Tool Coordinate Frame (TCF) is matched with the target), - the `tool_coordinate_frame` parameter should be provided to define the TCF relative to the PCF of the robot. + the `tool_coordinate_frame` parameter should be provided to define the TCF relative to the T0CF of the robot. - For robots with multiple end effector attachment points (such as the RFL Robot), the PCF depends on - the planning group used in the planning request, see :meth:`compas_fab.robots.Robot.plan_motion`. + For robots with multiple end effector attachment points (such as the RFL Robot), the attachment point depends on + the planning group selected in the planning request, see :meth:`compas_fab.robots.Robot.plan_motion`. Attributes ---------- From d3bf8f07be6d762a93f9fe5a40902b37e15084e3 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 26 Apr 2024 08:01:51 +0200 Subject: [PATCH 22/25] Update src/compas_fab/robots/targets.py Co-authored-by: Gonzalo Casas --- src/compas_fab/robots/targets.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index 7652e61f4..212024c05 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -208,7 +208,7 @@ class PointAxisTarget(Target): Attributes ---------- target_point : :class:`compas.geometry.Point` - The target point defined relative to the robot's world coordinate frame (WCF). + The target point defined relative to the world coordinate frame (WCF). target_z_axis : :class:`compas.geometry.Vector` The target axis is defined by the target_point and pointing towards this vector. The tool tip coordinate frame (TCF)'s Z axis can rotate around this axis. From eff3f6c4f2eaa3d6df661156e1161a701465624f Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 26 Apr 2024 14:23:17 +0800 Subject: [PATCH 23/25] Update GH Component tool tip and comments --- .../components/Cf_ConfigurationTarget/code.py | 2 +- .../Cf_ConfigurationTarget/metadata.json | 2 +- .../components/Cf_FrameTargetFromPlane/code.py | 18 ++++++++++-------- .../Cf_FrameTargetFromPlane/metadata.json | 2 +- .../components/Cf_PointAxisTarget/code.py | 2 +- .../Cf_PointAxisTarget/metadata.json | 2 +- src/compas_fab/robots/targets.py | 2 ++ 7 files changed, 17 insertions(+), 13 deletions(-) diff --git a/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/code.py b/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/code.py index 79f907ca6..9dc4c795e 100644 --- a/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/code.py +++ b/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/code.py @@ -1,5 +1,5 @@ """ -Create joint constraints for each of the robot's configurable joints based on a given target configuration. +Create configuration target for the robot's end-effector motion planning. COMPAS FAB v1.0.2 """ diff --git a/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/metadata.json b/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/metadata.json index d2935656d..93142dcde 100644 --- a/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/metadata.json +++ b/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/metadata.json @@ -3,7 +3,7 @@ "nickname": "Configuration target", "category": "COMPAS FAB", "subcategory": "Planning", - "description": "Create joint constraints for each of the robot's configurable joints based on a given target configuration.", + "description": "Create configuration target for the robot's end-effector motion planning.", "exposure": 8, "ghpython": { "isAdvancedMode": true, diff --git a/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py index 396808f27..5775d10f8 100644 --- a/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py +++ b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py @@ -1,5 +1,5 @@ """ -Create a position and an orientation constraint from a plane calculated for the group's end-effector link. +Create a fully constrained pose target for the robot's end-effector using a GH Plane or compas Frame. COMPAS FAB v1.0.2 """ @@ -27,14 +27,16 @@ def RunScript( tool_coordinate_frame = plane_to_compas_frame(tool_coordinate_frame) # Tolerance values - tolerance_position = tolerance_position or 0.001 - tolerance_xaxis = tolerance_xaxis or 1.0 - tolerance_yaxis = tolerance_yaxis or 1.0 - tolerance_zaxis = tolerance_zaxis or 1.0 + DEFAULT_TOLERANCE_METERS = 0.001 + DEFAULT_TOLERANCE_RADIANS = math.radians(1) + tolerance_position = tolerance_position or DEFAULT_TOLERANCE_METERS + tolerance_xaxis = tolerance_xaxis or DEFAULT_TOLERANCE_RADIANS + tolerance_yaxis = tolerance_yaxis or DEFAULT_TOLERANCE_RADIANS + tolerance_zaxis = tolerance_zaxis or DEFAULT_TOLERANCE_RADIANS tolerance_orientation = [ - math.radians(tolerance_xaxis), - math.radians(tolerance_yaxis), - math.radians(tolerance_zaxis), + (tolerance_xaxis), + (tolerance_yaxis), + (tolerance_zaxis), ] target = FrameTarget(frame, tolerance_position, tolerance_orientation, tool_coordinate_frame) diff --git a/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/metadata.json b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/metadata.json index b203ef31c..c903c6601 100644 --- a/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/metadata.json +++ b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/metadata.json @@ -3,7 +3,7 @@ "nickname": "Frame target", "category": "COMPAS FAB", "subcategory": "Planning", - "description": "Create a position and an orientation constraint from a plane calculated for the group's end-effector link.", + "description": "Create a fully constrained pose target for the robot's end-effector using a GH Plane or compas Frame.", "exposure": 8, "ghpython": { "isAdvancedMode": true, diff --git a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py index ccccc8d27..81e75fc8d 100644 --- a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py +++ b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py @@ -1,5 +1,5 @@ """ -Create a position and an orientation constraint from a plane calculated for the group's end-effector link. +Create a point and axis target for the robot's end-effector motion planning. COMPAS FAB v1.0.2 """ diff --git a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/metadata.json b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/metadata.json index 10a1e19bc..62a8de779 100644 --- a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/metadata.json +++ b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/metadata.json @@ -3,7 +3,7 @@ "nickname": "Point Axis Target", "category": "COMPAS FAB", "subcategory": "Planning", - "description": "Create a PointAxisTarget from a Point and a Vector.", + "description": "Create a point and axis target for the robot's end-effector motion planning.", "exposure": 8, "ghpython": { "isAdvancedMode": true, diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index 212024c05..5dfb91330 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -81,9 +81,11 @@ class FrameTarget(Target): The target frame. tolerance_position : float, optional The tolerance for the position. + Unit is meters. If not specified, the default value from the planner is used. tolerance_orientation : float, optional The tolerance for the orientation. + Unit is radians. If not specified, the default value from the planner is used. 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. From e52ff98bf077faccefeb915ee3cbace15cb31848 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 26 Apr 2024 14:29:49 +0800 Subject: [PATCH 24/25] Improve input description for Cf_PointAxisTarget --- .../ghpython/components/Cf_PointAxisTarget/metadata.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/metadata.json b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/metadata.json index 62a8de779..b5d39ea87 100644 --- a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/metadata.json +++ b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/metadata.json @@ -11,7 +11,7 @@ "inputParameters": [ { "name": "point", - "description": "The target point defined relative to the robot's base coordinate frame (RCF). Accepts Rhino / Grasshopper Point or COMPAS Point." + "description": "The target point defined relative to the world coordinate frame (WCF). Accepts Rhino / Grasshopper Point or COMPAS Point." }, { "name": "target_z_vector", @@ -24,7 +24,7 @@ }, { "name": "tool_coordinate_frame", - "description": "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). Accepts Rhino / Grasshopper Frame or COMPAS Frame." + "description": "The tool tip coordinate frame relative to the tool0 coordinate frame (T0CF) of the robot. If not specified, the target point is relative to the T0CF directly. Accepts Rhino / Grasshopper Frame or COMPAS Frame." } ], "outputParameters": [ From 4578c13c22001b3cc47a74c68350ea98154d396e Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 26 Apr 2024 17:03:44 +0800 Subject: [PATCH 25/25] Update tolerance for free rotation in convert_target_to_goal_constraints function --- src/compas_fab/backends/ros/backend_features/helpers.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/compas_fab/backends/ros/backend_features/helpers.py b/src/compas_fab/backends/ros/backend_features/helpers.py index fbf5aa965..a05a23374 100644 --- a/src/compas_fab/backends/ros/backend_features/helpers.py +++ b/src/compas_fab/backends/ros/backend_features/helpers.py @@ -77,8 +77,9 @@ def convert_target_to_goal_constraints(target, ee_link_name): pc = CF_PositionConstraint.from_point( tcf_point_in_wcf, target.tolerance_position, ee_link_name, tool_coordinate_frame ) + OC_TOLERANCE_FOR_FREE_ROTATION = [0.01, 0.01, 6.3] oc = CF_OrientationConstraint.from_frame( - tcf_frame_in_wcf, [6.35, 6.35, 0.01], ee_link_name, tool_coordinate_frame + tcf_frame_in_wcf, OC_TOLERANCE_FOR_FREE_ROTATION, ee_link_name, tool_coordinate_frame ) return [pc, oc]