diff --git a/CHANGELOG.md b/CHANGELOG.md index 67bca16a77..16d86a04b8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added +* Added `Robot.from_urdf()` method to load a robot from a URDF file, can optionally take SRDF and meshes. * Added `compas_fab.robots.Waypoints` class to represent a sequence of targets. It has two child classes: `FrameWaypoints` and `PointAxisWaypoints`. * Added `compas_fab.robots.Target` class to represent a motion planning target. * Added also child classes `FrameTarget`, `PointAxisTarget`, `ConfigurationTarget`, `ConstraintSetTarget` diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index 54b60a7021..cdc182de9b 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_fab from compas.data import Data from compas.geometry import Frame @@ -11,6 +12,7 @@ from compas_robots import Configuration from compas_robots import RobotModel from compas_robots.model import Joint +from compas_robots.resources import LocalPackageMeshLoader from compas_fab.robots.constraints import Constraint @@ -110,6 +112,49 @@ def __from_data__(cls, data): robot.attributes = attributes return robot + @classmethod + def from_urdf(cls, urdf_filename, srdf_filename=None, local_package_mesh_folder=None, client=None): + # type: (str, Optional[str], Optional[str], Optional[ClientInterface]) -> Robot + """Create a robot from URDF. + Optionally, SRDF can be provided to load semantics and a local package mesh folder to load mesh geometry. + + Parameters + ---------- + urdf_filename : :obj:`str` + Path to the URDF file. + srdf_filename : :obj:`str`, optional + Path to the SRDF file to load semantics. Default is `None`. + local_package_mesh_folder : :obj:`str`, optional + Path to the local package mesh folder. + If the path is provided, the geometry of the robot is loaded from this folder. + Default is `None`, which means that the geometry is not loaded. + client : :class:`compas_fab.backends.interfaces.ClientInterface`, optional + Backend client provided for the . Default is `None`. + + Returns + ------- + :class:`compas_fab.robots.Robot` + Newly created instance of the robot. + + """ + # NOTE: This import is here to avoid circular imports + from compas_fab.robots import RobotSemantics + + model = RobotModel.from_urdf_file(urdf_filename) + + if srdf_filename: + semantics = RobotSemantics.from_srdf_file(srdf_filename, model) + else: + semantics = None + + if local_package_mesh_folder: + loader = LocalPackageMeshLoader(compas_fab.get(local_package_mesh_folder), "") + model.load_geometry(loader) + + robot = cls(model, semantics=semantics, client=client) + + return robot + @property def scene_object(self): """Scene object used to visualize robot model.""" diff --git a/src/compas_fab/robots/robot_library.py b/src/compas_fab/robots/robot_library.py index d89739316b..0662b9a682 100644 --- a/src/compas_fab/robots/robot_library.py +++ b/src/compas_fab/robots/robot_library.py @@ -2,12 +2,8 @@ from __future__ import division from __future__ import print_function -from compas_robots import RobotModel -from compas_robots.resources import LocalPackageMeshLoader - import compas_fab -from .robot import Robot -from .semantics import RobotSemantics +from compas_fab.robots import Robot __all__ = [ "RobotLibrary", @@ -32,26 +28,6 @@ class RobotLibrary(object): 'ur5_robot' """ - @classmethod - 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.""" - - model = RobotModel.from_urdf_file(urdf_filename) - semantics = RobotSemantics.from_srdf_file(srdf_filename, model) - - if load_geometry: - loader = LocalPackageMeshLoader(compas_fab.get(local_package_mesh_folder), "") - model.load_geometry(loader) - - robot = Robot(model, semantics=semantics) - - if client: - robot.client = client - - return robot - @classmethod def rfl(cls, client=None, load_geometry=True): """Create and return the RFL robot with 4 ABB irb 4600 and twin-gantry setup. @@ -72,12 +48,11 @@ def rfl(cls, client=None, load_geometry=True): Newly created instance of the robot. """ - robot = cls._load_library_model( + robot = Robot.from_urdf( urdf_filename=compas_fab.get("robot_library/rfl/urdf/robot_description.urdf"), srdf_filename=compas_fab.get("robot_library/rfl/robot_description_semantic.srdf"), - local_package_mesh_folder="robot_library/rfl", + local_package_mesh_folder="robot_library/rfl" if load_geometry else None, client=client, - load_geometry=load_geometry, ) return robot @@ -102,12 +77,11 @@ def ur5(cls, client=None, load_geometry=True): Newly created instance of the robot. """ - robot = cls._load_library_model( + robot = Robot.from_urdf( urdf_filename=compas_fab.get("robot_library/ur5_robot/urdf/robot_description.urdf"), srdf_filename=compas_fab.get("robot_library/ur5_robot/robot_description_semantic.srdf"), - local_package_mesh_folder="robot_library/ur5_robot", + local_package_mesh_folder="robot_library/ur5_robot" if load_geometry else None, client=client, - load_geometry=load_geometry, ) return robot @@ -132,12 +106,11 @@ def ur10e(cls, client=None, load_geometry=True): Newly created instance of the robot. """ - robot = cls._load_library_model( + robot = Robot.from_urdf( urdf_filename=compas_fab.get("robot_library/ur10e_robot/urdf/robot_description.urdf"), srdf_filename=compas_fab.get("robot_library/ur10e_robot/robot_description_semantic.srdf"), - local_package_mesh_folder="robot_library/ur10e_robot", + local_package_mesh_folder="robot_library/ur10e_robot" if load_geometry else None, client=client, - load_geometry=load_geometry, ) return robot @@ -162,12 +135,11 @@ def abb_irb4600_40_255(cls, client=None, load_geometry=True): Newly created instance of the robot. """ - robot = cls._load_library_model( + robot = Robot.from_urdf( urdf_filename=compas_fab.get("robot_library/abb_irb4600_40_255/urdf/robot_description.urdf"), srdf_filename=compas_fab.get("robot_library/abb_irb4600_40_255/robot_description_semantic.srdf"), - local_package_mesh_folder="robot_library/abb_irb4600_40_255", + local_package_mesh_folder="robot_library/abb_irb4600_40_255" if load_geometry else None, client=client, - load_geometry=load_geometry, ) return robot