diff --git a/CHANGELOG.md b/CHANGELOG.md index c5ca3a8e1..8ac23bd39 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added +* Added `compas_fab.robots.Waypoints` class to represent a sequence of targets. It has two child classes: `FrameWaypoints` and `PointAxisWaypoints`. * Added `compas_fab.robots.Target` class to represent a motion planning target. * Added also child classes `FrameTarget`, `PointAxisTarget`, `ConfigurationTarget`, `ConstraintSetTarget` * Unlike previous constraints, `Targets` do not contain `group` as parameter. Instead, group parameter is passed to the planning call. diff --git a/src/compas_fab/robots/__init__.py b/src/compas_fab/robots/__init__.py index 04b23c06b..192ac21d7 100644 --- a/src/compas_fab/robots/__init__.py +++ b/src/compas_fab/robots/__init__.py @@ -46,8 +46,8 @@ CollisionMesh PlanningScene -Targets -------- +Targets and Waypoints +--------------------- .. autosummary:: :toctree: generated/ @@ -58,6 +58,9 @@ PointAxisTarget ConfigurationTarget ConstraintSetTarget + Waypoints + FrameWaypoints + PointAxisWaypoints Constraints ----------- @@ -132,6 +135,9 @@ PointAxisTarget, ConfigurationTarget, ConstraintSetTarget, + Waypoints, + FrameWaypoints, + PointAxisWaypoints, ) from .time_ import ( Duration, @@ -160,6 +166,7 @@ "ConstraintSetTarget", "Duration", "FrameTarget", + "FrameWaypoints", "Inertia", "JointConstraint", "JointTrajectory", @@ -167,6 +174,7 @@ "OrientationConstraint", "PlanningScene", "PointAxisTarget", + "PointAxisWaypoints", "PositionConstraint", "ReachabilityMap", "DeviationVectorsGenerator", @@ -177,6 +185,7 @@ "Target", "Tool", "Trajectory", + "Waypoints", "Wrench", "to_degrees", "to_radians", diff --git a/src/compas_fab/robots/constraints.py b/src/compas_fab/robots/constraints.py index 45f088a36..8e107d03d 100644 --- a/src/compas_fab/robots/constraints.py +++ b/src/compas_fab/robots/constraints.py @@ -669,7 +669,8 @@ def from_frame(cls, frame_WCF, tolerance_position, link_name, tool_coordinate_fr if tool_coordinate_frame: frame_WCF = from_tcf_to_t0cf(frame_WCF, tool_coordinate_frame) - sphere = Sphere(radius=tolerance_position, point=frame_WCF.point) + radius = float(tolerance_position) + sphere = Sphere(radius, point=frame_WCF.point) return cls.from_sphere(link_name, sphere, weight) @classmethod diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index 5dfb91330..64ff1b3c6 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -9,6 +9,9 @@ "PointAxisTarget", "ConfigurationTarget", "ConstraintSetTarget", + "Waypoints", + "FrameWaypoints", + "PointAxisWaypoints", ] @@ -47,6 +50,8 @@ def __data__(self): def scaled(self, factor): """Returns a scaled copy of the target. + If the user model is created in millimeters, the target should be scaled by a factor of 0.001 before passing to the planner. + Parameters ---------- factor : float @@ -91,7 +96,7 @@ class FrameTarget(Target): 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. + The human-readable name of the target. Defaults to 'Frame Target'. """ @@ -119,6 +124,7 @@ def __data__(self): "tolerance_position": self.tolerance_position, "tolerance_orientation": self.tolerance_orientation, "tool_coordinate_frame": self.tool_coordinate_frame, + "name": self.name, } @classmethod @@ -146,7 +152,7 @@ def from_transformation( 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. + The human-readable name of the target. Defaults to 'Frame Target'. Returns @@ -158,7 +164,7 @@ def from_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. + """Returns a copy of the :class:`FrameTarget` 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 @@ -176,7 +182,8 @@ def scaled(self, factor): """ target_frame = self.target_frame.scaled(factor) tolerance_position = self.tolerance_position * factor - tolerance_orientation = self.tolerance_orientation * factor + # Orientation tolerance is not scaled + tolerance_orientation = self.tolerance_orientation tool_coordinate_frame = self.tool_coordinate_frame.scaled(factor) if self.tool_coordinate_frame else None return FrameTarget(target_frame, tolerance_position, tolerance_orientation, tool_coordinate_frame, self.name) @@ -193,6 +200,8 @@ class PointAxisTarget(Target): 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. + Note that PointAxisTarget only represents a single target, + for a sequence of targets, consider using :class:`PointAxisWaypoints`. 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 @@ -213,6 +222,7 @@ class PointAxisTarget(Target): 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. + A unitized vector is recommended. 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. @@ -221,7 +231,9 @@ class PointAxisTarget(Target): The tool tip coordinate frame relative to the flange coordinate frame of the robot. If not specified, the target point is relative to the robot's flange (T0CF) and the Z axis of the flange can rotate around the target axis. - + name : str, optional + The human-readable name of the target. + Defaults to 'Point-Axis Target'. """ def __init__( @@ -238,12 +250,14 @@ def __init__( self.tolerance_position = tolerance_position self.tool_coordinate_frame = tool_coordinate_frame + @property 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, + "name": self.name, } def scaled(self, factor): @@ -261,9 +275,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_axis = self.target_z_axis.scaled # Vector is unitized and is not scaled + target_z_axis = self.target_z_axis # 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) + return PointAxisTarget(target_point, target_z_axis, tolerance_position, tool_coordinate_frame, self.name) class ConfigurationTarget(Target): @@ -294,6 +308,9 @@ class ConfigurationTarget(Target): 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. + name : str, optional + The human-readable name of the target. + Defaults to 'Configuration Target'. """ SUPPORTED_JOINT_TYPES = [Joint.PRISMATIC, Joint.REVOLUTE, Joint.CONTINUOUS] @@ -309,11 +326,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) + @property def __data__(self): return { "target_configuration": self.target_configuration, "tolerance_above": self.tolerance_above, "tolerance_below": self.tolerance_below, + "name": self.name, } @classmethod @@ -423,7 +442,7 @@ class ConstraintSetTarget(Target): 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. + Different planner backends may support different types of Constraints. See tutorial :ref:`targets` for more details. ConstraintSetTarget is only supported by Free motion planning, @@ -433,14 +452,21 @@ class ConstraintSetTarget(Target): ---------- constraint_set : :obj:`list` of :class:`compas_fab.robots.Constraint` A list of constraints to be satisfied. + name : str, optional + The human-readable name of the target. + Defaults to 'Constraint Set Target'. """ def __init__(self, constraint_set, name="Constraint Set Target"): super(ConstraintSetTarget, self).__init__(name=name) self.constraint_set = constraint_set + @property def __data__(self): - return {"constraint_set": self.constraint_set} + return { + "constraint_set": self.constraint_set, + "name": self.name, + } def scaled(self, factor): """Returns a scaled copy of the target. @@ -451,3 +477,243 @@ def scaled(self, factor): This target type does not support scaling. """ raise NotImplementedError + + +class Waypoints(Data): + """Represents a sequence of kinematic target for motion planning. + + Waypoints represent a sequence of targets the robot should pass through in the order they are defined. + This is in contrast to :class:`Target` which represent only a single target. + The initial (starting) point should not be included in the waypoints list. + It is valid for a Waypoints object to have one target. + + Waypoints are useful for tasks like painting, welding, or 3D printing, where the programmer + wants to define the waypoints the robot should pass through. + + Waypoints are intended to be used for motion planning with a planning backend by using :meth:`compas_fab.robot.plan_motion_with_waypoints`. + Different backends might support different types of waypoints. + The method of interpolation between the waypoints is controlled by the motion planner backend. + + Attributes + ---------- + name : str , optional, default = 'target' + A human-readable name for identifying the target. + + See Also + -------- + :class:`PointAxisWaypoints` + :class:`FrameWaypoints` + """ + + def __init__(self, name="Generic Waypoints"): + super(Waypoints, self).__init__() + self.name = name + + @property + def __data__(self): + raise NotImplementedError + + def scaled(self, factor): + """Returns a scaled copy of the waypoints. + + If the user model is created in millimeters, the target should be scaled by a factor of 0.001 before passing to the planner. + + Parameters + ---------- + factor : float + The scaling factor. + + Returns + ------- + :class:`Waypoints` + The scaled waypoints. + """ + raise NotImplementedError + + +class FrameWaypoints(Waypoints): + """Represents a sequence of fully constrained pose target for the robot's end-effector using a :class:`compas.geometry.Frame`. + + When using a FrameWaypoints, the end-effector has no translational or rotational freedom. + In another words, the pose of the end-effector is fully defined (constrained). + + The behavior of FrameWaypoints is similar to :class:`FrameTarget`, but it represents a sequence of targets. + + Attributes + ---------- + target_frames : :obj:`list` of :class:`compas.geometry.Frame` + The target frames. + 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 human-readable name of the target. + Defaults to 'Frame Waypoints'. + + """ + + def __init__( + self, + target_frames, + tolerance_position=None, + tolerance_orientation=None, + tool_coordinate_frame=None, + name="Frame Waypoints", + ): + super(FrameWaypoints, self).__init__(name=name) + self.target_frames = target_frames + self.tolerance_position = tolerance_position + self.tolerance_orientation = tolerance_orientation + if isinstance(tool_coordinate_frame, Transformation): + tool_coordinate_frame = Frame.from_transformation(tool_coordinate_frame) + self.tool_coordinate_frame = tool_coordinate_frame + + @property + def __data__(self): + return { + "target_frames": self.target_frames, + "tolerance_position": self.tolerance_position, + "tolerance_orientation": self.tolerance_orientation, + "tool_coordinate_frame": self.tool_coordinate_frame, + "name": self.name, + } + + @classmethod + def from_transformations( + cls, + transformations, + tolerance_position=None, + tolerance_orientation=None, + tool_coordinate_frame=None, + name="Frame Waypoints", + ): + """Creates a FrameWaypoints from a list of transformation matrices. + + Parameters + ---------- + transformations : :obj:`list` of :class: `compas.geometry.Transformation` + The list of transformation matrices. + 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 human-readable name of the target. + Defaults to 'Frame Target'. + + Returns + ------- + :class:`FrameWaypoints` + The frame waypoints. + """ + frames = [Frame.from_transformation(transformation) for transformation in transformations] + return cls(frames, tolerance_position, tolerance_orientation, tool_coordinate_frame, name) + + def scaled(self, factor): + """Returns a copy of the :class:`FrameWaypoints` where the target frames 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 FrameWaypoints should be scaled by a factor + of 0.001 before passing to the planner. + + Parameters + ---------- + factor : float + The scaling factor. + + Returns + ------- + :class:`FrameWaypoints` + The scaled frame waypoints. + """ + target_frames = [frame.scaled(factor) for frame in self.target_frames] + tolerance_position = self.tolerance_position * factor + tolerance_orientation = self.tolerance_orientation + tool_coordinate_frame = self.tool_coordinate_frame.scaled(factor) if self.tool_coordinate_frame else None + return FrameWaypoints( + target_frames, tolerance_position, tolerance_orientation, tool_coordinate_frame, self.name + ) + + +class PointAxisWaypoints(Waypoints): + """ + Represents a sequence of point and axis targets for the robot's end-effector motion planning. + + PointAxisTarget is suitable for tasks like drawing, 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 behavior of PointAxisWaypoints is similar to :class:`PointAxisTarget`, but it represents a sequence of targets. + See :class:`PointAxisTarget` for more details. + + Attributes + ---------- + target_points_and_axes : :obj:`list` of :obj:`tuple` of (:class:`compas.geometry.Point`, :class:`compas.geometry.Vector`) + The target points and axes. + Both values are defined relative to the world coordinate frame (WCF). + Unitized vectors are recommended for the target axes. + 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. + name : str, optional + The human-readable name of the target. + Defaults to 'Point-Axis Waypoints'. + + """ + + def __init__( + self, + target_points_and_axes, + tolerance_position=None, + tool_coordinate_frame=None, + name="Point-Axis Waypoints", + ): + super(PointAxisWaypoints, self).__init__(name=name) + self.target_points_and_axes = target_points_and_axes + self.tolerance_position = tolerance_position + self.tool_coordinate_frame = tool_coordinate_frame + + @property + def __data__(self): + return { + "target_points_and_axes": self.target_points_and_axes, + "tolerance_position": self.tolerance_position, + "tool_coordinate_frame": self.tool_coordinate_frame, + "name": self.name, + } + + def scaled(self, factor): + """Returns a copy of the target where the target points and tolerances are scaled. + + Parameters + ---------- + factor : float + The scaling factor. + + Returns + ------- + :class:`PointAxisWaypoints` + The scaled point-axis waypoints. + """ + # Axis is a unitized vector and is not scaled + target_points_and_axes = [(point.scaled(factor), axis) for point, axis in self.target_points_and_axes] + tolerance_position = self.tolerance_position * factor if self.tolerance_position else None + tool_coordinate_frame = self.tool_coordinate_frame.scaled(factor) if self.tool_coordinate_frame else None + return PointAxisWaypoints(target_points_and_axes, tolerance_position, tool_coordinate_frame, self.name) diff --git a/tests/api/compas_fab_api.json b/tests/api/compas_fab_api.json index c74f76f15..403e86b72 100644 --- a/tests/api/compas_fab_api.json +++ b/tests/api/compas_fab_api.json @@ -1,7 +1,7 @@ { "metadata": { - "compas_fab_version": "1.0.0", - "generated_on": "20240220" + "compas_fab_version": "1.0.2", + "generated_on": "20240503" }, "modules": { "compas_fab": [ @@ -13,6 +13,7 @@ "AnalyticalPlanCartesianMotion", "AnalyticalPyBulletClient", "BackendError", + "BackendFeatureNotSupportedError", "CancellableFutureResult", "CartesianMotionError", "CollisionError", @@ -41,9 +42,13 @@ "AttachedCollisionMesh", "BoundingVolume", "CollisionMesh", + "ConfigurationTarget", "Constraint", + "ConstraintSetTarget", "DeviationVectorsGenerator", "Duration", + "FrameTarget", + "FrameWaypoints", "Inertia", "JointConstraint", "JointTrajectory", @@ -51,12 +56,17 @@ "OrientationConstraint", "OrthonormalVectorsFromAxisGenerator", "PlanningScene", + "PointAxisTarget", + "PointAxisWaypoints", "PositionConstraint", "ReachabilityMap", "Robot", + "RobotLibrary", "RobotSemantics", + "Target", "Tool", "Trajectory", + "Waypoints", "Wrench", "to_degrees", "to_radians" @@ -77,6 +87,7 @@ "argsort", "clamp", "diffs", + "from_tcf_to_t0cf", "list_files_in_directory", "map_range", "range_geometric_row", diff --git a/tests/api/compas_fab_api_ipy.json b/tests/api/compas_fab_api_ipy.json index 120b14f6f..31a94a59b 100644 --- a/tests/api/compas_fab_api_ipy.json +++ b/tests/api/compas_fab_api_ipy.json @@ -1,85 +1,94 @@ { "metadata": { - "compas_fab_version": "1.0.0-20436a91", + "compas_fab_version": "1.0.0-20436a91", "generated_on": "20240220" - }, + }, "modules": { "compas_fab": [ "get" - ], + ], "compas_fab.backends": [ - "ABB_IRB4600_40_255Kinematics", - "AnalyticalInverseKinematics", - "AnalyticalPlanCartesianMotion", - "BackendError", - "CancellableFutureResult", - "CartesianMotionError", - "FutureResult", - "InverseKinematicsError", - "KinematicsError", - "MoveItPlanner", - "OffsetWristKinematics", - "RosClient", - "RosError", - "RosFileServerLoader", - "RosValidationError", - "SphericalWristKinematics", - "Staubli_TX260LKinematics", - "UR10Kinematics", - "UR10eKinematics", - "UR3Kinematics", - "UR3eKinematics", - "UR5Kinematics", + "ABB_IRB4600_40_255Kinematics", + "AnalyticalInverseKinematics", + "AnalyticalPlanCartesianMotion", + "BackendError", + "CancellableFutureResult", + "CartesianMotionError", + "FutureResult", + "InverseKinematicsError", + "KinematicsError", + "MoveItPlanner", + "OffsetWristKinematics", + "RosClient", + "RosError", + "RosFileServerLoader", + "RosValidationError", + "SphericalWristKinematics", + "Staubli_TX260LKinematics", + "UR10Kinematics", + "UR10eKinematics", + "UR3Kinematics", + "UR3eKinematics", + "UR5Kinematics", "UR5eKinematics" - ], + ], "compas_fab.robots": [ - "AttachedCollisionMesh", - "BoundingVolume", - "CollisionMesh", - "Constraint", - "DeviationVectorsGenerator", - "Duration", - "Inertia", - "JointConstraint", - "JointTrajectory", - "JointTrajectoryPoint", - "OrientationConstraint", - "OrthonormalVectorsFromAxisGenerator", - "PlanningScene", - "PositionConstraint", - "ReachabilityMap", - "Robot", - "RobotSemantics", - "Tool", - "Trajectory", - "Wrench", - "to_degrees", + "AttachedCollisionMesh", + "BoundingVolume", + "CollisionMesh", + "ConfigurationTarget", + "Constraint", + "ConstraintSetTarget", + "DeviationVectorsGenerator", + "Duration", + "FrameTarget", + "FrameWaypoints", + "Inertia", + "JointConstraint", + "JointTrajectory", + "JointTrajectoryPoint", + "OrientationConstraint", + "OrthonormalVectorsFromAxisGenerator", + "PlanningScene", + "PointAxisTarget", + "PointAxisWaypoints", + "PositionConstraint", + "ReachabilityMap", + "Robot", + "RobotLibrary", + "RobotSemantics", + "Target", + "Tool", + "Trajectory", + "Waypoints", + "Wrench", + "to_degrees", "to_radians" - ], + ], "compas_fab.sensors": [ - "PosCon3D", - "PosConCM", - "ProtocolError", - "SensorTimeoutError", + "PosCon3D", + "PosConCM", + "ProtocolError", + "SensorTimeoutError", "SerialSensor" - ], + ], "compas_fab.utilities": [ - "LazyLoader", - "allclose", - "arange", - "argmax", - "argmin", - "argsort", - "clamp", - "diffs", - "list_files_in_directory", - "map_range", - "range_geometric_row", - "read_csv_to_dictionary", - "read_data_from_json", - "read_data_from_pickle", - "sign", - "write_data_to_json", + "LazyLoader", + "allclose", + "arange", + "argmax", + "argmin", + "argsort", + "clamp", + "diffs", + "list_files_in_directory", + "map_range", + "range_geometric_row", + "read_csv_to_dictionary", + "read_data_from_json", + "read_data_from_pickle", + "sign", + "write_data_to_json", "write_data_to_pickle" ] } diff --git a/tests/robots/test_targets.py b/tests/robots/test_targets.py new file mode 100644 index 000000000..7577ff316 --- /dev/null +++ b/tests/robots/test_targets.py @@ -0,0 +1,226 @@ +import pytest + +from compas_fab.robots import FrameTarget +from compas_fab.robots import PointAxisTarget +from compas_fab.robots import ConfigurationTarget +from compas_fab.robots import ConstraintSetTarget +from compas_fab.robots import JointConstraint +from compas_fab.robots import OrientationConstraint +from compas_fab.robots import PositionConstraint + +from compas_fab.robots import FrameWaypoints +from compas_fab.robots import PointAxisWaypoints + +from compas_robots import Configuration +from compas_robots.model import Joint + +from compas.geometry import Frame +from compas.geometry import Point +from compas.geometry import Vector + + +@pytest.fixture +def target_frame(): + return Frame(Point(1.0, -2.0, 3.0), Vector(1.0, 0.0, 0.0), Vector(0.0, 1.0, 0.0)) + + +@pytest.fixture +def tool_coordinate_frame(): + return Frame(Point(0.0, 10.0, 20.0), Vector(1.0, 0.0, 0.0), Vector(0.0, 1.0, 0.0)) + + +@pytest.fixture +def target_configuration(): + return Configuration.from_prismatic_and_revolute_values( + [10.0, 20.0], [1.0, 2.0, 3.0, 4.0, 5.0, 6.0], ["J1", "J2", "J3", "J4", "J5", "J6", "J7", "J8"] + ) + + +@pytest.fixture +def frame_target(target_frame, tool_coordinate_frame): + tolerance_position = 0.001 + tolerance_orientation = 0.001 + name = "my testing name" + return FrameTarget(target_frame, tolerance_position, tolerance_orientation, tool_coordinate_frame, name) + + +@pytest.fixture +def point_axis_target(tool_coordinate_frame): + target_point = Point(1.0, -2.0, 3.0) + target_vector = Vector(1.0, -1.0, 0.0) + tolerance_position = 0.001 + name = "my testing name" + return PointAxisTarget(target_point, target_vector, tolerance_position, tool_coordinate_frame, name) + + +@pytest.fixture +def configuration_target(target_configuration): + tolerance_above = [0.01] * 8 + tolerance_below = [0.0009] * 8 + name = "my testing name" + return ConfigurationTarget(target_configuration, tolerance_above, tolerance_below, name) + + +def test_serialization_targets(frame_target, point_axis_target, configuration_target): + # FrameTarget + nt = FrameTarget.__from_data__(frame_target.__data__) + assert frame_target.target_frame == nt.target_frame + assert frame_target.tool_coordinate_frame == nt.tool_coordinate_frame + assert frame_target.tolerance_position == nt.tolerance_position + assert frame_target.tolerance_orientation == nt.tolerance_orientation + assert frame_target.name == nt.name + + # PointAxisTarget + nt = PointAxisTarget.__from_data__(point_axis_target.__data__) + assert point_axis_target.target_point == nt.target_point + assert point_axis_target.target_z_axis == nt.target_z_axis + assert point_axis_target.tool_coordinate_frame == nt.tool_coordinate_frame + assert point_axis_target.tolerance_position == nt.tolerance_position + assert point_axis_target.name == nt.name + + # ConfigurationTarget + nt = ConfigurationTarget.__from_data__(configuration_target.__data__) + assert configuration_target.target_configuration.close_to(nt.target_configuration) + assert configuration_target.tolerance_above == nt.tolerance_above + assert configuration_target.tolerance_below == nt.tolerance_below + assert configuration_target.name == nt.name + + +def test_serialization_constraint_sets(target_frame, tool_coordinate_frame, target_configuration): + tolerance_above = [0.01] * 8 + tolerance_below = [0.0009] * 8 + name = "my testing name" + + # ConstraintSetTarget with JointConstraint + constraint_set = JointConstraint.joint_constraints_from_configuration( + target_configuration, tolerance_above, tolerance_below + ) + target = ConstraintSetTarget(constraint_set, name) + nt = ConstraintSetTarget.__from_data__(target.__data__) + assert target.constraint_set == nt.constraint_set + assert target.name == nt.name + for c1, c2 in zip(target.constraint_set, nt.constraint_set): + assert c1.__data__ == c2.__data__ + + # ConstraintSetTarget with OrientationConstraint and PositionConstraint + link_name = "tool0" + tolerances_orientation = [0.0123] * 3 + orientation_constraint_weight = 0.789 + orientation_constraint = OrientationConstraint.from_frame( + target_frame, tolerances_orientation, link_name, tool_coordinate_frame, orientation_constraint_weight + ) + position_constraint_weight = 0.456 + tolerances_position = 0.567 + position_constraint = PositionConstraint.from_frame( + target_frame, tolerances_position, link_name, tool_coordinate_frame, position_constraint_weight + ) + target = ConstraintSetTarget([orientation_constraint, position_constraint], name) + nt = ConstraintSetTarget.__from_data__(target.__data__) + assert target.constraint_set == nt.constraint_set + assert target.name == nt.name + for c1, c2 in zip(target.constraint_set, nt.constraint_set): + assert c1.__data__ == c2.__data__ + + +@pytest.fixture +def frame_waypoints(): + target_frames = [] + target_frames.append(Frame(Point(1.0, -2.0, 3.0), Vector(1.0, 0.0, 0.0), Vector(0.0, 1.0, 0.0))) + target_frames.append(Frame(Point(4.0, -5.0, 6.0), Vector(1.0, 0.0, 0.0), Vector(0.0, 1.0, 0.0))) + target_frames.append(Frame(Point(7.0, -8.0, 9.0), Vector(-1.0, 0.0, 0.0), Vector(0.0, 1.0, 0.0))) + tolerance_position = 0.001 + tolerance_orientation = 0.001 + tool_coordinate_frame = Frame(Point(0.0, 10.0, 20.0), Vector(1.0, 0.0, 0.0), Vector(0.0, 1.0, 0.0)) + name = "my testing waypoints" + return FrameWaypoints(target_frames, tolerance_position, tolerance_orientation, tool_coordinate_frame, name) + + +@pytest.fixture +def point_axis_waypoints(): + target_points_and_axes = [] + target_points_and_axes.append((Point(1.0, -2.0, 3.0), Vector(1.0, 0.0, 0.0))) + target_points_and_axes.append((Point(4.0, -5.0, 6.0), Vector(1.0, 0.0, 0.0))) + target_points_and_axes.append((Point(7.0, -8.0, 9.0), Vector(-1.0, 0.0, 0.0))) + tolerance_position = 0.001 + tool_coordinate_frame = Frame(Point(0.0, 10.0, 20.0), Vector(1.0, 0.0, 0.0), Vector(0.0, 1.0, 0.0)) + name = "my testing waypoints" + return PointAxisWaypoints(target_points_and_axes, tolerance_position, tool_coordinate_frame, name) + + +def test_serialization_waypoints(frame_waypoints, point_axis_waypoints): + # FrameWaypoints + nt = FrameWaypoints.__from_data__(frame_waypoints.__data__) + for f1, f2 in zip(frame_waypoints.target_frames, nt.target_frames): + assert f1 == f2 + assert frame_waypoints.tolerance_position, nt.tolerance_position + assert frame_waypoints.tolerance_orientation, nt.tolerance_orientation + assert frame_waypoints.tool_coordinate_frame == nt.tool_coordinate_frame + assert frame_waypoints.name == nt.name + + # PointAxisWaypoints + nt = PointAxisWaypoints.__from_data__(point_axis_waypoints.__data__) + for (p1, a1), (p2, a2) in zip(point_axis_waypoints.target_points_and_axes, nt.target_points_and_axes): + assert p1 == p2 + assert a1 == a2 + assert point_axis_waypoints.tolerance_position == nt.tolerance_position + assert point_axis_waypoints.tool_coordinate_frame == nt.tool_coordinate_frame + assert point_axis_waypoints.name == nt.name + + +def test_target_scale(frame_target): + scale_factor = 0.001 + nt = frame_target.scaled(scale_factor) + assert nt.target_frame == frame_target.target_frame.scaled(scale_factor) + assert nt.tolerance_position == frame_target.tolerance_position * scale_factor + # orientation doesn't need scale + assert nt.tolerance_orientation == frame_target.tolerance_orientation + assert nt.tool_coordinate_frame == frame_target.tool_coordinate_frame.scaled(scale_factor) + + +def test_point_axis_target_scale(point_axis_target): + scale_factor = 0.001 + nt = point_axis_target.scaled(scale_factor) + assert nt.target_point == point_axis_target.target_point.scaled(scale_factor) + assert nt.target_z_axis == point_axis_target.target_z_axis + assert nt.tolerance_position == point_axis_target.tolerance_position * scale_factor + assert nt.tool_coordinate_frame == point_axis_target.tool_coordinate_frame.scaled(scale_factor) + + +def test_configuration_target_scale(configuration_target): + scale_factor = 0.001 + nt = configuration_target.scaled(scale_factor) + assert ( + nt.target_configuration.joint_values + == configuration_target.target_configuration.scaled(scale_factor).joint_values + ) + assert nt.target_configuration.joint_types == configuration_target.target_configuration.joint_types + assert nt.target_configuration.joint_names == configuration_target.target_configuration.joint_names + for i, joint_type in enumerate(configuration_target.target_configuration.joint_types): + if joint_type in (Joint.PLANAR, Joint.PRISMATIC): + assert ( + nt.target_configuration.joint_values[i] + == configuration_target.target_configuration.joint_values[i] * scale_factor + ) + else: + assert nt.target_configuration.joint_values[i] == configuration_target.target_configuration.joint_values[i] + + +def test_frame_waypoints_scale(frame_waypoints): + scale_factor = 0.001 + nt = frame_waypoints.scaled(scale_factor) + assert nt.tolerance_position == frame_waypoints.tolerance_position * scale_factor + # orientation doesn't need scale + assert nt.tolerance_orientation == frame_waypoints.tolerance_orientation + for f1, f2 in zip(frame_waypoints.target_frames, nt.target_frames): + assert f1.scaled(scale_factor) == f2 + assert nt.tool_coordinate_frame == frame_waypoints.tool_coordinate_frame.scaled(scale_factor) + + +def test_point_axis_waypoints_scale(point_axis_waypoints): + scale_factor = 0.001 + nt = point_axis_waypoints.scaled(scale_factor) + assert nt.tolerance_position == point_axis_waypoints.tolerance_position * scale_factor + for (p1, a1), (p2, a2) in zip(point_axis_waypoints.target_points_and_axes, nt.target_points_and_axes): + assert p1.scaled(scale_factor) == p2 + assert a1 == a2 + assert nt.tool_coordinate_frame == point_axis_waypoints.tool_coordinate_frame.scaled(scale_factor)