Skip to content

Commit

Permalink
Organize imports, Fix Lint, Fix Init files
Browse files Browse the repository at this point in the history
  • Loading branch information
yck011522 committed Sep 25, 2024
1 parent 60e8b55 commit ba854fe
Show file tree
Hide file tree
Showing 43 changed files with 411 additions and 382 deletions.
127 changes: 78 additions & 49 deletions src/compas_fab/backends/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,24 @@
import compas

# Base imports
from .exceptions import *
from .exceptions import (
BackendError,
BackendFeatureNotSupportedError,
BackendTargetNotSupportedError,
TargetModeMismatchError,
PlanningGroupNotExistsError,
InverseKinematicsError,
KinematicsError,
CollisionCheckError,
MotionPlanningError,
MPStartStateInCollisionError,
MPTargetInCollisionError,
MPInterpolationInCollisionError,
MPSearchTimeOutError,
MPNoIKSolutionError,
MPNoPlanFoundError,
MPMaxJumpError,
)

from .tasks import (
FutureResult,
Expand All @@ -123,19 +140,17 @@
MoveItPlanner,
)

# Analytic IK
# Kinematics imports
from .kinematics import (
# Kinematics - Analytic IK
AnalyticalInverseKinematics,
AnalyticalPlanCartesianMotion,
AnalyticalPyBulletPlanner,
AnalyticalKinematicsPlanner,
OffsetWristKinematics,
SphericalWristKinematics,
CartesianMotionError,
)

# Robot-specific analytic IK
from .kinematics import (
# Kinematics - Robot-specific analytic IK
UR3Kinematics,
UR3eKinematics,
UR5Kinematics,
Expand All @@ -146,8 +161,9 @@
ABB_IRB4600_40_255Kinematics,
)

# NOTE: IPY guard because PyBullet do not work in IronPython
# PyBullet imports
if not compas.IPY:
# PyBullet do not work in IronPython
from .pybullet import (
PyBulletClient,
PyBulletError,
Expand All @@ -156,45 +172,58 @@
PlanningGroupNotSupported,
)

# __all__ = [
# # Base
# "BackendError",
# "BackendFeatureNotSupportedError",
# "InverseKinematicsError",
# "KinematicsError",
# "CollisionCheckError",
# "FutureResult",
# "CancellableFutureResult",
# "MPMaxJumpError",
# # ROS
# "RosClient",
# "RosError",
# "RosValidationError",
# "RosFileServerLoader",
# "MoveItPlanner",
# # Analytic IK
# "AnalyticalInverseKinematics",
# "AnalyticalPlanCartesianMotion",
# "AnalyticalPyBulletPlanner",
# "AnalyticalKinematicsPlanner",
# "OffsetWristKinematics",
# "SphericalWristKinematics",
# "CartesianMotionError",
# # Robot-specific analytic IK
# "UR3Kinematics",
# "UR3eKinematics",
# "UR5Kinematics",
# "UR5eKinematics",
# "UR10Kinematics",
# "UR10eKinematics",
# "Staubli_TX260LKinematics",
# "ABB_IRB4600_40_255Kinematics",
# ]

# if not compas.IPY:
# __all__ += [
# "PyBulletClient",
# "PyBulletError",
# "PyBulletPlanner",
# "AnalyticalPyBulletClient",
# ]
__all__ = [
# Exceptions
"BackendError",
"BackendFeatureNotSupportedError",
"BackendTargetNotSupportedError",
"TargetModeMismatchError",
"PlanningGroupNotExistsError",
"InverseKinematicsError",
"KinematicsError",
"CollisionCheckError",
"MotionPlanningError",
"MPStartStateInCollisionError",
"MPTargetInCollisionError",
"MPInterpolationInCollisionError",
"MPSearchTimeOutError",
"MPNoIKSolutionError",
"MPNoPlanFoundError",
"MPMaxJumpError",
# Tasks
"FutureResult",
"CancellableFutureResult",
# ROS
"RosClient",
"RosError",
"RosValidationError",
"RosFileServerLoader",
"MoveItPlanner",
# Kinematics
"AnalyticalInverseKinematics",
"AnalyticalPlanCartesianMotion",
"AnalyticalPyBulletPlanner",
"AnalyticalKinematicsPlanner",
"OffsetWristKinematics",
"SphericalWristKinematics",
"CartesianMotionError",
# Kinematics - Robot-specific analytic IK
"UR3Kinematics",
"UR3eKinematics",
"UR5Kinematics",
"UR5eKinematics",
"UR10Kinematics",
"UR10eKinematics",
"Staubli_TX260LKinematics",
"ABB_IRB4600_40_255Kinematics",
]

