diff --git a/src/compas_fab/backends/interfaces/backend_features.py b/src/compas_fab/backends/interfaces/backend_features.py index af947b375..ef7e7aa15 100644 --- a/src/compas_fab/backends/interfaces/backend_features.py +++ b/src/compas_fab/backends/interfaces/backend_features.py @@ -2,6 +2,18 @@ from __future__ import division from __future__ import print_function +import compas + +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import Optional # noqa: F401 + from typing import Dict # noqa: F401 + from compas_fab.robots import Robot # noqa: F401 + from compas_robots import Configuration # noqa: F401 + from compas.geometry import Frame # noqa: F401 + class ForwardKinematics(object): """Interface for a Planner's forward kinematics feature. Any implementation of @@ -16,6 +28,7 @@ def __call__(self, robot, configuration, group=None, options=None): return self.forward_kinematics(robot, configuration, group, options) def forward_kinematics(self, robot, configuration, group=None, options=None): + # type: (Robot, Configuration, Optional[str], Optional[Dict]) -> Frame """Calculate the robot's forward kinematic. Parameters diff --git a/src/compas_fab/robots/constraints.py b/src/compas_fab/robots/constraints.py index 8e107d03d..bb9ac1499 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. @@ -675,12 +719,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 @@ -703,12 +748,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` @@ -731,12 +777,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` @@ -756,12 +803,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` @@ -784,7 +832,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 ---------- @@ -794,6 +843,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 @@ -807,6 +857,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/inertia.py b/src/compas_fab/robots/inertia.py index 960a1de37..481d83e85 100644 --- a/src/compas_fab/robots/inertia.py +++ b/src/compas_fab/robots/inertia.py @@ -5,7 +5,7 @@ ] -class Inertia: +class Inertia(object): """The moments of inertia represent the spatial distribution of mass in a rigid body. It depends on the mass, size, and shape of a rigid body with units of @@ -16,16 +16,16 @@ class Inertia: Attributes ---------- - inertia_tensor : list of float + inertia_tensor : :obj:`list` of :obj:`float` A symmetric positive-definite 3x3 matrix: | ixx ixy ixz | | ixy iyy iyz | | ixz iyz izz | with [ixx, iyy, izz] as the principal moments of inertia and [ixy, ixz, iyz] as the products of inertia. - mass: float + mass: :obj:`float` The mass of the object in kg. - center_of_mass : :class:`Point` + center_of_mass : :class:`compas.geometry.Point` The center of mass of the object in meters. Examples @@ -39,16 +39,18 @@ class Inertia: Notes ----- Assuming uniform mass density, inertial data can be obtained using the - free software MeshLab, refering to this great `tutorial `_. + free software MeshLab, referring to this great `tutorial `_. """ def __init__(self, inertia_tensor, mass, center_of_mass): + # type: (list[float], float, Point) -> None self.inertia_tensor = inertia_tensor self.mass = mass self.center_of_mass = Point(*center_of_mass) @property def principal_moments(self): + # type: () -> list[float] """Returns the diagonal elements of the inertia tensor [ixx, iyy, izz]""" inertia_tensor = self.inertia_tensor return [inertia_tensor[0][0], inertia_tensor[1][1], inertia_tensor[2][2]] diff --git a/src/compas_fab/robots/planning_scene.py b/src/compas_fab/robots/planning_scene.py index de8f87426..7d0f97e4c 100644 --- a/src/compas_fab/robots/planning_scene.py +++ b/src/compas_fab/robots/planning_scene.py @@ -2,11 +2,23 @@ from __future__ import division from __future__ import print_function +import compas + from compas.data import Data from compas.datastructures import Mesh from compas.geometry import Frame from compas.geometry import Scale +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import Optional # noqa: F401 + from compas_fab.backends.interfaces import ClientInterface # noqa: F401 + from compas_fab.robots import Robot # noqa: F401 + from compas_fab.robots import Tool # noqa: F401 + + __all__ = [ "AttachedCollisionMesh", "CollisionMesh", @@ -22,13 +34,14 @@ class CollisionMesh(Data): mesh : :class:`compas.datastructures.Mesh` The collision mesh. Ideally it is as coarse as possible. id : :obj:`str` - The id of the mesh, used to identify it for later operations + The id of the mesh, used for identifying it for later operations (:meth:`~PlanningScene.add_collision_mesh`, :meth:`~PlanningScene.remove_collision_mesh`, :meth:`~PlanningScene.append_collision_mesh` etc.) frame : :class:`compas.geometry.Frame`, optional - The frame of the mesh. Defaults to :meth:`compas.geometry.Frame.worldXY`. - root_name : :obj:`str` + The base frame of the mesh. + Defaults to :meth:`compas.geometry.Frame.worldXY`. + root_name : :obj:`str`, optional The name of the root link the collision mesh will be placed in. Defaults to ``'world'``. @@ -37,12 +50,13 @@ class CollisionMesh(Data): mesh : :class:`compas.datastructures.Mesh` The collision mesh. Ideally it is as coarse as possible. id : :obj:`str` - The id of the mesh, used to identify it for later operations + The id of the mesh, used for identifying it for later operations (:meth:`~PlanningScene.add_collision_mesh`, :meth:`~PlanningScene.remove_collision_mesh`, :meth:`~PlanningScene.append_collision_mesh` etc.) - frame : :class:`compas.geometry.Frame`, optional - The frame of the mesh. Defaults to :meth:`compas.geometry.Frame.worldXY`. + frame : :class:`compas.geometry.Frame` + The base frame of the mesh. + Defaults to :meth:`compas.geometry.Frame.worldXY`. root_name : :obj:`str` The name of the root link the collision mesh will be placed in. Defaults to ``'world'``. @@ -56,6 +70,7 @@ class CollisionMesh(Data): """ def __init__(self, mesh, id, frame=None, root_name=None): + # type: (Mesh, str, Optional[Frame], Optional[str]) -> None super(CollisionMesh, self).__init__() self.id = id self.mesh = mesh @@ -63,6 +78,7 @@ def __init__(self, mesh, id, frame=None, root_name=None): self.root_name = root_name or "world" def scale(self, scale_factor): + # type: (float) -> None """Scales the collision mesh uniformly. Parameters @@ -74,18 +90,23 @@ def scale(self, scale_factor): self.mesh.transform(S) def scaled(self, scale_factor): + # type: (float) -> None """Copies the collision mesh, and scales the copy uniformly. Parameters ---------- scale_factor : :obj:`float` Scale factor. + + Returns + ------- """ self.mesh = self.mesh.copy() self.scale(scale_factor) @classmethod def __from_data__(cls, data): + # type: (dict) -> CollisionMesh """Construct a collision mesh from its data representation. Parameters @@ -157,6 +178,7 @@ class AttachedCollisionMesh(Data): """ def __init__(self, collision_mesh, link_name, touch_links=None, weight=1.0): + # type: (CollisionMesh, str, Optional[list[str]], Optional[float]) -> None super(AttachedCollisionMesh, self).__init__() self.collision_mesh = collision_mesh if self.collision_mesh: @@ -167,6 +189,7 @@ def __init__(self, collision_mesh, link_name, touch_links=None, weight=1.0): @classmethod def __from_data__(cls, data): + # type: (dict) -> AttachedCollisionMesh """Construct an attached collision mesh from its data representation. Parameters @@ -204,26 +227,29 @@ class PlanningScene(object): Parameters ---------- - robot : :class:`Robot` + robot : :class:`compas_fab.robots.Robot` A reference to the robot in the planning scene. Attributes ---------- - robot : :class:`Robot` + robot : :class:`compas_fab.robots.Robot` A reference to the robot in the planning scene. client A reference to the robot's backend client. """ def __init__(self, robot): + # type: (Robot) -> None self.robot = robot @property def client(self): - """:class:`compas_fab.backend.ClientInterface`: The backend client.""" + # type: () -> ClientInterface + """:class:`compas_fab.backends.interfaces.ClientInterface`: The backend client.""" return self.robot.client def ensure_client(self): + # type: () -> None """Ensure that the planning scene's robot has a defined client. Raises @@ -235,11 +261,13 @@ def ensure_client(self): raise Exception("This method is only callable once a client is assigned") def reset(self): + # type: () -> None """Resets the planning scene, removing all added collision meshes.""" self.ensure_client() self.client.reset_planning_scene() def add_collision_mesh(self, collision_mesh, scale=False): + # type: (CollisionMesh, Optional[bool]) -> None """Add a collision mesh to the planning scene. If there is already a :class:`CollisionMesh` in the @@ -277,6 +305,7 @@ def add_collision_mesh(self, collision_mesh, scale=False): self.client.add_collision_mesh(collision_mesh) def remove_collision_mesh(self, id): + # type: (str) -> None """Remove a collision object from the planning scene. Parameters @@ -299,6 +328,7 @@ def remove_collision_mesh(self, id): self.robot.client.remove_collision_mesh(id) def append_collision_mesh(self, collision_mesh, scale=False): + # type: (CollisionMesh, Optional[bool]) -> None """Append a collision mesh to the planning scene. Appends a :class:`CollisionMesh` to the :class:`PlanningScene` using @@ -342,6 +372,7 @@ def append_collision_mesh(self, collision_mesh, scale=False): self.robot.client.append_collision_mesh(collision_mesh) def add_attached_collision_mesh(self, attached_collision_mesh, scale=False): + # type: (AttachedCollisionMesh, Optional[bool]) -> None """Add an attached collision object to the planning scene. Parameters @@ -379,6 +410,7 @@ def add_attached_collision_mesh(self, attached_collision_mesh, scale=False): self.client.add_attached_collision_mesh(attached_collision_mesh) def remove_attached_collision_mesh(self, id): + # type: (str) -> None """Remove an attached collision object from the planning scene. Parameters @@ -403,6 +435,7 @@ def remove_attached_collision_mesh(self, id): self.client.remove_attached_collision_mesh(id) def attach_collision_mesh_to_robot_end_effector(self, collision_mesh, scale=False, group=None): + # type: (CollisionMesh, Optional[bool], Optional[str]) -> None """Attaches a collision mesh to the robot's end-effector. Parameters @@ -443,6 +476,7 @@ def attach_collision_mesh_to_robot_end_effector(self, collision_mesh, scale=Fals self.add_attached_collision_mesh(acm) def add_attached_tool(self, tool=None, group=None): + # type: (Optional[Tool], Optional[str]) -> None """Add the robot's attached tool to the planning scene if tool is set.""" self.ensure_client() if tool: @@ -454,6 +488,7 @@ def add_attached_tool(self, tool=None, group=None): self.add_attached_collision_mesh(attached_collision_mesh) def remove_attached_tool(self): + # type: () -> None """Remove the robot's attached tool from the planning scene.""" self.ensure_client() diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index f932e266e..93751e833 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -3,6 +3,7 @@ from __future__ import print_function import random +import compas from compas.data import Data from compas.geometry import Frame @@ -13,6 +14,29 @@ from compas_fab.robots.constraints import Constraint +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 Iterator # noqa: F401 + 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 JointTrajectory # noqa: F401 + from compas_fab.robots import RobotSemantics # noqa: F401 + from compas_fab.robots import Tool # noqa: F401 + from compas_fab.robots import Target # noqa: F401 + from compas_fab.robots import Waypoints # noqa: F401 + from compas_fab.robots.planning_scene import CollisionMesh # noqa: F401 + from compas_robots.model import Link # noqa: F401 + from compas_robots.model import Joint # noqa: F401, F811 + from compas_robots.model import Material # noqa: F401 + from compas_robots.scene import BaseRobotModelObject # noqa: F401 + __all__ = [ "Robot", @@ -64,10 +88,11 @@ class Robot(Data): """ # NOTE: If the attribute function has a docstring, the first sentence will be used automatically in the class attribute's. - # However, the rest of the docstring, after the first fullstop symbol will be ignored. - # It is futile to add examples to the attribute docstrings, as they will not be rendered in the documentation. + # However, the rest of the docstring, after the first period symbol will be ignored. + # It is futile to add examples to the attribute docstring, as they will not be rendered in the documentation. def __init__(self, model=None, scene_object=None, semantics=None, client=None): + # type: (RobotModel, Optional[BaseRobotModelObject], Optional[RobotSemantics], Optional[ClientInterface]) -> Robot super(Robot, self).__init__() # These attributes have to be initiated first, # because they are used in the setters of the other attributes @@ -111,6 +136,7 @@ def __from_data__(cls, data): @property def scene_object(self): + # type: () -> BaseRobotModelObject | None """Scene object used to visualize robot model.""" return self._scene_object @@ -126,6 +152,7 @@ def scene_object(self, value): @classmethod def basic(cls, name, joints=None, links=None, materials=None, **kwargs): + # type: (str, Optional[List[Joint]], Optional[List[Link]], Optional[List[Material]], **Any) -> Robot """Create the most basic instance of a robot, based only on name. Parameters @@ -159,6 +186,7 @@ def basic(cls, name, joints=None, links=None, materials=None, **kwargs): @property def name(self): + # type: () -> str """Name of the robot, as defined by its model. Returns @@ -175,6 +203,7 @@ def name(self): @property def group_names(self): + # type: () -> List[str] """All planning groups of the robot, only available if semantics is set. Returns @@ -193,6 +222,7 @@ def group_names(self): @property def main_group_name(self): + # type: () -> str """ Robot's main planning group, only available if semantics is set. @@ -211,6 +241,7 @@ def root_name(self): @property def group_states(self): + # type: () -> Dict[str, dict] """All group states of the robot, only available if semantics is set. Returns @@ -229,15 +260,18 @@ def group_states(self): @property def attached_tools(self): + # type: () -> Dict[str, Tool] """Dictionary of tools and the planning groups that the tools are currently attached to, if any.""" return self._attached_tools @property def attached_tool(self): + # type: () -> Tool | None """Returns the tool attached to the default main planning group, if any.""" return self._attached_tools.get(self.main_group_name, None) def get_end_effector_link_name(self, group=None): + # type: (Optional[str]) -> str """Get the name of the robot's end effector link. Parameters @@ -261,6 +295,7 @@ def get_end_effector_link_name(self, group=None): return self.semantics.get_end_effector_link_name(group) def get_end_effector_link(self, group=None): + # type: (Optional[str]) -> Link """Get the robot's end effector link. Parameters @@ -283,6 +318,7 @@ def get_end_effector_link(self, group=None): return self.model.get_link_by_name(name) def get_end_effector_frame(self, group=None, full_configuration=None): + # type: (Optional[str], Optional[Configuration]) -> Frame """Get the frame of the robot's end effector. Parameters @@ -302,6 +338,7 @@ def get_end_effector_frame(self, group=None, full_configuration=None): return self.model.forward_kinematics(full_configuration, link_name=self.get_end_effector_link_name(group)) def get_base_link_name(self, group=None): + # type: (Optional[str]) -> str """Get the name of the robot's base link. Parameters @@ -325,6 +362,7 @@ def get_base_link_name(self, group=None): return self.semantics.get_base_link_name(group) def get_base_link(self, group=None): + # type: (Optional[str]) -> Link """Get the robot's base link. Parameters @@ -347,6 +385,7 @@ def get_base_link(self, group=None): return self.model.get_link_by_name(name) def get_base_frame(self, group=None, full_configuration=None): + # type: (Optional[str], Optional[Configuration]) -> Frame """Get the frame of the robot's base link, which is the robot's origin frame. Parameters @@ -366,6 +405,7 @@ def get_base_frame(self, group=None, full_configuration=None): return self.model.forward_kinematics(full_configuration, link_name=self.get_base_link_name(group)) def get_link_names(self, group=None): + # type: (Optional[str]) -> List[str] """Get the names of the links in the kinematic chain. Parameters @@ -391,6 +431,7 @@ def get_link_names(self, group=None): return link_names def get_link_names_with_collision_geometry(self): + # type: () -> List[str] """Get the names of the links with collision geometry. Returns @@ -406,6 +447,7 @@ def get_link_names_with_collision_geometry(self): return [link.name for link in self.model.iter_links() if link.collision] def get_configurable_joints(self, group=None): + # type: (Optional[str]) -> List[Joint] """Get the robot's configurable joints. Parameters @@ -438,6 +480,7 @@ def get_configurable_joints(self, group=None): return self.model.get_configurable_joints() def get_joint_types_by_names(self, names): + # type: (List[str]) -> List[int] """Get a list of joint types given a list of joint names. Parameters @@ -453,6 +496,7 @@ def get_joint_types_by_names(self, names): return self.model.get_joint_types_by_names(names) def get_joint_by_name(self, name): + # type: (str) -> Joint | None """RGet the joint in the robot model matching the given name. Parameters @@ -467,6 +511,7 @@ def get_joint_by_name(self, name): return self.model.get_joint_by_name(name) def get_configurable_joint_names(self, group=None): + # type: (Optional[str]) -> List[str] """Get the configurable joint names. Parameters @@ -494,6 +539,7 @@ def get_configurable_joint_names(self, group=None): return [j.name for j in configurable_joints] def get_configurable_joint_types(self, group=None): + # type: (Optional[str]) -> List[int] """Get the configurable joint types. Parameters @@ -520,6 +566,7 @@ def get_configurable_joint_types(self, group=None): return [j.type for j in configurable_joints] def get_attached_tool_collision_meshes(self): + # type: () -> List[List[CollisionMesh]] """Returns a list of all attached collisions meshes of each of the attached tools, if any. Returns @@ -534,6 +581,7 @@ def get_attached_tool_collision_meshes(self): # ========================================================================== def zero_configuration(self, group=None): + # type: (Optional[str]) -> Configuration """Get the zero joint configuration. If zero is out of joint limits ``(upper, lower)`` then @@ -559,6 +607,7 @@ def zero_configuration(self, group=None): return Configuration(values, joint_types, joint_names) def random_configuration(self, group=None): + # type: (Optional[str]) -> Configuration """Get a random configuration. Parameters @@ -586,6 +635,7 @@ def random_configuration(self, group=None): return Configuration(values, joint_types, joint_names) def get_group_configuration(self, group, full_configuration): + # type: (str, Configuration) -> Configuration """Get the group's configuration. Parameters @@ -608,6 +658,7 @@ def get_group_configuration(self, group, full_configuration): return Configuration(values, self.get_configurable_joint_types(group), group_joint_names) def merge_group_with_full_configuration(self, group_configuration, full_configuration, group): + # type: (Configuration, Configuration, str) -> Configuration """Get the robot's full configuration by merging a group's configuration with a full configuration. The group configuration takes precedence over the full configuration in case a joint value is present in both. @@ -644,6 +695,7 @@ def merge_group_with_full_configuration(self, group_configuration, full_configur return full_configuration def get_group_names_from_link_name(self, link_name): + # type: (str) -> List[str] """Get the names of the groups `link_name` belongs to. Parameters @@ -663,6 +715,7 @@ def get_group_names_from_link_name(self, link_name): return group_names def get_position_by_joint_name(self, configuration, joint_name, group=None): + # type: (Configuration, str, Optional[str]) -> float """Get the position of named joint in given configuration. Parameters @@ -691,6 +744,7 @@ def get_position_by_joint_name(self, configuration, joint_name, group=None): return configuration.joint_values[names.index(joint_name)] def _check_full_configuration_and_scale(self, full_configuration=None): + # type: (Optional[Configuration]) -> Tuple[Configuration, Configuration] """Either create a full configuration or check if the passed full configuration is valid. Parameters @@ -720,6 +774,7 @@ def _check_full_configuration_and_scale(self, full_configuration=None): return configuration, configuration.scaled(1.0 / self.scale_factor) def get_configuration_from_group_state(self, group, group_state): + # type: (str, str) -> Configuration """Get a :class:`compas_robots.Configuration` from a group's group state. Parameters @@ -745,6 +800,7 @@ def get_configuration_from_group_state(self, group, group_state): # ========================================================================== def transformation_RCF_WCF(self, group=None): + # type: (Optional[str]) -> Transformation """Get the transformation from the robot's coordinate system (RCF) to the world coordinate system (WCF). Parameters @@ -761,6 +817,7 @@ def transformation_RCF_WCF(self, group=None): return Transformation.from_change_of_basis(base_frame, Frame.worldXY()) def transformation_WCF_RCF(self, group=None): + # type: (Optional[str]) -> Transformation """Get the transformation from the world coordinate system (WCF) to the robot's coordinate system (RCF). Parameters @@ -777,6 +834,7 @@ def transformation_WCF_RCF(self, group=None): return Transformation.from_change_of_basis(Frame.worldXY(), base_frame) def set_RCF(self, robot_coordinate_frame, group=None): + # type: (Frame, Optional[str]) -> None """Move the origin frame of the robot to the robot_coordinate_frame. Raises @@ -789,6 +847,7 @@ def set_RCF(self, robot_coordinate_frame, group=None): raise NotImplementedError def get_RCF(self, group=None): + # type: (Optional[str]) -> Frame """Get the origin frame of the robot. Parameters @@ -804,6 +863,7 @@ def get_RCF(self, group=None): return self.get_base_frame(group) def to_local_coordinates(self, frame_WCF, group=None): + # type: (Frame, Optional[str]) -> Frame """Represent a frame from the world coordinate system (WCF) in the robot's coordinate system (RCF). Parameters @@ -830,6 +890,7 @@ def to_local_coordinates(self, frame_WCF, group=None): return frame_RCF def to_world_coordinates(self, frame_RCF, group=None): + # type: (Frame, Optional[str]) -> Frame """Represent a frame from the robot's coordinate system (RCF) in the world coordinate system (WCF). Parameters @@ -856,6 +917,7 @@ def to_world_coordinates(self, frame_RCF, group=None): return frame_WCF def _get_attached_tool_for_group(self, group_name=None): + # type: (Optional[str]) -> Tool """Get the tool attached to the given planning group. Group name defaults to main_group_name. Raises ValueError if group name is unknown or there is no tool currently attached to it""" group = group_name or self.main_group_name @@ -865,6 +927,7 @@ def _get_attached_tool_for_group(self, group_name=None): return self.attached_tools[group] def from_tcf_to_t0cf(self, frames_tcf, group=None): + # type: (List[Frame], Optional[str]) -> List[Frame] """Convert a list of frames at the robot's tool tip (tcf frame) to frames at the robot's flange (tool0 frame) using the attached tool. Parameters @@ -902,6 +965,7 @@ def from_tcf_to_t0cf(self, frames_tcf, group=None): return tool.from_tcf_to_t0cf(frames_tcf) def from_t0cf_to_tcf(self, frames_t0cf, group=None): + # type: (List[Frame], Optional[str]) -> List[Frame] """Convert frames at the robot's flange (tool0 frame) to frames at the robot's tool tip (tcf frame) using the attached tool. Parameters @@ -939,6 +1003,7 @@ def from_t0cf_to_tcf(self, frames_t0cf, group=None): return tool.from_t0cf_to_tcf(frames_t0cf) def attach_tool(self, tool, group=None, touch_links=None): + # type: (Tool, Optional[str], Optional[List[str]]) -> None """Attach a tool to the robot independently of the model definition. Parameters @@ -981,6 +1046,7 @@ def attach_tool(self, tool, group=None, touch_links=None): self.scene_object.attach_tool_model(tool.tool_model) def detach_tool(self, group=None): + # type: (Optional[str]) -> None """Detach the attached tool. Parameters @@ -1007,6 +1073,7 @@ def detach_tool(self, group=None): # ========================================================================== def ensure_client(self): + # type: () -> None """Check if the client is set. Raises @@ -1018,6 +1085,7 @@ def ensure_client(self): raise Exception("This method is only callable once a client is assigned.") def ensure_semantics(self): + # type: () -> None """Check if semantics is set. Raises @@ -1029,6 +1097,7 @@ def ensure_semantics(self): raise Exception("This method is only callable once a semantic model is assigned.") def ensure_geometry(self): + # type: () -> None """Check if the model's geometry has been loaded. Raises @@ -1051,6 +1120,7 @@ def inverse_kinematics( use_attached_tool_frame=True, options=None, ): + # type: (Frame, Optional[Configuration], Optional[str], Optional[bool], Optional[bool], Optional[Dict[str, Any]]) -> Configuration """Calculate the robot's inverse kinematic for a given frame. The inverse kinematic solvers are implemented as generators in order to fit both analytic @@ -1103,7 +1173,7 @@ def inverse_kinematics( >>> robot.inverse_kinematics(frame_WCF, start_configuration, group) # doctest: +SKIP Configuration((4.045, 5.130, -2.174, -6.098, -5.616, 6.283), (0, 0, 0, 0, 0, 0)) # doctest: +SKIP """ - # Pseudo-memoized sequential calls will re-use iterator if not exhaused + # Pseudo-memoized sequential calls will re-use iterator if not exhausted request_id = "{}-{}-{}-{}-{}".format( str(frame_WCF), str(start_configuration), str(group), str(return_full_configuration), str(options) ) @@ -1130,6 +1200,7 @@ def iter_inverse_kinematics( use_attached_tool_frame=True, options=None, ): + # type: (Frame, Optional[Configuration], Optional[str], Optional[bool], Optional[bool], Optional[Dict[str, Any]]) -> Iterator[Configuration] """Iterate over the inverse kinematic solutions of a robot. This method exposes the generator-based inverse kinematic solvers. Analytics solvers will return @@ -1207,9 +1278,10 @@ def iter_inverse_kinematics( if joint_positions: yield self._build_configuration(joint_positions, joint_names, group, return_full_configuration) else: - yield None # to accomodate analytic ik with keeping the order of solutions + yield None # to accommodate analytic ik with keeping the order of solutions def _build_configuration(self, joint_positions, joint_names, group, return_full_configuration): + # type: (List[float], List[str], str | None, bool) -> Configuration if return_full_configuration: # build configuration including passive joints, but no sorting joint_types = self.get_joint_types_by_names(joint_names) @@ -1224,6 +1296,7 @@ def _build_configuration(self, joint_positions, joint_names, group, return_full_ return configuration.scaled(self.scale_factor) def forward_kinematics(self, configuration, group=None, use_attached_tool_frame=True, options=None): + # type: (Configuration, Optional[str], Optional[bool], Optional[Dict[str, Any]]) -> Frame """Calculate the robot's forward kinematic. Parameters @@ -1315,6 +1388,7 @@ def forward_kinematics(self, configuration, group=None, use_attached_tool_frame= return frame_WCF def plan_cartesian_motion(self, waypoints, start_configuration=None, group=None, options=None): + # type: (Waypoints, Optional[Configuration], Optional[str], Optional[Dict[str, Any]]) -> JointTrajectory """Calculate a cartesian motion path (linear in tool space). Parameters @@ -1454,6 +1528,7 @@ def plan_cartesian_motion(self, waypoints, start_configuration=None, group=None, return trajectory def plan_motion(self, target, start_configuration=None, group=None, options=None): + # type: (Target, Optional[Configuration], Optional[str], Optional[Dict[str, Any]]) -> JointTrajectory """Calculate a motion path. Parameters @@ -1586,6 +1661,7 @@ def plan_motion(self, target, start_configuration=None, group=None, options=None return trajectory def transformed_frames(self, configuration, group=None): + # type: (Configuration, Optional[str]) -> List[Frame] """Get the robot's transformed frames. Parameters @@ -1606,6 +1682,7 @@ def transformed_frames(self, configuration, group=None): return self.model.transformed_frames(configuration) def transformed_axes(self, configuration, group=None): + # type: (Configuration, Optional[str]) -> List[Vector] """Get the robot's transformed axes. Parameters @@ -1630,6 +1707,7 @@ def transformed_axes(self, configuration, group=None): # ========================================================================== def update(self, configuration, group=None, visual=True, collision=True): + # type: (Configuration, Optional[str], Optional[bool], Optional[bool]) -> None """Update the robot's geometry. Parameters @@ -1654,14 +1732,17 @@ def update(self, configuration, group=None, visual=True, collision=True): self.scene_object.update(configuration, visual, collision) def draw_visual(self): + # type: () -> None """Draw the robot's visual geometry using the defined :attr:`Robot.scene_object`.""" return self.scene_object.draw_visual() def draw_collision(self): + # type: () -> None """Draw the robot's collision geometry using the defined :attr:`Robot.scene_object`.""" return self.scene_object.draw_collision() def draw(self): + # type: () -> None """Alias of :meth:`draw_visual`.""" return self.draw_visual() @@ -1672,6 +1753,7 @@ def draw(self): # return self.scene_object.draw_attached_tool() def scale(self, factor): + # type: (float) -> None """Scale the robot geometry by a factor (absolute). Parameters @@ -1691,8 +1773,9 @@ def scale(self, factor): @property def scale_factor(self): + # type: () -> float """A scale factor affecting planning targets, results and visualization. Typically, robot models are defined in meters, - if used in CAD environemnt where units is mm, use a scale_factor of 1000. + if used in CAD environment where units is mm, use a scale_factor of 1000. """ if self.scene_object: @@ -1701,6 +1784,7 @@ def scale_factor(self): return self._scale_factor def info(self): + # type: () -> None """Print information about the robot.""" print("The robot's name is '{}'.".format(self.name)) if self.semantics: diff --git a/src/compas_fab/robots/robot_library.py b/src/compas_fab/robots/robot_library.py index d89739316..75c67d9a9 100644 --- a/src/compas_fab/robots/robot_library.py +++ b/src/compas_fab/robots/robot_library.py @@ -2,6 +2,8 @@ from __future__ import division from __future__ import print_function +import compas + from compas_robots import RobotModel from compas_robots.resources import LocalPackageMeshLoader @@ -9,6 +11,14 @@ from .robot import Robot from .semantics import RobotSemantics +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import Optional # noqa: F401 + from compas_fab.backends.interfaces import ClientInterface # noqa: F401 + + __all__ = [ "RobotLibrary", ] @@ -36,7 +46,28 @@ class RobotLibrary(object): def _load_library_model( cls, urdf_filename, srdf_filename, local_package_mesh_folder, client=None, load_geometry=True ): - """Convenience method for loading robot from local cache directory.""" + # type: (str, str, str, Optional[ClientInterface], Optional[bool]) -> Robot + """Convenience method for loading robot from local cache directory. + + Parameters + ---------- + urdf_filename : :obj:`str` + Path to the URDF file. + srdf_filename : :obj:`str` + Path to the SRDF file. + local_package_mesh_folder : :obj:`str` + Path to the local package mesh folder. + client : :class:`compas_fab.backends.interfaces.ClientInterface`, optional + Backend client. Default is `None`. + load_geometry : :obj:`bool`, optional + Default is `True`, which means that the geometry is loaded. + + Returns + ------- + :class:`compas_fab.robots.Robot` + Newly created instance of the robot. + + """ model = RobotModel.from_urdf_file(urdf_filename) semantics = RobotSemantics.from_srdf_file(srdf_filename, model) @@ -54,15 +85,16 @@ def _load_library_model( @classmethod def rfl(cls, client=None, load_geometry=True): + # type: (Optional[ClientInterface], Optional[bool]) -> Robot """Create and return the RFL robot with 4 ABB irb 4600 and twin-gantry setup. The returned :class:`compas_fab.robots.Robot` object contains the robot model and semantics. Parameters ---------- - client: object + client: :class:`compas_fab.backends.interfaces.ClientInterface`, optional Backend client. Default is `None`. - load_geometry: bool, optional + load_geometry: :obj:`bool`, optional Default is `True`, which means that the geometry is loaded. `False` can be used to speed up the creation of the robot. @@ -84,15 +116,16 @@ def rfl(cls, client=None, load_geometry=True): @classmethod def ur5(cls, client=None, load_geometry=True): + # type: (Optional[ClientInterface], Optional[bool]) -> Robot """Returns a UR5 robot. The returned :class:`compas_fab.robots.Robot` object contains the robot model and semantics. Parameters ---------- - client: object + client: :class:`compas_fab.backends.interfaces.ClientInterface`, optional Backend client. Default is `None`. - load_geometry: bool, optional + load_geometry: :obj:`bool`, optional Default is `True`, which means that the geometry is loaded. `False` can be used to speed up the creation of the robot. @@ -114,15 +147,16 @@ def ur5(cls, client=None, load_geometry=True): @classmethod def ur10e(cls, client=None, load_geometry=True): + # type: (Optional[ClientInterface], Optional[bool]) -> Robot """Returns a UR10e robot. The returned :class:`compas_fab.robots.Robot` object contains the robot model and semantics. Parameters ---------- - client: object + client: :class:`compas_fab.backends.interfaces.ClientInterface`, optional Backend client. Default is `None`. - load_geometry: bool, optional + load_geometry: :obj:`bool`, optional Default is `True`, which means that the geometry is loaded. `False` can be used to speed up the creation of the robot. @@ -144,15 +178,16 @@ def ur10e(cls, client=None, load_geometry=True): @classmethod def abb_irb4600_40_255(cls, client=None, load_geometry=True): + # type: (Optional[ClientInterface], Optional[bool]) -> Robot """Returns a ABB irb4600-40/2.55 robot. The returned :class:`compas_fab.robots.Robot` object contains the robot model and semantics. Parameters ---------- - client: object + client: :class:`compas_fab.backends.interfaces.ClientInterface`, optional Backend client. Default is `None`. - load_geometry: bool, optional + load_geometry: :obj:`bool`, optional Default is `True`, which means that the geometry is loaded. `False` can be used to speed up the creation of the robot. diff --git a/src/compas_fab/robots/semantics.py b/src/compas_fab/robots/semantics.py index edf980f9c..e53b76c51 100644 --- a/src/compas_fab/robots/semantics.py +++ b/src/compas_fab/robots/semantics.py @@ -2,9 +2,27 @@ from __future__ import division from __future__ import print_function +import compas + from compas.data import Data from compas.files import XML +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import Any # noqa: F401 + from typing import Optional # noqa: F401 + from typing import Dict # noqa: F401 + from typing import Tuple # noqa: F401 + from typing import List # noqa: F401 + from typing import Set # noqa: F401 + from compas_robots import RobotModel # noqa: F401 + from compas_robots.model import Joint # noqa: F401 + from xml.etree.ElementTree import Element # noqa: F401 + + from compas_fab.backends.interfaces import ClientInterface # noqa: F401 + __all__ = [ "RobotSemantics", ] @@ -21,7 +39,7 @@ class RobotSemantics(Data): Parameters ---------- - robot_model : :class:`~compas_fab.robots.RobotModel` + robot_model : :class:`~compas_robots.RobotModel` The robot model. groups : :obj:`dict` of (:obj:`str`, :obj:`dict` of (``links`` : :obj:`list` of :obj:`str`, ``joints`` : :obj:`list` of :obj:`str`)), optional A nested dictionary defining planning groups. @@ -39,7 +57,7 @@ class RobotSemantics(Data): A list of passive joint names. end_effectors : :obj:`list` of :obj:`str`, optional A list of end effector link names. - disabled_collisions : :obj:`tuple` of (:obj:`str`, :obj:`str`), optional + disabled_collisions : :obj:`list` of :obj:`tuple` of (:obj:`str`, :obj:`str`), optional A set of disabled collision pairs. The order is not important, i.e. the pair `('link1', 'link2')` is the same as `('link2', 'link1')`. Only one pair is needed. @@ -50,7 +68,7 @@ class RobotSemantics(Data): - Level 1 keys are planning group names : :obj:`str`. - Level 2 keys are group state names : :obj:`str`. - - Level 3 keys are joint names and values are joint values : :obj:`str`. + - Level 3 keys are joint names :obj:`str` and values are joint values :obj:`float`. Attributes ---------- @@ -73,6 +91,7 @@ def __init__( disabled_collisions=None, group_states=None, ): + # type: (RobotModel, Optional[Dict[str, Dict[str, List[str]]]], Optional[str], Optional[List[str]], Optional[List[str]], List[Tuple[str,str]], Dict[str, Dict[str, Dict[str,float]]]) -> None super(RobotSemantics, self).__init__() self.robot_model = robot_model @@ -98,6 +117,7 @@ def __data__(self): @classmethod def __from_data__(cls, data): + # type: (Dict[str, Any]) -> RobotSemantics robot_model = data.get("robot_model") groups = data.get("groups", {}) main_group_name = data.get("main_group_name") @@ -121,14 +141,17 @@ def __from_data__(cls, data): @property def group_names(self): + # type: () -> List[str] return list(self.groups.keys()) @property def unordered_disabled_collisions(self): + # type: () -> Set[frozenset] return {frozenset(pair) for pair in self.disabled_collisions} @classmethod def from_srdf_file(cls, file, robot_model): + # type: (str, RobotModel) -> RobotSemantics """Create an instance of semantics based on an SRDF file path or file-like object. Parameters @@ -148,18 +171,40 @@ def from_srdf_file(cls, file, robot_model): >>> print(semantics.main_group_name) manipulator """ + xml = XML.from_file(file) return cls.from_xml(xml, robot_model) @classmethod def from_srdf_string(cls, text, robot_model): - """Create an instance of semantics based on an SRDF string.""" + # type: (str, RobotModel) -> RobotSemantics + """Create an instance of semantics based on an SRDF string. + + Parameters + ---------- + text : :obj:`str` + The SRDF data as a string. + robot_model : :class:`compas_robots.RobotModel` + The robot model is needed when loading the semantics. + """ + xml = XML.from_string(text) return cls.from_xml(xml, robot_model) @classmethod def from_xml(cls, xml, robot_model): - """Create an instance of semantics based on an XML object.""" + # type: (XML, RobotModel) -> RobotSemantics + """Create an instance of semantics based on an XML object. + + Parameters + ---------- + + xml : :class:`compas.files.XML` + The XML object containing the SRDF data. + robot_model : :class:`compas_robots.RobotModel` + The robot model is needed when loading the semantics. + """ + groups = _get_groups(xml.root, robot_model) passive_joints = _get_passive_joints(xml.root) end_effectors = _get_end_effectors(xml.root) @@ -180,6 +225,7 @@ def from_xml(cls, xml, robot_model): ) def get_end_effector_link_name(self, group=None): + # type: (Optional[str]) -> str """Get the name of the last link (end effector link) in a planning group. Parameters @@ -198,6 +244,7 @@ def get_end_effector_link_name(self, group=None): return self.groups[group]["links"][-1] def get_base_link_name(self, group=None): + # type: (Optional[str]) -> str """Get the name of the first link (base link) in a planning group. Parameters @@ -215,6 +262,7 @@ def get_base_link_name(self, group=None): return self.groups[group]["links"][0] def get_all_configurable_joints(self): + # type: () -> List[Joint] """Get all configurable :class:`compas_robots.model.Joint` of the robot. Configurable joints are joints that can be controlled, @@ -235,6 +283,7 @@ def get_all_configurable_joints(self): return joints def get_configurable_joints(self, group=None): + # type: (Optional[str]) -> List[Joint] """Get all configurable :class:`compas_robots.model.Joint` of a planning group. Configurable joints are joints that can be controlled, @@ -263,6 +312,7 @@ def get_configurable_joints(self, group=None): return joints def get_configurable_joint_names(self, group=None): + # type: (Optional[str]) -> List[str] """Get all the names of configurable joints of a planning group. Similar to :meth:`get_configurable_joints` but returning joint names. @@ -279,8 +329,13 @@ def get_configurable_joint_names(self, group=None): return [joint.name for joint in self.get_configurable_joints(group)] +# ------------------- # XML parsing methods +# ------------------- + + def _get_groups(root, robot_model): + # type: (Element, RobotModel) -> Dict[str, Dict[str, List[str]]] groups = {} for group in root.findall("group"): @@ -294,6 +349,7 @@ def _get_groups(root, robot_model): def _get_group_states(root): + # type: (Element) -> Dict[str, Dict[str, Dict[str, float]]] group_states = {} for group_state in root.findall("group_state"): @@ -306,6 +362,7 @@ def _get_group_states(root): def _get_group_link_names(group, root, robot_model): + # type: (Any, Element, RobotModel) -> List[str] link_names = [] for link in group.findall("link"): name = link.attrib["name"] @@ -337,6 +394,7 @@ def _get_group_link_names(group, root, robot_model): def _get_group_joint_names(group, root, robot_model): + # type: (Any, Element, RobotModel) -> List[str] joint_names = [] for link in group.findall("link"): link = robot_model.get_link_by_name(link.attrib["name"]) @@ -365,18 +423,22 @@ def _get_group_joint_names(group, root, robot_model): def _get_group_elem_by_name(group_name, root): + # type: (str, Element) -> Element for group_elem in root.findall("group"): if group_elem.attrib["name"] == group_name: return group_elem def _get_passive_joints(root): + # type: (Element) -> List[str] return [joint.attrib["name"] for joint in root.iter("passive_joint")] def _get_end_effectors(root): + # type: (Element) -> List[str] return [ee.attrib["parent_link"] for ee in root.findall("end_effector")] def _get_disabled_collisions(root): + # type: (Element) -> Set[Tuple[str, str]] return {tuple([dc.attrib["link1"], dc.attrib["link2"]]) for dc in root.iter("disable_collisions")} diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index fc08393ee..35648c140 100644 --- a/src/compas_fab/robots/targets.py +++ b/src/compas_fab/robots/targets.py @@ -1,8 +1,21 @@ +import compas + from compas.data import Data from compas.geometry import Frame from compas.geometry import Transformation from compas_robots.model import Joint +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import Optional # noqa: F401 + from typing import Tuple # noqa: F401 + from compas.geometry import Point # noqa: F401 + from compas.geometry import Vector # noqa: F401 + from compas_robots import Configuration # noqa: F401 + from compas_fab.robots import Constraint # noqa: F401 + __all__ = [ "Target", "FrameTarget", @@ -40,6 +53,7 @@ class Target(Data): """ def __init__(self, name="Generic Target"): + # type: (str) -> None super(Target, self).__init__() self.name = name @@ -48,6 +62,7 @@ def __data__(self): raise NotImplementedError def scaled(self, factor): + # type: (float) -> Target """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. @@ -109,6 +124,7 @@ def __init__( tool_coordinate_frame=None, name="Frame Target", ): + # type: (Frame, Optional[float], Optional[float], Optional[Frame | Transformation], Optional[str]) -> None super(FrameTarget, self).__init__(name=name) self.target_frame = target_frame self.tolerance_position = tolerance_position @@ -136,6 +152,7 @@ def from_transformation( tool_coordinate_frame=None, name="Frame Target", ): + # type: (Transformation, Optional[float], Optional[float], Optional[Frame | Transformation], Optional[str]) -> FrameTarget """Creates a FrameTarget from a transformation matrix. Parameters @@ -164,7 +181,8 @@ def from_transformation( return cls(frame, tolerance_position, tolerance_orientation, tool_coordinate_frame, name) def scaled(self, factor): - """Returns a copy of the :class:`FrameTarget` where the target frame and tolerances are scaled. + # type: (float) -> FrameTarget + """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 @@ -227,8 +245,8 @@ class PointAxisTarget(Target): 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. + 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 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 @@ -244,10 +262,13 @@ def __init__( tool_coordinate_frame=None, name="Point-Axis Target", ): + # type: (Point, Vector, Optional[float], Optional[Frame | Transformation], Optional[str]) -> None super(PointAxisTarget, self).__init__(name=name) self.target_point = target_point self.target_z_axis = target_z_axis self.tolerance_position = tolerance_position + if isinstance(tool_coordinate_frame, Transformation): + tool_coordinate_frame = Frame.from_transformation(tool_coordinate_frame) self.tool_coordinate_frame = tool_coordinate_frame @property @@ -261,6 +282,7 @@ def __data__(self): } def scaled(self, factor): + # type: (float) -> PointAxisTarget """Returns a copy of the target where the target point and tolerances are scaled. Parameters @@ -316,7 +338,7 @@ class ConfigurationTarget(Target): SUPPORTED_JOINT_TYPES = [Joint.PRISMATIC, Joint.REVOLUTE, Joint.CONTINUOUS] def __init__(self, target_configuration, tolerance_above=None, tolerance_below=None, name="Configuration Target"): - + # type: (Configuration, Optional[list[float]], Optional[list[float]], Optional[str]) -> None super(ConfigurationTarget, self).__init__(name=name) self.target_configuration = target_configuration # type: Configuration self.tolerance_above = tolerance_above @@ -337,6 +359,7 @@ def __data__(self): @classmethod def generate_default_tolerances(cls, configuration, tolerance_prismatic, tolerance_revolute): + # type: (Configuration, float, float) -> Tuple[list[float], list[float]] """Generates tolerances values for the target configuration based on the joint types. The parameters `tolerance_prismatic` and `tolerance_revolute` are used to generate the @@ -357,7 +380,7 @@ def generate_default_tolerances(cls, configuration, tolerance_prismatic, toleran Returns ------- - :obj:`tuple` of :obj:`list` of :obj:`float` + :obj:`tuple` of (:obj:`list` of :obj:`float`, :obj:`list` of :obj:`float`) The tolerances_above and tolerances_below lists. Examples @@ -386,6 +409,7 @@ def generate_default_tolerances(cls, configuration, tolerance_prismatic, toleran return tolerances_above, tolerances_below def scaled(self, factor): + # type: (float) -> ConfigurationTarget """Returns copy of the target where the target configuration and tolerances are scaled. This function should only be needed if the ConfigurationTarget was created @@ -407,6 +431,8 @@ def scaled(self, factor): target_configuration = self.target_configuration.scaled(factor) def scale_tolerance(tolerance, joint_types): + # type: (list[float], list[Joint]) -> list[float] + """Only scales the tolerances for prismatic and planar joints.""" scaled_tolerance = [] for t, joint_type in zip(tolerance, joint_types): if joint_type in (Joint.PLANAR, Joint.PRISMATIC): @@ -458,6 +484,7 @@ class ConstraintSetTarget(Target): """ def __init__(self, constraint_set, name="Constraint Set Target"): + # type: (list[Constraint], Optional[str]) -> None super(ConstraintSetTarget, self).__init__(name=name) self.constraint_set = constraint_set @@ -469,6 +496,7 @@ def __data__(self): } def scaled(self, factor): + # type: (float) -> ConstraintSetTarget """Returns a scaled copy of the target. Raises @@ -516,6 +544,7 @@ def __init__(self, tool_coordinate_frame, name="Generic Waypoints"): self.tool_coordinate_frame = tool_coordinate_frame def scaled(self, factor): + # type: (float) -> Waypoints """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. @@ -594,6 +623,7 @@ def from_transformations( tool_coordinate_frame=None, name="Frame Waypoints", ): + # type: (list[Transformation], Optional[float], Optional[float], Optional[Frame | Transformation], Optional[str]) -> FrameWaypoints """Creates a FrameWaypoints from a list of transformation matrices. Parameters @@ -622,6 +652,7 @@ def from_transformations( return cls(frames, tolerance_position, tolerance_orientation, tool_coordinate_frame, name) def scaled(self, factor): + # type: (float) -> FrameWaypoints """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. @@ -698,6 +729,7 @@ def __data__(self): } def scaled(self, factor): + # type: (float) -> PointAxisWaypoints """Returns a copy of the target where the target points and tolerances are scaled. Parameters diff --git a/src/compas_fab/robots/time_.py b/src/compas_fab/robots/time_.py index 9ddd54e16..07d176fce 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/tool.py b/src/compas_fab/robots/tool.py index fd3fa1f85..b0a77fe79 100644 --- a/src/compas_fab/robots/tool.py +++ b/src/compas_fab/robots/tool.py @@ -2,6 +2,8 @@ from __future__ import division from __future__ import print_function +import compas + from compas.data import Data from compas_robots import ToolModel from compas_robots.model import LinkGeometry @@ -9,6 +11,15 @@ from compas_fab.robots import AttachedCollisionMesh from compas_fab.robots import CollisionMesh +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import Optional # noqa: F401 + from typing import List # noqa: F401 + from compas.geometry import Frame # noqa: F401 + from compas.datastructures import Mesh # noqa: F401 + __all__ = ["Tool"] @@ -21,11 +32,11 @@ class Tool(Data): The visual mesh of the tool. frame_in_tool0_frame : :class:`compas.geometry.Frame` The tool coordinate frame (TCF) of the tool relative to the tool0 frame (T0CF). - collision : :class:`compas.datastructures.Mesh` + collision : :class:`compas.datastructures.Mesh`, optional The collision mesh representation of the tool. - name : :obj:`str` + name : :obj:`str`, optional The name of the `Tool`. Defaults to 'attached_tool'. - connected_to : :obj:`str` + connected_to : :obj:`str`, optional The name of the `Link` to which the tool is attached. Defaults to ``None``. Attributes @@ -50,17 +61,21 @@ class Tool(Data): """ def __init__(self, visual, frame_in_tool0_frame, collision=None, name="attached_tool", connected_to=None): + # type: (Mesh, Frame, Optional[Mesh], Optional[str], Optional[str]) -> None super(Tool, self).__init__() self.tool_model = ToolModel(visual, frame_in_tool0_frame, collision, name, connected_to) @classmethod def from_tool_model(cls, tool_model): + # type: (ToolModel) -> Tool + """Creates a `Tool` from a :class:`~compas_robots.ToolModel` instance.""" tool = cls(None, None) tool.tool_model = tool_model return tool @property def attached_collision_meshes(self): + # type: () -> List[AttachedCollisionMesh] # If the tool model is not set, return an empty list if not self.tool_model or len(self.tool_model.links) == 0: return [] @@ -80,22 +95,27 @@ def attached_collision_meshes(self): @property def name(self): + # type: () -> str return self.tool_model.name @property def connected_to(self): + # type: () -> str return self.tool_model.connected_to @connected_to.setter def connected_to(self, link_name): + # type: (str) -> None self.tool_model.connected_to = link_name @property def frame(self): + # type: () -> Frame return self.tool_model.frame @frame.setter def frame(self, frame): + # type: (Frame) -> None self.tool_model.frame = frame @property @@ -113,6 +133,7 @@ def __data__(self): @classmethod def __from_data__(cls, data): + # type: (dict) -> Tool """Construct a `Tool` from its data representation. Parameters @@ -131,11 +152,14 @@ def __from_data__(cls, data): return tool def update_touch_links(self, touch_links=None): + # type: (Optional[List[str]]) -> None + """Updates the list of names representing the links that the tool is allowed to touch.""" if touch_links: for acm in self.attached_collision_meshes: acm.touch_links = touch_links def from_tcf_to_t0cf(self, frames_tcf): + # type: (List[Frame]) -> List[Frame] """Converts a list of frames at the robot's tool tip (tcf frame) to frames at the robot's flange (tool0 frame). Parameters @@ -161,6 +185,7 @@ def from_tcf_to_t0cf(self, frames_tcf): return self.tool_model.from_tcf_to_t0cf(frames_tcf) def from_t0cf_to_tcf(self, frames_t0cf): + # type: (List[Frame]) -> List[Frame] """Converts frames at the robot's flange (tool0 frame) to frames at the robot's tool tip (tcf frame). Parameters diff --git a/src/compas_fab/robots/trajectory.py b/src/compas_fab/robots/trajectory.py index fd3070a00..57bee09b9 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 diff --git a/src/compas_fab/robots/units.py b/src/compas_fab/robots/units.py index af0d67646..7964773de 100644 --- a/src/compas_fab/robots/units.py +++ b/src/compas_fab/robots/units.py @@ -6,6 +6,11 @@ import math +import compas + +if not compas.IPY: + from typing import List # noqa: F401 + __all__ = [ "to_radians", "to_degrees", @@ -13,6 +18,7 @@ def to_radians(degrees): + # type: (List[float]) -> List[float] """Convert a list of floats representing degrees to a list of radians. Parameters @@ -29,6 +35,7 @@ def to_radians(degrees): def to_degrees(radians): + # type: (List[float]) -> List[float] """Convert a list of floats representing radians to a list of degrees. Parameters @@ -39,6 +46,6 @@ def to_degrees(radians): Returns ------- :obj:`list` of :obj:`float` - List of degress. + List of degrees. """ return [math.degrees(r) for r in radians] diff --git a/src/compas_fab/robots/wrench.py b/src/compas_fab/robots/wrench.py index 5128817d2..d69715629 100644 --- a/src/compas_fab/robots/wrench.py +++ b/src/compas_fab/robots/wrench.py @@ -7,6 +7,15 @@ from compas.geometry import Vector from compas.geometry import cross_vectors +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import List # noqa: F401 + from compas.geometry import Transformation # noqa: F401 + from compas.geometry import Frame # noqa: F401 + from compas.geometry import Point # noqa: F401 + if not compas.IPY: from scipy import stats else: @@ -40,6 +49,7 @@ class Wrench(Data): """ def __init__(self, force, torque): + # type: (Vector, Vector) -> None super(Wrench, self).__init__() self.force = force self.torque = torque @@ -61,6 +71,7 @@ def __data__(self): @classmethod def from_list(cls, values): + # type: (List[float]) -> Wrench """Construct a wrench from a list of 6 :obj:`float` values. Parameters @@ -83,6 +94,7 @@ def from_list(cls, values): @classmethod def by_samples(cls, wrenches, proportion_to_cut=0.1): + # type: (List[Wrench], float) -> Wrench """ Construct the wrench by sampled data, allowing to filter. @@ -124,19 +136,23 @@ def by_samples(cls, wrenches, proportion_to_cut=0.1): @property def force(self): + # type: () -> Vector return self._force @force.setter def force(self, vector): + # type: (Vector) -> None force = Vector(*list(vector)) self._force = force @property def torque(self): + # type: () -> Vector return self._torque @torque.setter def torque(self, vector): + # type: (Vector) -> None torque = Vector(*list(vector)) self._torque = torque @@ -163,6 +179,7 @@ def __repr__(self): # ========================================================================== def copy(self): + # type: () -> Wrench """Make a copy of this ``Wrench``. Returns @@ -255,6 +272,7 @@ def __ne__(self, other, tol=1e-05): # ========================================================================== def transform(self, transformation): + # type: (Transformation) -> None """Transforms a `Wrench` with the transformation. Parameters @@ -276,6 +294,7 @@ def transform(self, transformation): self.torque.transform(transformation) def transformed(self, transformation): + # type: (Transformation) -> Wrench """Returns a transformed copy of the `Wrench`. Parameters @@ -299,15 +318,16 @@ def transformed(self, transformation): return wrench def gravity_compensated(self, ft_sensor_frame, mass, center_of_mass): + # type: (Frame, float, Point) -> Wrench """Removes the force and torque in effect of gravity from the wrench. Parameters ---------- ft_sensor_frame : :class:`compas.geometry.Frame` The coordinate frame of the force torque sensor. - mass: float + mass : float The mass of the object in kg. - center_of_mass : :class:`Point` + center_of_mass : :class:`compas.geometry.Point` The center of mass of the object in meters. Returns diff --git a/src/compas_fab/utilities/filesystem.py b/src/compas_fab/utilities/filesystem.py index a7b88f0ee..c1fe74fc4 100644 --- a/src/compas_fab/utilities/filesystem.py +++ b/src/compas_fab/utilities/filesystem.py @@ -1,9 +1,19 @@ import os +import compas + __all__ = ["list_files_in_directory"] +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import List # noqa: F401 + from typing import Optional # noqa: F401 + def list_files_in_directory(directory, fullpath=False, extensions=None): + # type: (str, bool, Optional[List[str]]) -> List[str] """This function lists just the files in a directory, not sub-directories. Parameters @@ -39,6 +49,9 @@ def list_files_in_directory(directory, fullpath=False, extensions=None): if __name__ == "__main__": - path = os.path.join(os.path.dirname(__file__), "..", "robots", "ur", "ur10", "model") + path = os.path.join( + os.path.dirname(__file__), "..", "data", "robot_library", "rfl", "rfl_description", "meshes", "visual" + ) os.listdir(path) - print(list_files_in_directory(path, fullpath=True, extensions=["obj"])) + for file in list_files_in_directory(path, fullpath=True, extensions=["stl"]): + print(file) diff --git a/src/compas_fab/utilities/numbers.py b/src/compas_fab/utilities/numbers.py index e0ce02ef0..8123a1d89 100644 --- a/src/compas_fab/utilities/numbers.py +++ b/src/compas_fab/utilities/numbers.py @@ -1,4 +1,13 @@ import math +import compas + + +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import List # noqa: F401 + from typing import Optional # noqa: F401 __all__ = [ "allclose", @@ -15,8 +24,27 @@ def map_range(value, from_min, from_max, to_min, to_max): + # type: (float, float, float, float, float) -> float """Performs a linear interpolation of a value within the range of [from_min, from_max] to another range of [to_min, to_max]. + + Parameters + ---------- + value : :obj:`float` + The value to map. + from_min : :obj:`float` + The minimum value of the input range. + from_max : :obj:`float` + The maximum value of the input range. + to_min : :obj:`float` + The minimum value of the output range. + to_max : :obj:`float` + The maximum value of the output range. + + Returns + ------- + :obj:`float` + The mapped value. """ from_range = from_max - from_min to_range = to_max - to_min @@ -25,11 +53,27 @@ def map_range(value, from_min, from_max, to_min, to_max): def range_geometric_row(number, d, r=1.1): + # type: (float, int, float) -> List[float] """Returns a list of numbers with a certain relation to each other. The function divides one number into a list of d numbers [n0, n1, ...], such that their sum is number and the relation between the numbers is defined with n1 = n0 / r, n2 = n1 / r, n3 = n2 / r, ... + + Parameters + ---------- + number : :obj:`float` + The initial number to divide. This number is the first element + in the returned list. + d : :obj:`int` + The number of elements in the returned list. + r : :obj:`float`, optional + The ratio between the numbers in the list. Default is 1.1. + + Returns + ------- + :obj:`list` of :obj:`float` + The list of numbers with the geometric relation. """ if r <= 0: raise ValueError("r must be > 0") @@ -43,9 +87,24 @@ def range_geometric_row(number, d, r=1.1): def arange(start, stop, step): + # type: (float, float, float) -> List[float] """Returns evenly spaced values within a given interval. The function is similar to NumPy's *arange* function. + + Parameters + ---------- + start : :obj:`float` + The starting value of the sequence. + stop : :obj:`float` + The end value of the sequence. + step : :obj:`float` + The spacing between the values. + + Returns + ------- + :obj:`list` of :obj:`float` + The list of evenly spaced values. """ if math.fabs(stop - (start + step)) > math.fabs(stop - start): raise ValueError("Please check the sign of step.") @@ -55,23 +114,60 @@ def arange(start, stop, step): def diffs(l1, l2): + # type: (List[float], List[float]) -> List[float] """Returns the element-wise differences between two lists. + Parameters + ---------- + l1 : :obj:`list` of :obj:`float` + The first list. + l2 : :obj:`list` of :obj:`float` + The second list. + + Returns + ------- + :obj:`list` of :obj:`float` + The element-wise differences between the two lists. + Raises ------ ValueError If 2 lists of different length are passed. """ if len(l1) != len(l2): - raise ValueError("Pass 2 lists of equal length.") + raise ValueError("Two lists must be of equal length.") return [math.fabs(a - b) for a, b in zip(l1, l2)] def allclose(l1, l2, tol=1e-05): + # type: (List[float], List[float], Optional[float]) -> bool """Returns True if two lists are element-wise equal within a tolerance. The function is similar to NumPy's *allclose* function. + + Parameters + ---------- + l1 : :obj:`list` of :obj:`float` + The first list. + l2 : :obj:`list` of :obj:`float` + The second list. + tol : :obj:`float`, optional + The tolerance within which the lists are considered equal. Default is 1e-05. + + Returns + ------- + bool + True if the lists are equal within the tolerance, otherwise False. + + Raises + ------ + ValueError + If 2 lists of different length are passed. """ + + if len(l1) != len(l2): + raise ValueError("Two lists must be of equal length.") + for a, b in zip(l1, l2): if math.fabs(a - b) > tol: return False @@ -79,23 +175,46 @@ def allclose(l1, l2, tol=1e-05): def argsort(numbers): + # type: (List[float]) -> List[int] """Returns the indices that would sort an array of numbers. The function is similar to NumPy's *argsort* function. + Parameters + ---------- + numbers : :obj:`list` of :obj:`float` + A list of numbers. + + Returns + ------- + :obj:`list` of :obj:`int` + The indices that would sort the array. + Notes ----- For a large list of numbers reconsider using NumPy's *argsort* function, since this function might take too long. + """ return [i[0] for i in sorted(enumerate(numbers), key=lambda x: x[1])] def argmin(numbers): + # type: (List[float]) -> int """Returns the index of the minimum value in numbers. The function is similar to NumPy's *argmin* function. + Parameters + ---------- + numbers : :obj:`list` of :obj:`float` + A list of numbers. + + Returns + ------- + :obj:`int` + The index of the minimum value in the list. + Notes ----- For a large list of numbers reconsider using NumPy's *argmin* function, @@ -105,10 +224,21 @@ def argmin(numbers): def argmax(numbers): + # type: (List[float]) -> int """Returns the index of the maximum value in numbers. The function is similar to NumPy's *argmax* function. + Parameters + ---------- + numbers : :obj:`list` of :obj:`float` + A list of numbers. + + Returns + ------- + :obj:`int` + The index of the maximum value in the list. + Notes ----- For a large list of numbers reconsider using NumPy's *argmax* function, @@ -118,27 +248,41 @@ def argmax(numbers): def sign(number): + # type: (float) -> int """Returns the sign of a number: +1 or -1. Parameters ---------- number : float - A number. + A number to check the sign of. Returns ------- int - `+1` if the number has positive sign, otherwise `-1`. + `+1` if the number has positive sign, + `0` if the number is zero, + `-1` if the number has negative sign. """ return int(int((number) > 0) - int((number) < 0)) def clamp(value, min_value, max_value): + # type: (float, float, float) -> float """Clamps a value within the bound [min_value, max_value] + Parameters + ---------- + value : float + The value to clamp. + min_value : float + The minimum value. + max_value : float + The maximum value. + Returns ------- float + The clamped value. """ if min_value > max_value: raise ValueError("min_value must be bigger than max_value")