diff --git a/AUTHORS.rst b/AUTHORS.rst index f6343f212..38bc55aec 100644 --- a/AUTHORS.rst +++ b/AUTHORS.rst @@ -51,4 +51,3 @@ Authors * Chen Kasirer `@chenkasirer `_ * Edvard Bruun `@ebruun `_ * Victor Pok Yin Leung `@yck011522 `_ - diff --git a/CHANGELOG.md b/CHANGELOG.md index c13ceb067..c5ca3a8e1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,12 +9,26 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added +* 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 user before passing the target to the `plan_motion` function. + ### 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. +* 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 the behavior of Duration class when accepting both seconds (float) and nanoseconds (int) where the decimal point of seconds and the nanoseconds add up to more than 1 second. +* 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 +* Removed `forward_kinematics_deprecated` and `inverse_kinematics_deprecated` method from `Robot` class ## [1.0.2] 2024-02-22 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 ******************************************************************************* 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..de0bf9ce6 --- /dev/null +++ b/docs/examples/02_description_models/03_targets.rst @@ -0,0 +1,40 @@ +.. _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.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.robots.PointAxisTarget` class is used for specifying a target +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 specific robot configuration (joint values). +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. + +The :class:`compas_fab.robots.ConstraintSetTarget` class is used to specify a list of +constraints as a planning target. This is intended for advanced users who want to create custom +combination of constraints. See :class:`compas_fab.robots.Constraint` for available +constraints. At the moment, only the ROS MoveIt planning backend supports this target type. 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 e3fe18475..388f3d93a 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..a05a23374 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,63 @@ 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 handling different Target types. + + Parameters + ---------- + target: :class:`compas_fab.robots.Target` + The target for the robot to reach. + + Returns + ------- + list of :class:`Constraint` + List of constraint objects. + """ + + if isinstance(target, ConstraintSetTarget): + return target.constraint_set + + 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 isinstance(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 isinstance(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_TOLERANCE_FOR_FREE_ROTATION = [0.01, 0.01, 6.3] + oc = CF_OrientationConstraint.from_frame( + tcf_frame_in_wcf, OC_TOLERANCE_FOR_FREE_ROTATION, ee_link_name, tool_coordinate_frame + ) + 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.""" 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..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 @@ -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 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 @@ -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..9dc4c795e --- /dev/null +++ b/src/compas_fab/ghpython/components/Cf_ConfigurationTarget/code.py @@ -0,0 +1,30 @@ +""" +Create configuration target for the robot's end-effector motion planning. + +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 61% rename from src/compas_fab/ghpython/components/Cf_ConstraintsFromConfiguration/metadata.json rename to src/compas_fab/ghpython/components/Cf_ConfigurationTarget/metadata.json index 1835341b2..93142dcde 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": "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.", + "description": "Create configuration target for the robot's end-effector motion planning.", "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 configuration target" } ] } -} +} \ 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..5775d10f8 --- /dev/null +++ b/src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/code.py @@ -0,0 +1,44 @@ +""" +Create a fully constrained pose target for the robot's end-effector using a GH Plane or compas Frame. + +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 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 + 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 = [ + (tolerance_xaxis), + (tolerance_yaxis), + (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 59% rename from src/compas_fab/ghpython/components/Cf_ConstraintsFromPlane/metadata.json rename to src/compas_fab/ghpython/components/Cf_FrameTargetFromPlane/metadata.json index 2e4473c5d..c903c6601 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": "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.", + "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, "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 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 frame target." } ] } -} +} \ 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..7aa6dc4f3 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, @@ -28,16 +28,9 @@ 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 goal_constraints - and compute - ): + if robot and robot.client and robot.client.is_connected and start_configuration 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..81e75fc8d --- /dev/null +++ b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py @@ -0,0 +1,27 @@ +""" +Create a point and axis target for the robot's end-effector motion planning. + +COMPAS FAB v1.0.2 +""" + +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_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_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) + + 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 000000000..0613d36bb Binary files /dev/null and b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/icon.png differ diff --git a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/metadata.json b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/metadata.json new file mode 100644 index 000000000..b5d39ea87 --- /dev/null +++ b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/metadata.json @@ -0,0 +1,37 @@ +{ + "name": "Point Axis Target", + "nickname": "Point Axis Target", + "category": "COMPAS FAB", + "subcategory": "Planning", + "description": "Create a point and axis target for the robot's end-effector motion planning.", + "exposure": 8, + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "point", + "description": "The target point defined relative to the world coordinate frame (WCF). Accepts Rhino / Grasshopper Point or COMPAS Point." + }, + { + "name": "target_z_vector", + "description": "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. Accepts Rhino / Grasshopper Vector or COMPAS Vector." + }, + { + "name": "tolerance_position", + "description": "The allowed tolerance of the tool reaching the target point.", + "typeHintID": "float" + }, + { + "name": "tool_coordinate_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": [ + { + "name": "target", + "description": "A FrameTarget." + } + ] + } +} \ No newline at end of file diff --git a/src/compas_fab/robots/__init__.py b/src/compas_fab/robots/__init__.py index 8759472b9..04b23c06b 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 ----------- @@ -113,6 +126,13 @@ from .semantics import ( RobotSemantics, ) +from .targets import ( + Target, + FrameTarget, + PointAxisTarget, + ConfigurationTarget, + ConstraintSetTarget, +) from .time_ import ( Duration, ) @@ -135,14 +155,18 @@ "AttachedCollisionMesh", "BoundingVolume", "CollisionMesh", + "ConfigurationTarget", "Constraint", + "ConstraintSetTarget", "Duration", + "FrameTarget", "Inertia", "JointConstraint", "JointTrajectory", "JointTrajectoryPoint", "OrientationConstraint", "PlanningScene", + "PointAxisTarget", "PositionConstraint", "ReachabilityMap", "DeviationVectorsGenerator", @@ -150,6 +174,7 @@ "Robot", "RobotLibrary", "RobotSemantics", + "Target", "Tool", "Trajectory", "Wrench", diff --git a/src/compas_fab/robots/constraints.py b/src/compas_fab/robots/constraints.py index 88465e8a4..45f088a36 100644 --- a/src/compas_fab/robots/constraints.py +++ b/src/compas_fab/robots/constraints.py @@ -2,13 +2,17 @@ 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 + +from compas_fab.utilities import from_tcf_to_t0cf __all__ = ["BoundingVolume", "Constraint", "JointConstraint", "OrientationConstraint", "PositionConstraint"] -class BoundingVolume(object): +class BoundingVolume(Data): """A container for describing a bounding volume. Parameters @@ -57,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`. @@ -168,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 @@ -216,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 @@ -292,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. @@ -320,9 +345,82 @@ def copy(self): cls = type(self) return cls(self.joint_name, self.value, self.tolerance_above, self.tolerance_below, self.weight) + @classmethod + def joint_constraints_from_configuration(cls, configuration, tolerances_above, tolerances_below): + """Create joint constraints for all joints of the configuration. + One constraint is created for each joint. + + 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 in the list, 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. + + Returns + ------- + :obj:`list` of :class:`JointConstraint` + + Raises + ------ + :exc:`ValueError` + If the passed configuration does not have joint names. + :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. + + """ + joint_names = configuration.joint_names + joint_values = configuration.joint_values + if not joint_names: + raise ValueError("The passed configuration has no joint_names") + + if len(joint_names) != len(configuration.joint_values): + raise ValueError( + "The passed configuration has {} joint_names but {} joint_values".format( + len(joint_names), len(joint_values) + ) + ) + if len(tolerances_above) == 1: + tolerances_above = tolerances_above * len(joint_names) + elif len(tolerances_above) != len(configuration.joint_values): + raise ValueError( + "The number of `tolerances_above` values should either be 1 or equal to the number of joint_values ({}), current number of `tolerances_above` values: {}".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 number of `tolerances_above` values should either be 1 or equal to the number of joint_values ({}), current number of `tolerances_above` values: {}".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 + class OrientationConstraint(Constraint): - r"""Constrains a link to be within a certain orientation. + """Constrains a link to be within a certain orientation. Parameters ---------- @@ -332,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]``. @@ -349,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` @@ -379,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`. @@ -407,6 +513,70 @@ def copy(self): cls = type(self) return cls(self.link_name, self.quaternion[:], self.tolerances[:], self.weight) + @classmethod + def from_frame(cls, frame_WCF, tolerances_orientation, link_name, tool_coordinate_frame=None, weight=1.0): + """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. + + Parameters + ---------- + frame_WCF: :class:`compas.geometry.Frame` + The frame from which we create the orientation constraint. + tolerances_orientation: :obj:`list` of :obj:`float` + 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. + 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`, optional + A weighting factor for this constraint. Denotes relative importance to + other constraints. Closer to zero means less important. Defaults to + ``1``. + + 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_orientation 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 + -------- + >>> 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 + >>> 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: + 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 +617,61 @@ 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 :class:`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 + -------- + >>> 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 + >>> 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: + 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,9 +728,34 @@ 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`. + """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 9acca41b6..8cc68350f 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", @@ -1041,268 +1037,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 - -------- - >>> robot = RobotLibrary.ur5() - >>> 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('tool0', [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] - - end_effector_link_name = 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(end_effector_link_name, 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 - -------- - >>> robot = RobotLibrary.ur5() - >>> 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('tool0', 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] - - end_effector_link_name = self.get_end_effector_link_name(group) - sphere = Sphere(radius=tolerance_position, point=frame_WCF.point) - return PositionConstraint.from_sphere(end_effector_link_name, 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 - -------- - >>> robot = RobotLibrary.ur5() - >>> 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('tool0', BoundingVolume(2, Sphere(Point(0.400, 0.300, 0.400), 0.001)), 1.0), # doctest: +SKIP - OrientationConstraint('tool0', [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:`compas_robots.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 - -------- - >>> robot = RobotLibrary.ur5() - >>> 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 # ========================================================================== @@ -1488,30 +1222,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. @@ -1603,9 +1313,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 ): @@ -1727,48 +1434,22 @@ 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. - 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. + 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: @@ -1789,35 +1470,37 @@ def plan_motion(self, goal_constraints, start_configuration=None, group=None, op The calculated trajectory. Examples - -------- + --------ssssss - Using position and orientation constraints: + Using a :class:`~compas_fab.robots.FrameTarget` : >>> with RosClient() as client: # doctest: +SKIP #: This doctest can pass locally but persistently fails on CI in GitHub. "roslibpy.core.RosTimeoutError: Failed to connect to ROS" ... robot = client.load_robot() ... 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 + ... target = FrameTarget(frame, tolerance_position, tolerance_orientation) ... goal_constraints = robot.constraints_from_frame(frame, tolerance_position, tolerances_axes, group) - ... trajectory = robot.plan_motion(goal_constraints, start_configuration, group, {'planner_id': 'RRTConnect'}) + ... trajectory = robot.plan_motion((target, start_configuration, group, {'planner_id': 'RRTConnect'}) ... print(trajectory.fraction) 1.0 - Using joint constraints (to the UP configuration): + Using a :class:`~compas_fab.robots.ConfigurationTarget` : >>> with RosClient() as client: # doctest: +SKIP #: This doctest can pass locally but persistently fails on CI in GitHub. "roslibpy.core.RosTimeoutError: Failed to connect to ROS" ... robot = client.load_robot() - ... 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'}) ... print(trajectory.fraction) 1.0 @@ -1841,16 +1524,9 @@ 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) + # Scale Target Definitions + if self.scale_factor != 1.0: + target = target.scaled(1.0 / self.scale_factor) # Transform path constraints to RCF and scale if path_constraints: @@ -1876,7 +1552,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, @@ -1890,34 +1566,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 new file mode 100644 index 000000000..5dfb91330 --- /dev/null +++ b/src/compas_fab/robots/targets.py @@ -0,0 +1,453 @@ +from compas.data import Data +from compas.geometry import Frame +from compas.geometry import Transformation +from compas_robots.model import Joint + +__all__ = [ + "Target", + "FrameTarget", + "PointAxisTarget", + "ConfigurationTarget", + "ConstraintSetTarget", +] + + +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 + + @property + def __data__(self): + raise NotImplementedError + + 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`. + + 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 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 T0CF of the robot. + + 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 + ---------- + target_frame : :class:`compas.geometry.Frame` + 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. + 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, + 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 + self.tolerance_orientation = tolerance_orientation + if isinstance(tool_coordinate_frame, Transformation): + tool_coordinate_frame = Frame.from_transformation(tool_coordinate_frame) + self.tool_coordinate_frame = tool_coordinate_frame + + @property + def __data__(self): + 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, + 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` 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 + 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): + """ + 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 + world coordinate frame (WCF). + + 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 + 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 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. + 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 (T0CF) and the + Z axis of the flange can rotate around the target axis. + + """ + + def __init__( + self, + target_point, + 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_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_axis": self.target_z_axis, + "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. + + 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_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_axis, tool_coordinate_frame, tolerance_position, self.name) + + +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 behavior of the backend planner may + 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"): + + 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) + + 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. + + 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): + """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. + 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 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:`targets` for more details. + + ConstraintSetTarget is only supported by Free motion planning, + Cartesian motion planning do not support this target type. + + Attributes + ---------- + 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 + + def __data__(self): + return {"constraint_set": self.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..0403ab03c 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,6 +82,9 @@ range_geometric_row, sign, ) +from .transformation import ( + from_tcf_to_t0cf, +) __all__ = [ # file_io @@ -96,4 +108,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)