# PyBullet
if not compas.IPY:
__all__ += [
"PyBulletClient",
"PyBulletError",
"PyBulletPlanner",
"AnalyticalPyBulletClient",
"PlanningGroupNotSupported",
]
6 changes: 3 additions & 3 deletions src/compas_fab/backends/interfaces/client.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,14 @@

import compas

from compas_fab.robots import RobotCell
from compas_fab.robots import RobotCellState

if compas.IPY:
from typing import TYPE_CHECKING

if TYPE_CHECKING:
from compas_fab.robots import Robot
from compas_fab.robots import Robot # noqa: F401
from compas_fab.robots import RobotCell # noqa: F401
from compas_fab.robots import RobotCellState # noqa: F401


class ClientInterface(object):
Expand Down
14 changes: 3 additions & 11 deletions src/compas_fab/backends/interfaces/planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -16,17 +16,9 @@
from typing import TYPE_CHECKING

if TYPE_CHECKING:
from compas_fab.robots import Robot
from compas_fab.robots import RobotCell
from compas_fab.robots import RobotCellState
from compas_fab.robots import Target
from compas_fab.robots import Waypoints

from compas_fab.backends.interfaces import ClientInterface

from typing import List
from typing import Optional
from compas.geometry import Frame
from typing import List # noqa: F401
from compas_fab.robots import RobotCellState # noqa: F401
from compas_fab.backends.interfaces import ClientInterface # noqa: F401


class PlannerInterface(object):
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@
from compas_fab.backends.exceptions import BackendError
from compas_fab.backends.interfaces import ForwardKinematics


Expand All @@ -12,8 +11,8 @@
from compas_fab.robots import RobotCellState # noqa: F401
from compas_fab.robots import Robot # noqa: F401
from compas_fab.robots import TargetMode # noqa: F401
from compas.geometry import Frame
from typing import Dict, List, Optional, Tuple # noqa: F401
from compas.geometry import Frame # noqa: F401
from typing import Optional # noqa: F401


class AnalyticalForwardKinematics(ForwardKinematics):
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@
from compas_fab.backends.exceptions import BackendError
from compas_fab.backends.exceptions import BackendTargetNotSupportedError
from compas_fab.backends.exceptions import InverseKinematicsError
from compas_fab.backends.interfaces import InverseKinematics
Expand All @@ -16,7 +15,8 @@
from compas_fab.robots import RobotCellState # noqa: F401
from compas_fab.robots import Robot # noqa: F401
from compas_fab.robots import Target # noqa: F401
from typing import Dict, List, Optional, Tuple # noqa: F401
from typing import Dict # noqa: F401
from typing import Optional # noqa: F401
from typing import Generator # noqa: F401

from ..utils import joint_angles_to_configurations
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@
from compas_fab.robots import JointTrajectoryPoint
from compas_fab.robots import FrameWaypoints
from compas_fab.robots import PointAxisWaypoints
from compas_fab.utilities import from_tcf_to_t0cf


class AnalyticalPlanCartesianMotion(PlanCartesianMotion):
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,17 +12,15 @@
from compas_fab.robots import Robot # noqa: F401
from compas_fab.robots import Target # noqa: F401
from typing import Generator # noqa: F401
from typing import Dict, List, Optional, Tuple # noqa: F401
from compas_robots import Configuration # noqa: F401
from typing import Dict # noqa: F401
from typing import Optional # noqa: F401
from compas_fab.backends import PyBulletClient # noqa: F401

from compas_robots import Configuration

from compas_fab.backends import CollisionCheckError
from compas_fab.robots import FrameTarget

from ..utils import joint_angles_to_configurations
from ..utils import try_to_fit_configurations_between_bounds


