From 11816efdc3639c57e2525d198b8c4bd62b6c9881 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 10 May 2024 22:00:45 +0800 Subject: [PATCH] type hint for Robot Time Constraints and Trajectory --- src/compas_fab/robots/constraints.py | 79 +++++++++++++++++++++++----- src/compas_fab/robots/robot.py | 1 + src/compas_fab/robots/time_.py | 2 + src/compas_fab/robots/trajectory.py | 28 ++++++++++ 4 files changed, 96 insertions(+), 14 deletions(-) diff --git a/src/compas_fab/robots/constraints.py b/src/compas_fab/robots/constraints.py index 45f088a36c..d2fb106fe1 100644 --- a/src/compas_fab/robots/constraints.py +++ b/src/compas_fab/robots/constraints.py @@ -2,6 +2,8 @@ from __future__ import division from __future__ import print_function +import compas + from compas.data import Data from compas.geometry import Rotation from compas.geometry import Scale @@ -9,6 +11,19 @@ from compas_fab.utilities import from_tcf_to_t0cf +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import Optional # noqa: F401 + from typing import Union # noqa: F401 + from compas.datastructures import Mesh # noqa: F401 + from compas.geometry import Box # noqa: F401 + from compas.geometry import Frame # noqa: F401 + from compas.geometry import Point # noqa: F401 + from compas.geometry import Transformation # noqa: F401 + from compas_robots import Configuration # noqa: F401 + __all__ = ["BoundingVolume", "Constraint", "JointConstraint", "OrientationConstraint", "PositionConstraint"] @@ -56,6 +71,7 @@ class BoundingVolume(Data): VOLUME_TYPES = (BOX, SPHERE, MESH) def __init__(self, volume_type, volume): + # type: (int, Union[Box, Sphere, Mesh]) -> None if volume_type not in self.VOLUME_TYPES: raise ValueError("Type must be one of {}".format(self.VOLUME_TYPES)) self.type = volume_type @@ -69,6 +85,7 @@ def __data__(self): @classmethod def from_box(cls, box): + # type: (Box) -> BoundingVolume """Create a :class:`BoundingVolume` from a :class:`compas.geometry.Box`. Parameters @@ -94,6 +111,7 @@ def from_box(cls, box): @classmethod def from_sphere(cls, sphere): + # type: (Sphere) -> BoundingVolume """Create a :class:`BoundingVolume` from a :class:`compas.geometry.Sphere`. Parameters @@ -118,6 +136,7 @@ def from_sphere(cls, sphere): @classmethod def from_mesh(cls, mesh): + # type: (Mesh) -> BoundingVolume """Create a :class:`BoundingVolume` from a :class:`compas.datastructures.Mesh`. Parameters @@ -142,6 +161,7 @@ def from_mesh(cls, mesh): return cls(cls.MESH, mesh) def scale(self, scale_factor): + # type: (float) -> None """Scale the volume uniformly. Parameters @@ -153,6 +173,7 @@ def scale(self, scale_factor): self.transform(S) def transform(self, transformation): + # type: (Transformation) -> None """Transform the volume using a :class:`compas.geometry.Transformation`. Parameters @@ -167,6 +188,7 @@ def __repr__(self): return "BoundingVolume({!r}, {!r})".format(self.type, self.volume) def copy(self): + # type: () -> BoundingVolume """Make a copy of this :class:`BoundingVolume`. Returns @@ -221,6 +243,7 @@ class Constraint(Data): CONSTRAINT_TYPES = (JOINT, POSITION, ORIENTATION) def __init__(self, constraint_type, weight=1.0): + # type: (int, Optional[float]) -> None if constraint_type not in self.CONSTRAINT_TYPES: raise ValueError("Type must be %d, %d or %d" % self.CONSTRAINT_TYPES) self.type = constraint_type @@ -233,14 +256,17 @@ def __data__(self): } def transform(self, transformation): + # type: (Transformation) -> None """Transform the :class:`Constraint`.""" pass def scale(self, scale_factor): + # type: (float) -> None """Scale the :class:`Constraint`.""" pass def scaled(self, scale_factor): + # type: (float) -> Constraint """Get a scaled copy of this :class:`Constraint`. Parameters @@ -253,6 +279,7 @@ def scaled(self, scale_factor): return c def copy(self): + # type: () -> Constraint """Create a copy of this :class:`Constraint`. Returns @@ -269,7 +296,7 @@ class JointConstraint(Constraint): Parameters ---------- joint_name : :obj:`str` - The name of the joint this contraint refers to. + The name of the joint this constraint refers to. value : :obj:`float` The targeted value for that joint. tolerance_above : :obj:`float` @@ -284,7 +311,7 @@ class JointConstraint(Constraint): Attributes ---------- joint_name : :obj:`str` - The name of the joint this contraint refers to. + The name of the joint this constraint refers to. value : :obj:`float` The targeted value for that joint. tolerance_above : :obj:`float` @@ -302,6 +329,7 @@ class JointConstraint(Constraint): """ def __init__(self, joint_name, value, tolerance_above=0.0, tolerance_below=0.0, weight=1.0): + # type: (str, float, Optional[float], Optional[float], Optional[float]) -> None super(JointConstraint, self).__init__(self.JOINT, weight) self.joint_name = joint_name self.value = value @@ -318,6 +346,7 @@ def __data__(self): } def scale(self, scale_factor): + # type: (float) -> None """Scale (multiply) the constraint with a factor. Parameters @@ -336,6 +365,7 @@ def __repr__(self): ) def copy(self): + # type: () -> JointConstraint """Create a copy of this :class:`JointConstraint`. Returns @@ -347,6 +377,7 @@ def copy(self): @classmethod def joint_constraints_from_configuration(cls, configuration, tolerances_above, tolerances_below): + # type: (Configuration, list, list) -> list[JointConstraint] """Create joint constraints for all joints of the configuration. One constraint is created for each joint. @@ -425,7 +456,7 @@ class OrientationConstraint(Constraint): Parameters ---------- link_name : :obj:`str` - The name of the link this contraint refers to. + The name of the link this constraint refers to. quaternion : :obj:`list` of :obj:`float` The desired orientation of the link specified by a quaternion in the order of ``[w, x, y, z]``. @@ -442,17 +473,19 @@ class OrientationConstraint(Constraint): Attributes ---------- link_name : :obj:`str` - The name of the link this contraint refers to. + The name of the link this constraint refers to. quaternion : :obj:`list` of :obj:`float` The desired orientation of the link specified by a quaternion in the order of ``[w, x, y, z]``. - tolerances : :obj:`list` of :obj:`float` + tolerances : :obj:`list` of :obj:`float`, optional 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` + Defaults to ``[0.01, 0.01, 0.01]``. + 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.0``. Notes ----- @@ -472,10 +505,16 @@ class OrientationConstraint(Constraint): """ def __init__(self, link_name, quaternion, tolerances=None, weight=1.0): + # type: (str, list[float], Optional[list[float]], Optional[float]) -> None super(OrientationConstraint, self).__init__(self.ORIENTATION, weight) self.link_name = link_name self.quaternion = [float(a) for a in list(quaternion)] - self.tolerances = [float(a) for a in list(tolerances)] if tolerances else [0.01] * 3 + if isinstance(tolerances, list): + self.tolerances = [float(a) for a in list(tolerances)] + elif isinstance(tolerances, float): + self.tolerances = [tolerances] * 3 + else: + self.tolerances = [0.01, 0.01, 0.01] def __data__(self): return { @@ -486,6 +525,7 @@ def __data__(self): } def transform(self, transformation): + # type: (Transformation) -> None """Transform the volume using a :class:`compas.geometry.Transformation`. Parameters @@ -504,6 +544,7 @@ def __repr__(self): ) def copy(self): + # type: () -> OrientationConstraint """Create a copy of this :class:`OrientationConstraint`. Returns @@ -515,6 +556,7 @@ def copy(self): @classmethod def from_frame(cls, frame_WCF, tolerances_orientation, link_name, tool_coordinate_frame=None, weight=1.0): + # type: (Frame, list[float], str, Optional[Frame], Optional[float]) -> OrientationConstraint """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. @@ -584,7 +626,7 @@ class PositionConstraint(Constraint): Parameters ---------- link_name : :obj:`str` - The name of the link this contraint refers to. + The name of the link this constraint refers to. bounding_volume : :class:`BoundingVolume` The volume this constraint refers to. weight : :obj:`float`, optional @@ -595,7 +637,7 @@ class PositionConstraint(Constraint): Attributes ---------- link_name : :obj:`str` - The name of the link this contraint refers to. + The name of the link this constraint refers to. bounding_volume : :class:`BoundingVolume` The volume this constraint refers to. weight : :obj:`float` @@ -612,6 +654,7 @@ class PositionConstraint(Constraint): """ def __init__(self, link_name, bounding_volume, weight=1.0): + # type: (str, BoundingVolume, Optional[float]) -> None super(PositionConstraint, self).__init__(self.POSITION, weight) self.link_name = link_name self.bounding_volume = bounding_volume @@ -626,6 +669,7 @@ def __data__(self): @classmethod def from_frame(cls, frame_WCF, tolerance_position, link_name, tool_coordinate_frame=None, weight=1.0): + # type: (Frame, float, str, Optional[Frame], Optional[float]) -> PositionConstraint """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. @@ -674,12 +718,13 @@ def from_frame(cls, frame_WCF, tolerance_position, link_name, tool_coordinate_fr @classmethod def from_box(cls, link_name, box, weight=1.0): + # type: (str, Box, Optional[float]) -> PositionConstraint """Create a :class:`PositionConstraint` from a :class:`compas.geometry.Box`. Parameters ---------- link_name: :obj:`str` - The name of the link this contraint refers to. + The name of the link this constraint refers to. box : :class:`compas.geometry.Box` Box defining the bounding volume this constraint refers to. weight : :obj:`float`, optional @@ -702,12 +747,13 @@ def from_box(cls, link_name, box, weight=1.0): @classmethod def from_sphere(cls, link_name, sphere, weight=1.0): + # type: (str, Sphere, Optional[float]) -> PositionConstraint """Create a :class:`PositionConstraint` from a :class:`compas.geometry.Sphere`. Parameters ---------- link_name : :obj:`str` - The name of the link this contraint refers to. + The name of the link this constraint refers to. sphere : :class:`compas.geometry.Sphere` Sphere defining the bounding volume this constraint refers to. weight : :obj:`float` @@ -730,12 +776,13 @@ def from_sphere(cls, link_name, sphere, weight=1.0): @classmethod def from_point(cls, link_name, point, tolerance_position, weight=1.0): + # type: (str, Point, float, Optional[float]) -> PositionConstraint """Create a :class:`PositionConstraint` from a point. Parameters ---------- link_name : :obj:`str` - The name of the link this contraint refers to. + The name of the link this constraint refers to. point : :class:`compas.geometry.Point` Point defining the bounding volume this constraint refers to. tolerance_position : :obj:`float` @@ -755,12 +802,13 @@ def from_point(cls, link_name, point, tolerance_position, weight=1.0): @classmethod def from_mesh(cls, link_name, mesh, weight=1.0): + # type: (str, Mesh, Optional[float]) -> PositionConstraint """Create a :class:`PositionConstraint` from a :class:`compas.datastructures.Mesh`. Parameters ---------- link_name : :obj:`str` - The name of the link this contraint refers to. + The name of the link this constraint refers to. mesh : :class:`compas.datastructures.Mesh` Mesh defining the bounding volume this constraint refers to. weight : :obj:`float` @@ -783,7 +831,8 @@ def from_mesh(cls, link_name, mesh, weight=1.0): return cls(link_name, bounding_volume, weight) def scale(self, scale_factor): - """Scale the :attr:`bounding_volume` uniformely. + # type: (float) -> None + """Scale the :attr:`bounding_volume` uniformly. Parameters ---------- @@ -793,6 +842,7 @@ def scale(self, scale_factor): self.bounding_volume.scale(scale_factor) def transform(self, transformation): + # type: (Transformation) -> None """Transform the :attr:`bounding_volume` using a :class:`compas.geometry.Transformation`. Parameters @@ -806,6 +856,7 @@ def __repr__(self): return "PositionConstraint({!r}, {!r}, {!r})".format(self.link_name, self.bounding_volume, self.weight) def copy(self): + # type: () -> PositionConstraint """Create a copy of this :class:`PositionConstraint`. Returns diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index fa637643b0..71525c2c56 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -24,6 +24,7 @@ from typing import List # noqa: F401 from typing import Optional # noqa: F401 from typing import Tuple # noqa: F401 + from compas.geometry import Vector # noqa: F401 from compas_fab.backends.interfaces import ClientInterface # noqa: F401 from compas_fab.robots import RobotSemantics # noqa: F401 from compas_fab.robots import Tool # noqa: F401 diff --git a/src/compas_fab/robots/time_.py b/src/compas_fab/robots/time_.py index 9ddd54e16f..07d176fce5 100644 --- a/src/compas_fab/robots/time_.py +++ b/src/compas_fab/robots/time_.py @@ -50,6 +50,7 @@ class Duration(Data): """ def __init__(self, secs, nsecs): + # type: (int | float, int) -> None super(Duration, self).__init__() sec_to_nano_factor = 1e9 quotient, remainder = divmod(secs, 1) @@ -79,6 +80,7 @@ def __ne__(self, other): @property def seconds(self): + # type: () -> float return self.secs + 1e-9 * self.nsecs @property diff --git a/src/compas_fab/robots/trajectory.py b/src/compas_fab/robots/trajectory.py index fd3070a00d..57bee09b9b 100644 --- a/src/compas_fab/robots/trajectory.py +++ b/src/compas_fab/robots/trajectory.py @@ -2,6 +2,8 @@ from __future__ import division from __future__ import print_function +import compas + from compas.data import Data from compas.tolerance import TOL from compas_robots import Configuration @@ -10,6 +12,14 @@ from compas_fab.robots import AttachedCollisionMesh from compas_fab.robots.time_ import Duration +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import Any # noqa: F401 + from typing import Dict # noqa: F401 + from typing import List # noqa: F401 + __all__ = [ "JointTrajectory", "JointTrajectoryPoint", @@ -75,6 +85,7 @@ def __init__( time_from_start=None, joint_names=None, ): + # type: (List[float], List[int], List[float], List[float], List[float], Duration, List[str]) -> None super(JointTrajectoryPoint, self).__init__(joint_values, joint_types, joint_names) self.velocities = velocities or len(self.joint_values) * [0.0] self.accelerations = accelerations or len(self.joint_values) * [0.0] @@ -94,16 +105,19 @@ def __str__(self): @property def positions(self): + # type: () -> List[float] """:obj:`list` of :obj:`float` : Alias of `joint_values`.""" return self.joint_values @property def velocities(self): + # type: () -> List[float] """:obj:`list` of :obj:`float` : Velocity of each joint.""" return self._velocities @velocities.setter def velocities(self, velocities): + # type: (List[float]) -> None if len(self.joint_values) != len(velocities): raise ValueError("Must have {} velocities, but {} given.".format(len(self.joint_values), len(velocities))) @@ -111,11 +125,13 @@ def velocities(self, velocities): @property def accelerations(self): + # type: () -> List[float] """:obj:`list` of :obj:`float` : Acceleration of each joint.""" return self._accelerations @accelerations.setter def accelerations(self, accelerations): + # type: (List[float]) -> None if len(self.joint_values) != len(accelerations): raise ValueError( "Must have {} accelerations, but {} given.".format(len(self.joint_values), len(accelerations)) @@ -125,11 +141,13 @@ def accelerations(self, accelerations): @property def effort(self): + # type: () -> List[float] """:obj:`list` of :obj:`float` : Effort of each joint.""" return self._effort @effort.setter def effort(self, effort): + # type: (List[float]) -> None if len(self.joint_values) != len(effort): raise ValueError("Must have {} efforts, but {} given.".format(len(self.joint_values), len(effort))) @@ -153,6 +171,7 @@ def __data__(self): @classmethod def __from_data__(cls, data): + # type: (Dict[str, Any]) -> JointTrajectoryPoint joint_values = FixedLengthList(data.get("joint_values") or data.get("values") or []) joint_types = FixedLengthList(data.get("joint_types") or data.get("types") or []) joint_names = FixedLengthList(data.get("joint_names") or []) @@ -174,23 +193,27 @@ def __from_data__(cls, data): @property def velocity_dict(self): + # type: () -> Dict[str, float] """A dictionary of joint velocities by joint name.""" self.check_joint_names() return dict(zip(self.joint_names, self.velocities)) @property def acceleration_dict(self): + # type: () -> Dict[str, float] """A dictionary of joint accelerations by joint name.""" self.check_joint_names() return dict(zip(self.joint_names, self.accelerations)) @property def effort_dict(self): + # type: () -> Dict[str, float] """A dictionary of joint efforts by joint name.""" self.check_joint_names() return dict(zip(self.joint_names, self.effort)) def merged(self, other): + # type: (JointTrajectoryPoint) -> JointTrajectoryPoint """Get a new ``JointTrajectoryPoint`` with this ``JointTrajectoryPoint`` merged with another ``JointTrajectoryPoint``. The other ``JointTrajectoryPoint`` takes precedence over this ``JointTrajectoryPoint`` in case a joint value is present in both. @@ -254,6 +277,7 @@ class Trajectory(Data): """ def __init__(self, attributes=None): + # type: (Dict[str, Any]) -> None super(Trajectory, self).__init__() self.attributes = attributes or {} self.planning_time = -1 @@ -267,6 +291,7 @@ def __data__(self): @classmethod def __from_data__(cls, data): + # type: (Dict[str, Any]) -> Trajectory trajectory = cls(attributes=data["attributes"]) trajectory.planning_time = data["planning_time"] return trajectory @@ -318,6 +343,7 @@ def __init__( attached_collision_meshes=None, attributes=None, ): + # type: (List[JointTrajectoryPoint], List[str], Configuration, float, List[AttachedCollisionMesh], Dict[str, Any]) -> None super(JointTrajectory, self).__init__(attributes=attributes) self.points = trajectory_points or [] self.joint_names = joint_names or [] @@ -341,6 +367,7 @@ def __data__(self): @classmethod def __from_data__(cls, data): + # type: (Dict[str, Any]) -> JointTrajectory points = list(map(JointTrajectoryPoint.__from_data__, data.get("points") or [])) joint_names = data.get("joint_names", []) start_configuration = data.get("start_configuration", None) @@ -364,6 +391,7 @@ def __from_data__(cls, data): @property def time_from_start(self): + # type: () -> float """:obj:`float` : Effectively, time from start for the last point in the trajectory.""" if not self.points: return 0.0