class AnalyticalPybulletInverseKinematics(AnalyticalInverseKinematics):
"""Callable to calculate the robot's inverse kinematics for a given frame.
Expand Down Expand Up @@ -73,7 +71,7 @@ def iter_inverse_kinematics_frame_target(self, target, start_state, group, optio

options = options or {}
planner = self # type: AnalyticalPyBulletPlanner
robot = planner.client.robot_cell.robot # type: Robot
robot = planner.client.robot_cell.robot # type: Robot # noqa: F841
client = self.client # type: PyBulletClient

# Set robot cell state to start state if provided
Expand Down Expand Up @@ -102,7 +100,7 @@ def iter_inverse_kinematics_frame_target(self, target, start_state, group, optio
client.set_robot_configuration(configuration)
# Passing the `_skip_set_robot_cell_state` option to the collision check function
planner.check_collision(None, options={"_skip_set_robot_cell_state": True})
except CollisionCheckError as e:
except CollisionCheckError:
# If keep order is true, yield a None to keep the order of the solutions
if options.get("keep_order", True):
yield None
Expand Down
Original file line number Diff line number Diff line change
@@ -1,26 +1,13 @@
from compas_fab.backends.interfaces import SetRobotCellState

import compas
from compas.geometry import Frame

from compas_fab.backends.interfaces import SetRobotCellState

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 typing import List # noqa: F401
from typing import Tuple # noqa: F401

from compas_fab.robots import Robot # noqa: F401
from compas_robots import Configuration # noqa: F401
from compas.geometry import Frame # noqa: F401
from compas_fab.backends.interfaces import ClientInterface # noqa: F401
from compas_fab.robots import RobotCell # noqa: F401
from compas_fab.robots import RobotCellState # noqa: F401
from compas_fab.backends import PyBulletClient # noqa: F401

from compas_fab.backends.pybullet.const import STATIC_MASS
from compas_fab.robots import RobotCellState # noqa: F401


class AnalyticalSetRobotCellState(SetRobotCellState):
Expand Down
30 changes: 19 additions & 11 deletions src/compas_fab/backends/kinematics/planner.py
Original file line number Diff line number Diff line change
@@ -1,18 +1,26 @@
import compas
from compas_fab.backends.interfaces.planner import PlannerInterface

from compas_fab.backends.pybullet.backend_features import *
from compas_fab.backends.kinematics.backend_features import *
from compas_fab.backends.kinematics.solvers import *

from compas_fab.backends.kinematics.client import AnalyticalKinematicsClient # noqa: F401
from compas_fab.backends.pybullet.client import PyBulletClient # noqa: F401
from compas import IPY

if compas.IPY:
from compas_fab.backends.interfaces.planner import PlannerInterface
from compas_fab.backends.kinematics.backend_features import AnalyticalForwardKinematics
from compas_fab.backends.kinematics.backend_features import AnalyticalInverseKinematics
from compas_fab.backends.kinematics.backend_features import AnalyticalPlanCartesianMotion
from compas_fab.backends.kinematics.backend_features import AnalyticalPybulletInverseKinematics
from compas_fab.backends.kinematics.backend_features import AnalyticalSetRobotCell
from compas_fab.backends.kinematics.backend_features import AnalyticalSetRobotCellState
from compas_fab.backends.kinematics.client import AnalyticalKinematicsClient
from compas_fab.backends.pybullet.backend_features import PyBulletCheckCollision
from compas_fab.backends.pybullet.backend_features import PyBulletForwardKinematics
from compas_fab.backends.pybullet.backend_features import PyBulletSetRobotCell
from compas_fab.backends.pybullet.backend_features import PyBulletSetRobotCellState

if IPY:
from typing import TYPE_CHECKING

if TYPE_CHECKING:
from typing import Optional
from typing import Optional # noqa: F401

from compas_fab.backends.kinematics.solvers import AnalyticalKinematics # noqa: F401
from compas_fab.backends.pybullet.client import PyBulletClient # noqa: F401

__all__ = [
"AnalyticalPyBulletPlanner",
Expand Down
1 change: 0 additions & 1 deletion src/compas_fab/backends/kinematics/solvers/__init__.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@
from .analytical_kinematics import AnalyticalKinematics # noqa: F403
from .spherical_wrist_kinematics import * # noqa: F403
from .offset_wrist_kinematics import * # noqa: F403

Expand Down
Original file line number Diff line number Diff line change
@@ -1,6 +1,13 @@
from compas.geometry import Frame
from compas import IPY

from typing import List, Optional
if IPY:
from typing import TYPE_CHECKING

if TYPE_CHECKING:
from typing import List # noqa: F401
from typing import Optional # noqa: F401

from compas.geometry import Frame # noqa: F401


class AnalyticalKinematics(object):
Expand Down
7 changes: 6 additions & 1 deletion src/compas_fab/backends/kinematics/solvers/offset_wrist.py
Original file line number Diff line number Diff line change
Expand Up @@ -7,11 +7,16 @@
from math import sin
from math import sqrt

from compas import IPY
from compas.geometry import Frame

from compas_fab.utilities import sign

from typing import List
if IPY:
from typing import TYPE_CHECKING

if TYPE_CHECKING:
from typing import List # noqa: F401


def forward_kinematics_offset_wrist(joint_values, params):
Expand Down
Loading

0 comments on commit ba854fe

Please sign in to comment.