diff --git a/Descriptor/Descriptor.py b/Descriptor/Descriptor.py index bec8c35..4d64ad4 100644 --- a/Descriptor/Descriptor.py +++ b/Descriptor/Descriptor.py @@ -1,8 +1,7 @@ -import adsk.core, adsk.fusion, adsk.cam, traceback +import adsk.core, adsk.fusion, traceback from .core.ui import config_settings from .core import manager -from .core import io ui_handlers = [] diff --git a/Descriptor/core/io.py b/Descriptor/core/io.py index b32ab6e..51efb98 100644 --- a/Descriptor/core/io.py +++ b/Descriptor/core/io.py @@ -1,14 +1,27 @@ -import os +import os.path, sys, fileinput +from typing import Dict, List, Sequence, Tuple +from xml.etree.ElementTree import Element, ElementTree, SubElement +import xml.etree.ElementTree as ET import adsk, adsk.core, adsk.fusion -from . import parts -from . import parser -from . import manager -from collections import Counter, defaultdict -def visible_to_stl(design, save_dir, root, accuracy, body_dict, sub_mesh, body_mapper, _app): +from .parser import Configurator +from .parts import Joint, Link +from . import utils +from shutil import copytree + + +def visible_to_stl( + design: adsk.fusion.Design, + save_dir: str, + root: adsk.fusion.Component, + accuracy: adsk.fusion.MeshRefinementSettings, + sub_mesh: bool, + body_mapper: Dict[str, List[Tuple[adsk.fusion.BRepBody, str]]], + _app, +): """ export top-level components as a single stl file into "save_dir/" - + Parameters ---------- design: adsk.fusion.Design @@ -17,65 +30,48 @@ def visible_to_stl(design, save_dir, root, accuracy, body_dict, sub_mesh, body_m directory path to save root: adsk.fusion.Component root component of the design - accuracy: int + accuracy: adsk.fusion.MeshRefinementSettings enum accuracy value to use for stl export component_map: list list of all bodies to use for stl export """ - + # create a single exportManager instance exporter = design.exportManager - # Setup new document for saving to - des: adsk.fusion.Design = _app.activeProduct - - newDoc: adsk.core.Document = _app.documents.add(adsk.core.DocumentTypes.FusionDesignDocumentType, True) - newDes: adsk.fusion.Design = newDoc.products.itemByProductType('DesignProductType') + newDoc: adsk.core.Document = _app.documents.add(adsk.core.DocumentTypes.FusionDesignDocumentType, True) + newDes = newDoc.products.itemByProductType("DesignProductType") + assert isinstance(newDes, adsk.fusion.Design) newRoot = newDes.rootComponent # get the script location - save_dir = os.path.join(save_dir,'meshes') - try: os.mkdir(save_dir) - except: pass - - # Export top-level occurrences - occ = root.occurrences.asList - # hack for correct stl placement by turning off all visibility first - visible_components = [] - for oc in occ: - if oc.isLightBulbOn: - visible_components.append(oc) - - # Make sure no repeated body names - body_count = Counter() - - for oc in visible_components: - # Create a new exporter in case its a memory thing - exporter = design.exportManager - - occName = oc.name.replace(':', '_').replace(' ','') - - component_exporter(exporter, newRoot, body_mapper[oc.entityToken], os.path.join(save_dir,f'{occName}')) - - if sub_mesh: - # get the bodies associated with this top-level component (which will contain sub-components) - bodies = body_mapper[oc.entityToken] + save_dir = os.path.join(save_dir, "meshes") + try: + os.mkdir(save_dir) + except: + pass - for body in bodies: - if body.isLightBulbOn: + try: + for name, bodies in body_mapper.items(): + if not bodies: + continue - # Since there are alot of similar names, we need to store the parent component as well in the filename - body_name = body.name.replace(':','_').replace(' ','') - body_name_cnt = f'{body_name}_{body_count[body_name]}' - body_count[body_name] += 1 + # Create a new exporter in case its a memory thing + exporter = design.exportManager - save_name = os.path.join(save_dir,f'{occName}_{body_name_cnt}') + occName = utils.format_name(name) + stl_exporter(exporter, accuracy, newRoot, [b for b, _ in bodies], os.path.join(save_dir, occName)) - body_exporter(exporter, newRoot, body, save_name) + if sub_mesh and len(bodies) > 1: + for body, body_name in bodies: + if body.isVisible: + stl_exporter(exporter, accuracy, newRoot, [body], os.path.join(save_dir, body_name)) + finally: + newDoc.close(False) -def component_exporter(exportMgr, newRoot, body_lst, filename): - ''' Copy a component to a new document, save, then delete. +def stl_exporter(exportMgr, accuracy, newRoot, body_lst, filename): + """Copy a component to a new document, save, then delete. Modified from solution proposed by BrianEkins https://EkinsSolutions.com @@ -89,7 +85,7 @@ def component_exporter(exportMgr, newRoot, body_lst, filename): _description_ filename : _type_ _description_ - ''' + """ tBrep = adsk.fusion.TemporaryBRepManager.get() @@ -97,76 +93,25 @@ def component_exporter(exportMgr, newRoot, body_lst, filename): bf.startEdit() for body in body_lst: - if not body.isLightBulbOn: continue tBody = tBrep.copy(body) newRoot.bRepBodies.add(tBody, bf) bf.finishEdit() - stlOptions = exportMgr.createSTLExportOptions(newRoot, f'{filename}.stl') + stlOptions = exportMgr.createSTLExportOptions(newRoot, f"{filename}.stl") + stlOptions.meshRefinement = accuracy exportMgr.execute(stlOptions) bf.deleteMe() -def body_exporter(exportMgr, newRoot, body, filename): - tBrep = adsk.fusion.TemporaryBRepManager.get() - - tBody = tBrep.copy(body) - - bf = newRoot.features.baseFeatures.add() - bf.startEdit() - newRoot.bRepBodies.add(tBody, bf) - bf.finishEdit() - - newBody = newRoot.bRepBodies[0] - - stl_options = exportMgr.createSTLExportOptions(newBody, filename) - stl_options.sendToPrintUtility = False - stl_options.isBinaryFormat = True - # stl_options.meshRefinement = accuracy - exportMgr.execute(stl_options) - - bf.deleteMe() class Writer: - def __init__(self) -> None: - pass + def __init__(self, save_dir: str, config: Configurator) -> None: + self.save_dir = save_dir + self.config = config - def write_link(self, config, file_name): - ''' Write links information into urdf file_name - - Parameters - ---------- - config : Configurator - root nodes instance of configurator class - file_name: str - urdf full path - - ''' - - with open(file_name, mode='a', encoding="utf-8") as f: - for _, link in config.links.items(): - f.write(f'{link.link_xml}\n') - - def write_joint(self, file_name, config): - ''' Write joints and transmission information into urdf file_name - - Parameters - ---------- - file_name: str - urdf full path - config : Configurator - root nodes instance of configurator class - - ''' - - with open(file_name, mode='a', encoding="utf-8") as f: - for _, joint in config.joints.items(): - f.write(f'{joint.joint_xml}\n') - - - def write_urdf(self, save_dir, config): - ''' Write each component of the xml structure to file + def write_urdf(self) -> None: + """Write each component of the xml structure to file Parameters ---------- @@ -174,28 +119,74 @@ def write_urdf(self, save_dir, config): path to save file config : Configurator root nodes instance of configurator class - ''' - - save_dir = os.path.join(save_dir,'urdf') - try: os.mkdir(save_dir) - except: pass - file_name = os.path.join(save_dir, f'{config.name}.urdf') # the name of urdf file - - with open(file_name, mode='w', encoding="utf-8") as f: - f.write('\n') - f.write(f'\n\n') - f.write('\n') - f.write(' \n') - f.write('\n\n') - - self.write_link(config, file_name) - self.write_joint(file_name, config) - - with open(file_name, mode='a') as f: - f.write('\n') - -def write_hello_pybullet(robot_name, save_dir): - ''' Writes a sample script which loads the URDF in pybullet + """ + + try: + os.mkdir(self.save_dir) + except: + pass + file_name = os.path.join(self.save_dir, f"{self.config.name}.xacro") # the name of urdf file + if self.config.extra_links: + links = self.config.links.copy() + for link in self.config.extra_links: + del links[link] + joints = { + joint: self.config.joints[joint] + for joint in self.config.joints + if self.config.joints[joint].child not in self.config.extra_links + } + self.write_xacro(file_name, links, joints) + file_name_full = os.path.join(self.save_dir, f"{self.config.name}-full.xacro") + self.write_xacro(file_name_full, {link: self.config.links[link] for link in self.config.extra_links}, {}) + else: + self.write_xacro(file_name, self.config.links, self.config.joints) + + material_file_name = os.path.join(self.save_dir, f"materials.xacro") + self.write_materials_xacro(material_file_name) + + def write_materials_xacro(self, material_file_name) -> None: + robot = Element("robot", {"name": self.config.name, "xmlns:xacro": "http://www.ros.org/wiki/xacro"}) + for color in self.config.color_dict: + material = SubElement(robot, "material", {"name": color}) + SubElement(material, "color", {"rgba": self.config.color_dict[color]}) + + tree = ElementTree(robot) + ET.indent(tree, space=" ") + + with open(material_file_name, mode="wb") as f: + tree.write(f, "utf-8", xml_declaration=True) + f.write(b"\n") + + def write_xacro(self, file_name: str, links: Dict[str, Link], joints: Dict[str, Joint]) -> None: + robot = Element("robot", {"xmlns:xacro": "http://www.ros.org/wiki/xacro", "name": self.config.name}) + SubElement(robot, "xacro:include", {"filename": f"$(find {self.config.name})/urdf/materials.xacro"}) + + # Add dummy link since KDL does not support a root link with an inertia + # From https://robotics.stackexchange.com/a/97510 + SubElement(robot, "link", {"name": "dummy_link"}) + assert self.config.base_link is not None + dummy_joint = SubElement(robot, "joint", {"name": "dummy_link_joint", "type": "fixed"}) + SubElement(dummy_joint, "parent", {"link": "dummy_link"}) + SubElement(dummy_joint, "child", {"link": self.config.base_link_name}) + + for _, link in links.items(): + xml = link.link_xml() + if xml is not None: + robot.append(xml) + + for _, joint in joints.items(): + robot.append(joint.joint_xml()) + + tree = ElementTree(robot) + ET.indent(tree, space=" ") + + with open(file_name, mode="wb") as f: + tree.write(f, "utf-8", xml_declaration=True) + f.write(b"\n") + + +def write_hello_pybullet(robot_name, save_dir) -> None: + """Writes a sample script which loads the URDF in pybullet Modified from https://github.com/yanshil/Fusion2PyBullet @@ -205,10 +196,10 @@ def write_hello_pybullet(robot_name, save_dir): name to use for directory save_dir : str path to store file - ''' + """ - robot_urdf = f'{robot_name}.urdf' ## basename of robot.urdf - file_name = os.path.join(save_dir,'hello_bullet.py') + robot_urdf = f"{robot_name}.urdf" ## basename of robot.urdf + file_name = os.path.join(save_dir, "hello_bullet.py") hello_pybullet = """ import pybullet as p import os @@ -234,7 +225,78 @@ def write_hello_pybullet(robot_name, save_dir): print(cubePos,cubeOrn) p.disconnect() """ - hello_pybullet = hello_pybullet.replace('TEMPLATE.urdf', robot_urdf) - with open(file_name, mode='w') as f: + hello_pybullet = hello_pybullet.replace("TEMPLATE.urdf", robot_urdf) + with open(file_name, mode="w") as f: f.write(hello_pybullet) - f.write('\n') + f.write("\n") + + +def copy_ros2(save_dir, package_name) -> None: + # Use current directory to find `package_ros2` + package_ros2_path = os.path.dirname(os.path.abspath(os.path.dirname(__file__))) + "/package_ros2/" + copy_package(save_dir, package_ros2_path) + update_cmakelists(save_dir, package_name) + update_package_xml(save_dir, package_name) + update_package_name(save_dir + "/launch/robot_description.launch.py", package_name) + + +def copy_gazebo(save_dir, package_name) -> None: + # Use current directory to find `gazebo_package` + gazebo_package_path = os.path.dirname(os.path.abspath(os.path.dirname(__file__))) + "/gazebo_package/" + copy_package(save_dir, gazebo_package_path) + update_cmakelists(save_dir, package_name) + update_package_xml(save_dir, package_name) + update_package_name(save_dir + "/launch/robot_description.launch.py", package_name) # Also include rviz alone + update_package_name(save_dir + "/launch/gazebo.launch.py", package_name) + + +def copy_moveit(save_dir, package_name) -> None: + # Use current directory to find `moveit_package` + moveit_package_path = os.path.dirname(os.path.abspath(os.path.dirname(__file__))) + "/moveit_package/" + copy_package(save_dir, moveit_package_path) + update_cmakelists(save_dir, package_name) + update_package_xml(save_dir, package_name) + update_package_name(save_dir + "/launch/setup_assistant.launch.py", package_name) + + +def copy_package(save_dir, package_dir) -> None: + try: + os.mkdir(save_dir + "/launch") + except: + pass + try: + os.mkdir(save_dir + "/urdf") + except: + pass + copytree(package_dir, save_dir, dirs_exist_ok=True) + + +def update_cmakelists(save_dir, package_name) -> None: + file_name = save_dir + "/CMakeLists.txt" + + for line in fileinput.input(file_name, inplace=True): + if "project(fusion2urdf)" in line: + sys.stdout.write("project(" + package_name + ")\n") + else: + sys.stdout.write(line) + + +def update_package_name(file_name, package_name) -> None: + # Replace 'fusion2urdf' with the package_name + for line in fileinput.input(file_name, inplace=True): + if "fusion2urdf" in line: + sys.stdout.write(line.replace("fusion2urdf", package_name)) + else: + sys.stdout.write(line) + + +def update_package_xml(save_dir, package_name) -> None: + file_name = save_dir + "/package.xml" + + for line in fileinput.input(file_name, inplace=True): + if "" in line: + sys.stdout.write(" " + package_name + "\n") + elif "" in line: + sys.stdout.write("The " + package_name + " package\n") + else: + sys.stdout.write(line) diff --git a/Descriptor/core/manager.py b/Descriptor/core/manager.py index 9162f18..f03a5ea 100644 --- a/Descriptor/core/manager.py +++ b/Descriptor/core/manager.py @@ -1,22 +1,30 @@ -import os -from functools import partial - -import adsk +from collections import OrderedDict +from datetime import datetime +import subprocess +from typing import Dict, List, Optional +import os +import os.path +import zoneinfo +import yaml + +import adsk.core +import adsk.fusion from . import parser from . import io +from . import utils class Manager: ''' Manager class for setting params and generating URDF ''' - root = None - design = None - _app = None + root: Optional[adsk.fusion.Component] = None + design: Optional[adsk.fusion.Design] = None + _app: Optional[adsk.core.Application] = None - def __init__(self, save_dir, save_mesh, sub_mesh, mesh_resolution, inertia_precision, - document_units, target_units, joint_order, target_platform) -> None: + def __init__(self, save_dir, robot_name, save_mesh, sub_mesh, mesh_resolution, inertia_precision, + target_units, target_platform, config_file) -> None: '''Initialization of Manager class Parameters @@ -33,23 +41,29 @@ def __init__(self, save_dir, save_mesh, sub_mesh, mesh_resolution, inertia_preci base units of current file target_units : str target files units - joint_order : str - if parent or child should be component 1 target_platform : str which configuration to use for exporting urdf ''' self.save_mesh = save_mesh self.sub_mesh = sub_mesh - if document_units=='mm': doc_u = 0.001 - elif document_units=='cm': doc_u = 0.01 - elif document_units=='m': doc_u = 1.0 + assert self.design is not None + + doc_u = 1.0 + if self.design.unitsManager.defaultLengthUnits=='mm': doc_u = 0.001 + elif self.design.unitsManager.defaultLengthUnits=='cm': doc_u = 0.01 + elif self.design.unitsManager.defaultLengthUnits=='m': doc_u = 1.0 + else: + raise ValueError(f"Inexpected document units: '{self.design.unitsManager.defaultLengthUnits}'") + + tar_u = 1.0 if target_units=='mm': tar_u = 0.001 elif target_units=='cm': tar_u = 0.01 elif target_units=='m': tar_u = 1.0 - self.scale = tar_u / doc_u + self.scale = doc_u / tar_u + self.cm = 0.01 / tar_u if inertia_precision == 'Low': self.inert_accuracy = adsk.fusion.CalculationAccuracy.LowCalculationAccuracy @@ -64,17 +78,43 @@ def __init__(self, save_dir, save_mesh, sub_mesh, mesh_resolution, inertia_preci self.mesh_accuracy = adsk.fusion.MeshRefinementSettings.MeshRefinementMedium elif mesh_resolution == 'High': self.mesh_accuracy = adsk.fusion.MeshRefinementSettings.MeshRefinementHigh - - if joint_order == 'Parent': - self.joint_order = ('p','c') - elif joint_order == 'Child': - self.joint_order = ('c','p') - else: - raise ValueError(f'Order method not supported') # set the target platform self.target_platform = target_platform + self.robot_name = robot_name + + self.name_map: Dict[str, str] = {} + self.merge_links: Dict[str, List[str]] = {} + self.extra_links: List[str] = [] + self.locations: Dict[str, Dict[str,str]] = {} + self.root_name: Optional[str] = None + if config_file: + with open(config_file, "rb") as yml: + configuration = yaml.load(yml, yaml.SafeLoader) + if not isinstance(configuration, dict): + raise(ValueError(f"Malformed config '{config_file}': top level should be a dictionary")) + wrong_keys = set(configuration).difference([ + "RobotName", "SaveMesh", "SubMesh", "MeshResolution", "InertiaPrecision", + "TargetUnits", "TargetPlatform", "NameMap", "MergeLinks", + "Locations", "Extras", "Root", + ]) + if wrong_keys: + raise(ValueError(f"Malformed config '{config_file}': unexpected top-level keys: {list(wrong_keys)}")) + self.name_map = configuration.get("NameMap", {}) + if self.name_map is None: + self.name_map = {} + self.merge_links = configuration.get("MergeLinks", {}) + if self.merge_links is None: + self.merge_links = {} + self.extra_links = configuration.get("Extras", []) + if self.extra_links is None: + self.extra_links = [] + self.locations = configuration.get("Locations", {}) + if self.locations is None: + self.locations = {} + self.root_name = configuration.get("Root") + # Set directory self._set_dir(save_dir) @@ -86,9 +126,8 @@ def _set_dir(self, save_dir): save_dir : str path to save ''' - # set the names - robot_name = Manager.root.name.split()[0] - package_name = robot_name + '_description' + # set the names + package_name = self.robot_name + '_description' self.save_dir = os.path.join(save_dir, package_name) try: os.mkdir(self.save_dir) @@ -102,38 +141,103 @@ def preview(self): dict mapping of joint names with parent-> child relationship ''' - - config = parser.Configurator(Manager.root) + assert Manager.root is not None + + config = parser.Configurator(Manager.root, self.scale, self.cm, self.robot_name, self.name_map, self.merge_links, self.locations, self.extra_links, self.root_name) config.inertia_accuracy = self.inert_accuracy - config.joint_order = self.joint_order - config.scale = self.scale ## Return array of tuples (parent, child) config.get_scene_configuration() return config.get_joint_preview() + @staticmethod + def get_git_info() -> str: + my_dir = os.path.abspath(os.path.dirname(__file__)) + git = ["git", "-C", my_dir] + def call_git(cmd: List[str]) -> str: + cmd = git + cmd + out = subprocess.check_output(cmd, stderr=subprocess.DEVNULL) + return out.decode("utf-8").strip() + try: + if call_git(['rev-parse', '--is-inside-work-tree']) != "true": + return "" + url = "" + try: + branch = call_git(['rev-parse', '--abbrev-ref', '--symbolic-full-name', '@{u}']) + if "/" in branch: + remote = branch.split('/')[0] + url = call_git(['remote', 'get-url', remote]) + except subprocess.CalledProcessError: + pass + commit_hash = call_git(["rev-parse", "HEAD"]) + commit_timestamp = call_git(["log", "-1", "--format=%ct"]) + commit_author = call_git(["log", "-1", "--format=%an"]) + commit_datetime = datetime.fromtimestamp(int(commit_timestamp), tz=zoneinfo.ZoneInfo("UTC")) + commit_date = commit_datetime.astimezone().strftime("%A, %B %d, %Y at %I:%M %p %Z") + if url: + repo_name = url + else: + repo_name = os.path.basename(os.path.dirname(os.path.dirname(my_dir))) + return f"{repo_name} rev {commit_hash} dated {commit_date} by {commit_author}" + except subprocess.CalledProcessError: + return "" def run(self): ''' process the scene, including writing to directory and exporting mesh, if applicable - ''' - - config = parser.Configurator(Manager.root) + ''' + assert Manager.root is not None + assert Manager.design is not None + + utils.start_log_timer() + if self._app is not None and self._app.activeViewport is not None: + utils.viewport = self._app.activeViewport + utils.log("*** Parsing ***") + config = parser.Configurator(Manager.root, self.scale, self.cm, self.robot_name, self.name_map, self.merge_links, self.locations, self.extra_links, self.root_name) config.inertia_accuracy = self.inert_accuracy - config.scale = self.scale - config.joint_order = self.joint_order config.sub_mesh = self.sub_mesh + utils.log("** Getting scene configuration **") config.get_scene_configuration() + utils.log("** Parsing the configuration **") config.parse() # -------------------- # Generate URDF - writer = io.Writer() - writer.write_urdf(self.save_dir, config) - + utils.log(f"*** Generating URDF under {os.path.realpath(self.save_dir)} ***") + self.urdf_dir = os.path.join(self.save_dir,'urdf') + writer = io.Writer(self.urdf_dir, config) + writer.write_urdf() + + if config.locs: + dict = {l: {loc.name: {"xyz": loc.xyz, "rpy": loc.rpy} for loc in locs} for l, locs in config.locs.items()} + with open(os.path.join(self.urdf_dir, "locations.yaml"), "wt") as f: + yaml.dump(dict, f, yaml.SafeDumper, default_flow_style=None, sort_keys=False, indent=3) + + with open(os.path.join(self.urdf_dir, "fusion2urdf.txt"), "wt") as f: + git_info = self.get_git_info() + if git_info: + git_info = f"\n\tusing {git_info}" + f.write(f"URDF structure created from Fusion Model {Manager.root.name}{git_info}:\n") + for s in config.tree_str: + f.write(s) + f.write("\n") + + utils.log(f"*** Generating {self.target_platform} configuration") if self.target_platform == 'pyBullet': io.write_hello_pybullet(config.name, self.save_dir) + elif self.target_platform == 'rviz': + io.copy_ros2(self.save_dir, config.name) + elif self.target_platform == 'Gazebo': + io.copy_gazebo(self.save_dir, config.name) + elif self.target_platform == 'MoveIt': + io.copy_moveit(self.save_dir, config.name) + # Custom STL Export if self.save_mesh: - io.visible_to_stl(Manager.design, self.save_dir, Manager.root, self.mesh_accuracy, config.body_dict, self.sub_mesh, config.body_mapper, Manager._app) + utils.log("*** Generating mesh STLs ***") + io.visible_to_stl(Manager.design, self.save_dir, Manager.root, self.mesh_accuracy, self.sub_mesh, config.body_dict, Manager._app) + utils.log(f"*** Done! Time elapsed: {utils.time_elapsed():.1f}s ***") + if utils.all_warnings: + utils.log(f"There were {len(utils.all_warnings)} warnings!\n\t" + "\n\t".join(utils.all_warnings)) + utils.all_warnings = [] diff --git a/Descriptor/core/parser.py b/Descriptor/core/parser.py index f5403a4..59dfa11 100644 --- a/Descriptor/core/parser.py +++ b/Descriptor/core/parser.py @@ -2,15 +2,32 @@ module to parse fusion file ''' -import copy -import adsk, adsk.core, adsk.fusion +from typing import Dict, Iterable, List, Optional, Sequence, Set, Tuple, Union +from dataclasses import dataclass, field + +import adsk.core, adsk.fusion +import numpy as np from . import transforms from . import parts -from collections import Counter, defaultdict +from . import utils +from collections import OrderedDict, defaultdict + +@dataclass(frozen=True, kw_only=True, eq=False) +class JointInfo: + name: str + parent: str + child: str + type: str = "fixed" + origin: adsk.core.Vector3D = field(default_factory=adsk.core.Vector3D.create) + axis: adsk.core.Vector3D = field(default_factory=adsk.core.Vector3D.create) + upper_limit: float = 0.0 + lower_limit: float = 0.0 class Hierarchy: ''' hierarchy of the design space ''' + total_components = 0 + def __init__(self, component) -> None: ''' Initialize Hierarchy class to parse document and define component relationships. Uses a recursive traversal (based off of fusion example) and provides helper functions @@ -21,56 +38,51 @@ def __init__(self, component) -> None: fusions root component to use for traversal ''' - self.children = [] - self.component = component - self.name = component.name - self._parent = None + self.children: List["Hierarchy"] = [] + self.component: adsk.fusion.Occurrence = component + self.name: str = component.name + Hierarchy.total_components += 1 + if utils.LOG_DEBUG: + utils.log(f"... {Hierarchy.total_components}. Collected {self.name}...") - def _add_child(self, c): + def _add_child(self, c: "Hierarchy") -> None: self.children.append(c) - c.parent = self - def get_children(self): + def get_children(self) -> List["Hierarchy"]: return self.children - def get_all_children(self): + def get_all_children(self) -> Dict[str, "Hierarchy"]: ''' get all children and sub children of this instance ''' - child_map = {} - parent_stack = set() - parent_stack.update(self.get_children()) - while len(parent_stack) != 0: + child_map = OrderedDict() + parent_stack: List["Hierarchy"] = [] + parent_stack += self.get_children() + while parent_stack: # Pop an element form the stack (order shouldn't matter) - tmp = parent_stack.pop() + tmp = parent_stack.pop(0) # Add this child to the map # use the entity token, more accurate than the name of the component (since there are multiple) child_map[tmp.component.entityToken] = tmp - # Check if this child has children - if len(tmp.get_children())> 0: - # add them to the parent_stack - parent_stack.update(tmp.get_children()) - + parent_stack += tmp.get_children() return child_map - def get_flat_body(self): + def get_flat_body(self) -> List[adsk.fusion.BRepBody]: ''' get a flat list of all components and child components ''' child_list = [] - body_list = [] - parent_stack = set() + body_list: List[List[adsk.fusion.BRepBody]] = [] child_set = list(self.get_all_children().values()) if len(child_set) == 0: - body_list.append([self.component.bRepBodies.item(x) for x in range(0, self.component.bRepBodies.count) ]) + body_list.append(list(self.component.bRepBodies)) child_list = [x.children for x in child_set if len(x.children)>0] - childs = [] + parent_stack : List[Hierarchy] = [] for c in child_list: for _c in c: - childs.append(_c) + parent_stack.append(_c) - parent_stack.update(childs) closed_set = set() while len(parent_stack) != 0: @@ -79,7 +91,7 @@ def get_flat_body(self): closed_set.add(tmp) # Get any bodies directly associated with this component if tmp.component.bRepBodies.count > 0: - body_list.append([tmp.component.bRepBodies.item(x) for x in range(0, tmp.component.bRepBodies.count) ]) + body_list.append(list(tmp.component.bRepBodies)) # Check if this child has children if len(tmp.children)> 0: @@ -87,47 +99,19 @@ def get_flat_body(self): child_set = list(self.get_all_children().values()) child_list = [x.children for x in child_set if len(x.children)>0] - childs = [] for c in child_list: for _c in c: if _c not in closed_set: - childs.append(_c) + parent_stack.append(_c) - parent_stack.update(childs) - - flat_bodies = [] + flat_bodies: List[adsk.fusion.BRepBody] = [] for body in body_list: flat_bodies.extend(body) return flat_bodies - def get_all_parents(self): - ''' get all the parents of this instance ''' - - child_stack = set() - child_stack.add(self) - parent_map = [] - while len(child_stack) != 0: - tmp = child_stack.pop() - if tmp.parent is None: - return parent_map - parent_map.append(tmp.parent.component.entityToken) - child_stack.add(tmp.parent) - - return parent_map - - @property - def parent(self): - if self._parent is None: - return None - return self._parent - - @parent.setter - def parent(self,v): - self._parent = v - @staticmethod - def traverse(occurrences, parent=None): + def traverse(occurrences: adsk.fusion.OccurrenceList, parent: Optional["Hierarchy"] = None) -> "Hierarchy": '''Recursively create class instances and define a parent->child structure Based on the fusion 360 API docs @@ -143,26 +127,61 @@ def traverse(occurrences, parent=None): Instance of the class ''' + assert occurrences for i in range(0, occurrences.count): occ = occurrences.item(i) + cur = Hierarchy(occ) if parent is None: pass else: - parent._add_child(cur) if occ.childOccurrences: Hierarchy.traverse(occ.childOccurrences, parent=cur) - return cur + return cur # type: ignore[undef] + +def get_origin(o: Optional[adsk.core.Base]) -> Union[adsk.core.Vector3D, None]: + if isinstance(o, adsk.fusion.JointGeometry): + return o.origin.asVector() + elif o is None: + return None + elif isinstance(o, adsk.fusion.JointOrigin): + return get_origin(o.geometry) + else: + utils.fatal(f"parser.get_origin: unexpected {o} of type {type(o)}") + +def get_context_name(c: Optional[adsk.fusion.Occurrence]) -> str: + return c.name if c is not None else 'ROOT level' + +def getMatrixFromRoot(occ: Optional[adsk.fusion.Occurrence]) -> adsk.core.Matrix3D: + """ + Given an occurence, return its coordinate transform w.r.t the global frame + This is inspired by https://forums.autodesk.com/t5/fusion-api-and-scripts/how-to-get-the-joint-origin-in-world-context/m-p/10051818/highlight/true#M12545, + but somehow this is completely unneeded (and results in incorrect values) as .transform2 is already + always in global frame, despite what the documentation says!!! + """ + mat = adsk.core.Matrix3D.create() + while occ is not None: + mat.transformBy(occ.transform2) + occ = occ.assemblyContext + return mat class Configurator: - joint_type_list = [ 'fixed', 'revolute', 'prismatic', 'Cylinderical', - 'PinSlot', 'Planner', 'Ball'] # these are the names in urdf - - def __init__(self, root) -> None: + # Map to URDF type + joint_types: Dict[adsk.fusion.JointTypes, str] = { + adsk.fusion.JointTypes.RigidJointType: "fixed", + adsk.fusion.JointTypes.RevoluteJointType: "revolute", + adsk.fusion.JointTypes.SliderJointType: "prismatic", + adsk.fusion.JointTypes.CylindricalJointType: "Cylindrical_unsupported", + adsk.fusion.JointTypes.PinSlotJointType: "PinSlot_unsupported", + adsk.fusion.JointTypes.PlanarJointType: "planar", + adsk.fusion.JointTypes.BallJointType: "Ball_unsupported", + } + + def __init__(self, root: adsk.fusion.Component, scale: float, cm: float, name: str, name_map: Dict[str, str], merge_links: Dict[str, List[str]], locations: Dict[str, Dict[str, str]], extra_links: Sequence[str], root_name: Optional[str]) -> None: ''' Initializes Configurator class to handle building hierarchy and parsing Parameters ---------- @@ -172,45 +191,77 @@ def __init__(self, root) -> None: # Export top-level occurrences self.root = root self.occ = root.occurrences.asList - self.inertial_dict = {} self.inertia_accuracy = adsk.fusion.CalculationAccuracy.LowCalculationAccuracy - self.links_xyz_dict = {} # needed ? - self.sub_mesh = False - self.joints_dict = {} - self.body_dict = {} - self.links = {} # Link class - self.joints = {} # Joint class for writing to file - self.joint_order = ('p','c') # Order of joints defined by components - self.scale = 100.0 # Units to convert to meters (or whatever simulator takes) - self.inertia_scale = 10000.0 # units to convert mass - self.base_links= set() - # self.component_map = set() - - self.root_node = None - + self.links_by_token: Dict[str, str] = OrderedDict() + self.links_by_name : Dict[str, adsk.fusion.Occurrence] = OrderedDict() + self.joints_dict: Dict[str, JointInfo] = OrderedDict() + self.body_dict: Dict[str, List[Tuple[adsk.fusion.BRepBody, str]]] = OrderedDict() + self.material_dict: Dict[str, str] = OrderedDict() + self.color_dict: Dict[str, str] = OrderedDict() + self.links: Dict[str, parts.Link] = OrderedDict() # Link class + self.joints: Dict[str, parts.Joint] = OrderedDict() # Joint class for writing to file + self.locs: Dict[str, List[parts.Location]] = OrderedDict() + self.scale = scale # Convert autodesk units to meters (or whatever simulator takes) + self.cm = cm # Convert cm units to meters (or whatever simulator takes) + parts.Link.scale = str(self.scale) + self.eps = 1e-7 / self.scale + self.base_link: Optional[adsk.fusion.Occurrence] = None + self.component_map: Dict[str, Hierarchy] = OrderedDict() # Entity tokens for each component + self.bodies_collected: Set[str] = set() # For later sanity checking - all bodies passed to URDF + self.name_map = name_map + self.merge_links = merge_links + self.locations = locations + self.extra_links = extra_links + + self.root_node: Optional[Hierarchy] = None + self.root_name = root_name + + self.name = name + self.mesh_folder = f'{name}/meshes/' + + def close_enough(self, a, b) -> bool: + if isinstance(a, float) and isinstance(b, float): + return abs(a-b) < self.eps + elif isinstance(a, list) and isinstance(b, list): + assert len(a) == len(b) + return all((self.close_enough(aa,bb) for aa,bb in zip(a,b))) + elif isinstance(a, tuple) and isinstance(b, tuple): + assert len(a) == len(b) + return all((self.close_enough(aa,bb) for aa,bb in zip(a,b))) + elif isinstance(a, adsk.core.Vector3D) and isinstance(b, adsk.core.Vector3D): + return self.close_enough(a.asArray(), b.asArray()) + elif isinstance(a, adsk.core.Point3D) and isinstance(b, adsk.core.Point3D): + return self.close_enough(a.asArray(), b.asArray()) + elif isinstance(a, adsk.core.Matrix3D) and isinstance(b, adsk.core.Matrix3D): + return self.close_enough(a.asArray(), b.asArray()) + else: + utils.fatal(f"parser.Configurator.close_enough: {type(a)} and {type(b)}: not supported") + def get_scene_configuration(self): '''Build the graph of how the scene components are related ''' - + Hierarchy.total_components = 0 + utils.log("* Traversing the hierarchy *") self.root_node = Hierarchy(self.root) occ_list=self.root.occurrences.asList - Hierarchy.traverse(occ_list, self.root_node) + utils.log(f"* Collected {Hierarchy.total_components} components, processing *") self.component_map = self.root_node.get_all_children() - + utils.log("* Processing sub-bodies *") self.get_sub_bodies() return self.component_map - - def get_sub_bodies(self): + def get_sub_bodies(self) -> None: ''' temp fix for ensuring that a top-level component is associated with bodies''' # write the immediate children of root node - self.body_mapper = defaultdict(list) + self.body_mapper: Dict[str, List[adsk.fusion.BRepBody]] = defaultdict(list) + + assert self.root_node is not None # for k,v in self.component_map.items(): for v in self.root_node.children: @@ -218,32 +269,21 @@ def get_sub_bodies(self): children = set() children.update(v.children) - top_level_body = [v.component.bRepBodies.item(x) for x in range(0, v.component.bRepBodies.count) ] - top_level_body = [x for x in top_level_body if x.isLightBulbOn] + top_level_body = [x for x in v.component.bRepBodies if x.isVisible] # add to the body mapper - self.body_mapper[v.component.entityToken].extend(top_level_body) - - top_body_name = [x.name for x in top_level_body] + if top_level_body != []: + self.body_mapper[v.component.entityToken].extend(top_level_body) while children: cur = children.pop() children.update(cur.children) - sub_level_body = [cur.component.bRepBodies.item(x) for x in range(0, cur.component.bRepBodies.count) ] - sub_level_body = [x for x in sub_level_body if x.isLightBulbOn ] + sub_level_body = [x for x in cur.component.bRepBodies if x.isVisible] # add to this body mapper again - self.body_mapper[v.component.entityToken].extend(sub_level_body) - - sub_body_name = [x.name for x in sub_level_body] + self.body_mapper[cur.component.entityToken].extend(sub_level_body) - for oc in self.occ: - # Iterate through bodies, only add mass of bodies that are visible (lightbulb) - # body_cnt = oc.bRepBodies.count - # mapped_comp =self.component_map[oc.entityToken] - body_lst = self.component_map[oc.entityToken].get_flat_body() - - def get_joint_preview(self): + def get_joint_preview(self) -> Dict[str, JointInfo]: ''' Get the scenes joint relationships without calculating links Returns ------- @@ -257,267 +297,667 @@ def get_joint_preview(self): def parse(self): ''' parse the scene by building up inertia and joints''' - self._inertia() - self._joints() self._base() - self._build_links() - self._build_joints() - - @property - def name(self): - ''' Name of the root component ''' - return self.root.name.split()[0] + self._joints() + self._links() + self._materials() + self._build() def _base(self): - ''' Get the base link(s) ''' - - for oc in self.occ: - if oc.isGrounded: - name = oc.name - self.base_links.add(name) - - def _inertia(self): - ''' - Define inertia values - - Notes - ----- - Original Authors: @syuntoku, @yanshil - Modified by @cadop - ''' - - for oc in self.occ: - occs_dict = {} - prop = oc.getPhysicalProperties(self.inertia_accuracy) - - occs_dict['name'] = oc.name - - mass = prop.mass # kg - - # Iterate through bodies, only add mass of bodies that are visible (lightbulb) - # body_cnt = oc.bRepBodies.count - # mapped_comp =self.component_map[oc.entityToken] - body_lst = self.component_map[oc.entityToken].get_flat_body() - - if len(body_lst) > 0: - for body in body_lst: - # Check if this body is hidden - # - # body = oc.bRepBodies.item(i) - if not body.isLightBulbOn: - mass -= body.physicalProperties.mass - - - occs_dict['mass'] = mass - center_of_mass = [_/self.scale for _ in prop.centerOfMass.asArray()] ## cm to m - occs_dict['center_of_mass'] = center_of_mass - - - # https://help.autodesk.com/view/fusion360/ENU/?guid=GUID-ce341ee6-4490-11e5-b25b-f8b156d7cd97 - (_, xx, yy, zz, xy, yz, xz) = prop.getXYZMomentsOfInertia() - moment_inertia_world = [_ / self.inertia_scale for _ in [xx, yy, zz, xy, yz, xz] ] ## kg / cm^2 -> kg/m^2 - - occs_dict['inertia'] = transforms.origin2center_of_mass(moment_inertia_world, center_of_mass, mass) - - self.inertial_dict[oc.name] = occs_dict - - def _is_joint_valid(self, joint): - '''_summary_ - Parameters - ---------- - joint : _type_ - _description_ - ''' - - try: - joint.geometryOrOriginOne.origin.asArray() - joint.geometryOrOriginTwo.origin.asArray() - return True - - except: - return False + ''' Get the base link ''' + if self.root_name is not None and self.root_name in self.merge_links: + self.base_link = self._resolve_name(self.merge_links[self.root_name][0]) + else: + for oc in self._iterate_through_occurrences(): + # Get only the first grounded link + if ( + (self.root_name is None and oc.isGrounded) or + (self.root_name is not None and self.root_name == oc.name) + ): + # We must store this object because we cannot occurrences + self.base_link = oc + break + if self.base_link is None: + if self.root_name is None: + utils.fatal("Failed to find a grounded occurrence for URDF root. Make one of the Fusion occurrences grounded or specify 'Root: name' in the configuration file") + else: + utils.fatal(f"Occurrence '{self.root_name}' specified in the 'Root:' section of the configuration file not found in the design") + self.get_name(self.base_link) + + def get_name(self, oc: adsk.fusion.Occurrence) -> str: + if oc.entityToken in self.links_by_token: + return self.links_by_token[oc.entityToken] + name = utils.rename_if_duplicate(self.name_map.get(oc.name, oc.name), self.links_by_name) + self.links_by_name[name] = oc + self.links_by_token[oc.entityToken] = name + utils.log(f"DEBUG: link '{oc.name}' ('{oc.fullPathName}') became '{name}'") + return name + + def _get_inertia(self, oc: adsk.fusion.Occurrence): + occs_dict = {} + + prop = oc.getPhysicalProperties(self.inertia_accuracy) + + mass = prop.mass # kg + + # Iterate through bodies, only add mass of bodies that are visible (lightbulb) + body_lst = self.component_map[oc.entityToken].get_flat_body() + + if len(body_lst) > 0: + for body in body_lst: + # Check if this body is hidden + # + # body = oc.bRepBodies.item(i) + if not body.isVisible: + mass -= body.physicalProperties.mass + + occs_dict['mass'] = mass + + center_of_mass = prop.centerOfMass.copy() + + # transform, cm -> m + transform = self.link_origins[self.links_by_token[oc.entityToken]].copy() + assert transform.invert() + c_o_m = center_of_mass.copy() + assert c_o_m.transformBy(transform) + # It is in cm, not in design units. + occs_dict['center_of_mass'] = [c * self.cm for c in c_o_m.asArray()] + + utils.log( + f"DEBUG: {oc.name}: origin={utils.vector_to_str(oc.transform2.translation)}," + f" center_mass(global)={utils.vector_to_str(prop.centerOfMass)}," + f" center_mass(URDF)={occs_dict['center_of_mass']}") + + moments = prop.getXYZMomentsOfInertia() + if not moments[0]: + utils.fatal(f"Retrieving moments of inertia for {oc.name} failed") + + # https://help.autodesk.com/view/fusion360/ENU/?guid=GUID-ce341ee6-4490-11e5-b25b-f8b156d7cd97 + occs_dict['inertia'] = [_ * self.cm * self.cm for _ in transforms.origin2center_of_mass(moments[1:], prop.centerOfMass.asArray(), mass) ] ## kg / cm^2 -> kg/m^2 + + return occs_dict + + def _iterate_through_occurrences(self) -> Iterable[adsk.fusion.Occurrence]: + for token in self.component_map.values(): + yield token.component def _joints(self): ''' Iterates over joints list and defines properties for each joint (along with its relationship) - - ''' - - for joint in self.root.joints: - - joint_dict = {} - joint_type = Configurator.joint_type_list[joint.jointMotion.jointType] - joint_dict['type'] = joint_type - - # switch by the type of the joint - joint_dict['axis'] = [0, 0, 0] - joint_dict['upper_limit'] = 0.0 - joint_dict['lower_limit'] = 0.0 + ''' - occ_one = joint.occurrenceOne - occ_two = joint.occurrenceTwo - - # Check that both bodies are valid (e.g. there is no missing reference) - if not self._is_joint_valid(joint): - # TODO: Handle in a better way (like warning message) + for joint in self.root.allJoints: + if joint.healthState in [adsk.fusion.FeatureHealthStates.SuppressedFeatureHealthState, adsk.fusion.FeatureHealthStates.RolledBackFeatureHealthState]: + continue + try: + if isinstance(joint.jointMotion, adsk.fusion.RevoluteJointMotion): + if joint.jointMotion.rotationValue != 0.0: + utils.log(f"WARNING: joint {joint.name} was not at 0, rotating it to 0") + joint.jointMotion.rotationValue = 0.0 + elif isinstance(joint.jointMotion, adsk.fusion.SliderJointMotion): + if joint.jointMotion.slideValue != 0.0: + utils.log(f"WARNING: joint {joint.name} was not at 0, sliding it to 0") + joint.jointMotion.slideValue = 0.0 + except Exception as e: + try: + o1 = joint.occurrenceOne.fullPathName + except: + o1 = "Unknown" + try: + o2 = joint.occurrenceTwo.fullPathName + except: + o2 = "Unknown" + utils.fatal( + f"Fusion errored out trying to operate on `jointMotion` of joint {joint.name}" + f" (between {o1} and {o2}, child of {joint.parentComponent.name}) with Health State {joint.healthState}: {e}") + + for joint in sorted(self.root.allJoints, key=lambda joint: joint.name): + if joint.healthState in [adsk.fusion.FeatureHealthStates.SuppressedFeatureHealthState, adsk.fusion.FeatureHealthStates.RolledBackFeatureHealthState]: + utils.log(f"Skipping joint {joint.name} (child of {joint.parentComponent.name}) as it is suppressed or rolled back") continue - geom_one_origin = joint.geometryOrOriginOne.origin.asArray() - geom_one_primary = joint.geometryOrOriginOne.primaryAxisVector.asArray() - geom_one_secondary = joint.geometryOrOriginOne.secondaryAxisVector.asArray() - geom_one_third = joint.geometryOrOriginOne.thirdAxisVector.asArray() - - # Check if this is already top level - # Check if the parent_list only contains one entity - parent_list = self.component_map[occ_one.entityToken].get_all_parents() - if len(parent_list) == 1: - pass - # If it is not, get the mapping and trace it back up - else: - # the expectation is there is at least two items, the last is the full body - # the second to last should be the next top-most component - # reset occurrence one - occ_one = self.component_map[parent_list[-2]].component - - parent_list = self.component_map[occ_two.entityToken].get_all_parents() - if len(parent_list) == 1: - pass - else: - # the expectation is there is at least two items, the last is the full body - # the second to last should be the next top-most component - # reset occurrence two - occ_two = self.component_map[parent_list[-2]].component - - geom_two_origin = joint.geometryOrOriginTwo.origin.asArray() - geom_two_primary = joint.geometryOrOriginTwo.primaryAxisVector.asArray() - geom_two_secondary = joint.geometryOrOriginTwo.secondaryAxisVector.asArray() - geom_two_third = joint.geometryOrOriginTwo.thirdAxisVector.asArray() - - joint_type = joint.jointMotion.objectType # string - - # Only Revolute joints have rotation axis - if 'RigidJointMotion' in joint_type: - pass - else: - - joint_vector = joint.jointMotion.rotationAxisVector.asArray() - - joint_rot_val = joint.jointMotion.rotationValue - joint_limit_max = joint.jointMotion.rotationLimits.maximumValue - joint_limit_min = joint.jointMotion.rotationLimits.minimumValue - - if abs(joint_limit_max - joint_limit_min) == 0: - joint_limit_min = -3.14159 - joint_limit_max = 3.14159 - - # joint_angle = joint.angle.value - - joint_dict['axis'] = joint_vector - joint_dict['upper_limit'] = joint_limit_max - joint_dict['lower_limit'] = joint_limit_min - - # Reverses which is parent and child - if self.joint_order == ('p','c'): - joint_dict['parent'] = occ_one.name - joint_dict['child'] = occ_two.name - elif self.joint_order == ('c','p'): - joint_dict['child'] = occ_one.name - joint_dict['parent'] = occ_two.name - else: - raise ValueError(f'Order {self.joint_order} not supported') + if joint.healthState != adsk.fusion.FeatureHealthStates.HealthyFeatureHealthState: + try: + o1 = joint.occurrenceOne.fullPathName + except: + o1 = "Unknown" + try: + o2 = joint.occurrenceTwo.fullPathName + except: + o2 = "Unknown" + utils.fatal(f"Joint {joint.name} (between {o1} and {o2}, child of {joint.parentComponent.name}) is in unexpected Health State {joint.healthState}, {joint.errorOrWarningMessage=}") + + orig_name = joint.name + # Rename if the joint already exists in our dictionary + try: + _ = joint.entityToken # Just making sure it exists + + joint_type = Configurator.joint_types[joint.jointMotion.jointType] + + occ_one = joint.occurrenceOne + occ_two = joint.occurrenceTwo + except RuntimeError as e: + utils.log(f"WARNING: Failed to process joint {joint.name} (child of {joint.parentComponent.name}): {e}, {joint.isValid=}. This is likely a Fusion bug - the joint was likely deleted, but somehow we still see it. Will ignore it.") + continue - joint_dict['xyz'] = [ x/self.scale for x in geom_one_origin] + if occ_one is None or occ_two is None: + utils.log(f"WARNING: Failed to process joint {joint.name} (child of {joint.parentComponent.name}): {joint.isValid=}: occ_one is {None if occ_one is None else occ_one.name}, occ_two is {None if occ_two is None else occ_two.name}") + continue - self.joints_dict[joint.name] = joint_dict + name = utils.rename_if_duplicate(self.name_map.get(joint.name, joint.name), self.joints_dict) - def _build_links(self): - ''' create links ''' + parent = self.get_name(occ_one) + child = self.get_name(occ_two) - mesh_folder = 'meshes/' + if utils.LOG_DEBUG: + utils.log(f"... Processing joint {orig_name}->{name} of type {joint_type}, between {occ_one.name}->{parent} and {occ_two.name}->{child}") - #creates list of bodies that are visible + utils.log(f"DEBUG: Got from Fusion: {joint_type} {name} connecting") + utils.log(f"DEBUG: ... {parent} @ {occ_one.transform2.translation.asArray()} and") + utils.log(f"DEBUG: ... {child} @ {occ_two.transform2.translation.asArray()}") - self.body_dict = defaultdict(list) # key : occurrence name -> value : list of bodies under that occurrence - body_dict_urdf = defaultdict(list) # list to send to parts.py - duplicate_bodies = defaultdict(int) # key : name -> value : # of instances + if joint_type == "fixed": + info = JointInfo(name=name, child=child, parent=parent) - oc_name = '' - # Make sure no repeated body names - body_count = Counter() + else: + try: + geom_one_origin = get_origin(joint.geometryOrOriginOne) + except RuntimeError: + geom_one_origin = None + try: + geom_two_origin = get_origin(joint.geometryOrOriginTwo) + except RuntimeError: + geom_two_origin = None + + utils.log(f"DEBUG: ... Origin 1: {utils.vector_to_str(geom_one_origin) if geom_one_origin is not None else None}") + utils.log(f"DEBUG: ... Origin 2: {utils.vector_to_str(geom_two_origin) if geom_two_origin is not None else None}") + + if occ_one.assemblyContext != occ_two.assemblyContext: + utils.log(f"DEBUG: Non-fixed joint {name} crosses the assembly context boundary:" + f" {parent} is in {get_context_name(occ_one.assemblyContext)}" + f" but {child} is in {get_context_name(occ_two.assemblyContext)}") + + if geom_one_origin is None: + utils.fatal(f'Non-fixed joint {orig_name} does not have an origin, aborting') + elif geom_two_origin is not None and not self.close_enough(geom_two_origin, geom_one_origin): + utils.log(f'WARNING: Occurrences {occ_one.name} and {occ_two.name} of non-fixed {orig_name}' + + f' have origins {geom_one_origin.asArray()} and {geom_two_origin.asArray()}' + f' that do not coincide.') + + # Only Revolute joints have rotation axis + if isinstance(joint.jointMotion, adsk.fusion.RevoluteJointMotion): + assert joint.jointMotion.rotationLimits.isMaximumValueEnabled + assert joint.jointMotion.rotationLimits.isMinimumValueEnabled + joint_vector = joint.jointMotion.rotationAxisVector + # The values are in radians per + # https://help.autodesk.com/view/fusion360/ENU/?guid=GUID-e3fb19a1-d7ef-4b34-a6f5-76a907d6a774 + joint_limit_max = joint.jointMotion.rotationLimits.maximumValue + joint_limit_min = joint.jointMotion.rotationLimits.minimumValue + + if abs(joint_limit_max - joint_limit_min) == 0: + # Rotation is unlimited + joint_limit_min = -3.14159 + joint_limit_max = 3.14159 + elif isinstance(joint.jointMotion, adsk.fusion.SliderJointMotion): + assert joint.jointMotion.slideLimits.isMaximumValueEnabled + assert joint.jointMotion.slideLimits.isMinimumValueEnabled + joint_vector=joint.jointMotion.slideDirectionVector + + # The values are in cm per + # https://help.autodesk.com/view/fusion360/ENU/?guid=GUID-e3fb19a1-d7ef-4b34-a6f5-76a907d6a774 + joint_limit_max = joint.jointMotion.slideLimits.maximumValue * self.cm + joint_limit_min = joint.jointMotion.slideLimits.minimumValue * self.cm + else: + # Keep default limits for 'RigidJointMotion' or others + joint_vector = adsk.core.Vector3D.create() + joint_limit_max = 0.0 + joint_limit_min = 0.0 + + info = JointInfo( + name=name, child=child, parent=parent, origin=geom_one_origin, type=joint_type, + axis=joint_vector, upper_limit=joint_limit_max, lower_limit=joint_limit_min) + + self.joints_dict[name] = info + + # Add RigidGroups as fixed joints + for group in sorted(self.root.allRigidGroups, key=lambda group: group.name): + original_group_name = group.name + try: + if group.isSuppressed: + utils.log(f"WARNING: Skipping suppressed rigid group {original_group_name} (child of {group.parentComponent.name})") + continue + if not group.isValid: + utils.log(f"WARNING: skipping invalid rigid group {original_group_name} (child of {group.parentComponent.name})") + continue + except RuntimeError as e: + utils.log(f"WARNING: skipping invalid rigid group {original_group_name}: (child of {group.parentComponent.name}) {e}") + continue + utils.log(f"DEBUG: Processing rigid group {original_group_name}: {[(occ.name if occ else None) for occ in group.occurrences]}") + parent_occ: Optional[adsk.fusion.Occurrence] = None + for occ in group.occurrences: + if occ is None: + continue + elif parent_occ is None: + # Assumes that the first occurrence will be the parent + parent_occ = occ + continue + rigid_group_occ_name = utils.rename_if_duplicate(original_group_name, self.joints_dict) + + parent_occ_name = self.get_name(parent_occ) # type: ignore[undef] + occ_name = self.get_name(occ) + utils.log( + f"DEBUG: Got from Fusion: {rigid_group_occ_name}, connecting", + f"parent {parent_occ_name} @ {utils.vector_to_str(parent_occ.transform2.translation)} and" # type: ignore[undef] + f"child {occ_name} {utils.vector_to_str(occ.transform2.translation)}") + self.joints_dict[rigid_group_occ_name] = JointInfo(name=rigid_group_occ_name, parent=parent_occ_name, child=occ_name) - for oc in self.occ: - oc_name = oc.name.replace(':','_').replace(' ','') - # self.body_dict[oc_name] = [] - # body_lst = self.component_map[oc.entityToken].get_flat_body() #gets list of all bodies in the occurrence + self.assembly_tokens: Set[str] = set() + for occ in self.root.allOccurrences: + if occ.childOccurrences.count > 0: + # It's an assembly + self.assembly_tokens.add(occ.entityToken) + + def get_assembly_links(self, occ: adsk.fusion.Occurrence, parent_included: bool) -> List[str]: + result: List[str] = [] + for child in occ.childOccurrences: + child_included = child.entityToken in self.links_by_token + if child_included: + result.append(self.links_by_token[child.entityToken]) + child_included = parent_included or child_included + if child.entityToken in self.assembly_tokens: + result += self.get_assembly_links(child, child_included) + elif not child_included: + result.append(self.get_name(child)) + utils.log(f"DEBUG: get_assembly_links({occ.name}) = {result}") + return result + + @staticmethod + def _mk_pattern(name: str) -> Union[str, Tuple[str,str]]: + c = name.count("*") + if c > 1: + utils.fatal(f"Occurrance name pattern '{name}' is invalid: only one '*' is supported") + if c: + pref, suff = name.split("*", 1) + return (pref, suff) + return name + + @staticmethod + def _match(candidate: str, pattern: Union[str, Tuple[str,str]]) -> bool: + if isinstance(pattern, str): + return candidate == pattern + pref, suff = pattern + return len(candidate) >= len(pref) + len(suff) and candidate.startswith(pref) and candidate.endswith(suff) + + def _resolve_name(self, name:str) -> adsk.fusion.Occurrence: + if "+" in name: + name_parts = name.split("+") + l = len(name_parts) + patts = [self._mk_pattern(p) for p in name_parts] + candidate: Optional[adsk.fusion.Occurrence]= None + for occ in self._iterate_through_occurrences(): + path = occ.fullPathName.split("+") + if len(path) < l: + continue + mismatch = False + for (cand, patt) in zip(path[-l:], patts): + if not self._match(cand, patt): + mismatch = True + break + if mismatch: + continue + if candidate is None: + candidate = occ + else: + utils.fatal(f"Name/pattern '{name}' in configuration file matches at least two occurrences: '{candidate.fullPathName}' and '{occ.fullPathName}', update to be more specific") + if not candidate: + utils.fatal(f"Name/pattern '{name}' in configuration file does not match any occurrences") + return candidate + patt = self._mk_pattern(name) + candidates = [occ for occ in self._iterate_through_occurrences() if self._match(occ.name, patt)] + if not candidates: + utils.fatal(f"Name/pattern '{name}' in configuration file does not match any occurrences") + if len(candidates) > 1: + utils.fatal(f"Name/pattern '{name}' in configuration file matches at least two occurrences: '{candidates[0].fullPathName}' and '{candidates[1].fullPathName}', update to be more specific") + return candidates[0] + + def _links(self): + self.merged_links_by_link: Dict[str, Tuple[str, List[str], List[adsk.fusion.Occurrence]]] = OrderedDict() + self.merged_links_by_name: Dict[str, Tuple[str, List[str], List[adsk.fusion.Occurrence]]] = OrderedDict() + + for name, names in self.merge_links.items(): + if not names: + utils.fatal(f"Invalid MergeLinks YAML config setting: merged link '{name}' is empty, which is not allowed") + link_names = [] + for n in names: + occ = self._resolve_name(n) + if occ.entityToken in self.links_by_token or occ.entityToken in self.assembly_tokens: + link_names.append(self.get_name(occ)) + if occ.entityToken in self.assembly_tokens: + try: + link_names += self.get_assembly_links(occ, occ.entityToken in self.links_by_token) + except ValueError as e: + utils.fatal(f"Invalid MergeLinks YAML config setting: assembly '{n}' for merged link '{name}' could not be processed: {e.args[0]}") + if name in self.links_by_name and name not in names: + utils.fatal(f"Invalid MergeLinks YAML config setting: merged '{name}' clashes with existing Fusion link '{self.links_by_name[name].fullPathName}'; add the latter to NameMap in YAML to avoid the name clash") + link_names = list(OrderedDict.fromkeys(link_names)) # Remove duplicates + val = name, link_names, [self.links_by_name[n] for n in link_names] + utils.log(f"Merged link {name} <- occurrences {link_names}") + self.merged_links_by_name[name] = val + for link_name in link_names: + if link_name in self.merged_links_by_link: + utils.fatal(f"Invalid MergeLinks YAML config setting: {link_name} is included in two merged links: '{name}' and '{self.merged_links_by_link[link_name][0]}'") + self.merged_links_by_link[link_name] = val + + body_names: Dict[str, Tuple[()]] = OrderedDict() + + renames = set(self.name_map) - body_lst = self.body_mapper[oc.entityToken] - - if len(body_lst) > 0: - for body in body_lst: - # Check if this body is hidden - if body.isLightBulbOn: - if body.name in duplicate_bodies: - duplicate_bodies[body.name] +=1 - self.body_dict[oc_name].append(body) + for oc in self._iterate_through_occurrences(): + renames.difference_update([oc.name]) + occ_name, _, occs = self._get_merge(oc) + if occ_name in self.body_dict: + continue - body_name = body.name.replace(':','_').replace(' ','') - body_name_cnt = f'{body_name}_{body_count[body_name]}' - body_count[body_name] += 1 + oc_name = utils.format_name(occ_name) + self.body_dict[occ_name] = [] + bodies = set() - unique_bodyname = f'{oc_name}_{body_name_cnt}' - body_dict_urdf[oc_name].append(unique_bodyname) + for sub_oc in occs: + sub_oc_name = utils.format_name(self.get_name(sub_oc)) + if sub_oc_name != oc_name: + sub_oc_name = f"{oc_name}__{sub_oc_name}" + for body in self.body_mapper[sub_oc.entityToken]: + # Check if this body is hidden + if body.isVisible and body.entityToken not in bodies: + body_name = f"{sub_oc_name}__{utils.format_name(body.name)}" + unique_bodyname = utils.rename_if_duplicate(body_name, body_names) + body_names[unique_bodyname] = () + self.body_dict[occ_name].append((body, unique_bodyname)) + bodies.add(body.entityToken) + + if renames: + ValueError("Invalid NameMap YAML config setting: some of the links are not in Fusion: '" + "', '".join(renames) + "'") + + def __add_link(self, name: str, occs: List[adsk.fusion.Occurrence]): + urdf_origin = self.link_origins[name] + inv = urdf_origin.copy() + assert inv.invert() + #fusion_origin = occ.transform2.translation.asArray() + + mass = 0.0 + visible = False + center_of_mass = np.zeros(3) + inertia_tensor = np.zeros(6) + for occ in occs: + inertia = self._get_inertia(occ) + utils.log(f"DEBUG: link {occ.name} urdf_origin at {utils.vector_to_str(urdf_origin.translation)}" + f" (rpy={utils.rpy_to_str(transforms.so3_to_euler(urdf_origin))})" + f" and inv at {utils.vector_to_str(inv.translation)} (rpy={utils.rpy_to_str(transforms.so3_to_euler(inv))})") + mass += inertia['mass'] + visible += visible or occ.isVisible + center_of_mass += np.array(inertia['center_of_mass']) * inertia['mass'] + inertia_tensor += np.array(inertia['inertia']) + if len(occs) == 1: + inertia_tensor = inertia['inertia'] # type: ignore + center_of_mass = inertia['center_of_mass'] # type: ignore + else: + inertia_tensor = list(inertia_tensor) + center_of_mass = list(center_of_mass/mass) + + self.bodies_collected.update(body.entityToken for body, _ in self.body_dict[name]) + + self.links[name] = parts.Link(name = utils.format_name(name), + xyz = (u * self.cm for u in inv.translation.asArray()), + rpy = transforms.so3_to_euler(inv), + center_of_mass = center_of_mass, + sub_folder = self.mesh_folder, + mass = mass, + inertia_tensor = inertia_tensor, + bodies = [body_name for _, body_name in self.body_dict[name]], + sub_mesh = self.sub_mesh, + material_dict = self.material_dict, + visible = visible) + + def __get_material(self, appearance: Optional[adsk.core.Appearance]) -> str: + # Material should always have an appearance, but just in case + if appearance is not None: + # Only supports one appearance per occurrence so return the first + for prop in appearance.appearanceProperties: + if type(prop) == adsk.core.ColorProperty: + prop_name = appearance.name + color_name = utils.convert_german(prop_name) + color_name = utils.format_name(color_name) + self.color_dict[color_name] = f"{prop.value.red/255} {prop.value.green/255} {prop.value.blue/255} {prop.value.opacity/255}" + return color_name + return "silver_default" + + def _materials(self) -> None: + # Adapted from SpaceMaster85/fusion2urdf + self.color_dict['silver_default'] = "0.700 0.700 0.700 1.000" + + if self.sub_mesh: + for occ_name, bodies in self.body_dict.items(): + if len(bodies) > 1: + for body, body_name in bodies: + self.material_dict[body_name] = self.__get_material(body.appearance) + else: + appearance = self.__get_material(bodies[0][0].appearance) if bodies else 'silver_default' + self.material_dict[utils.format_name(occ_name)] = appearance + else: + for occ in self.links_by_name.values(): + occ_name, _, occs = self._get_merge(occ) + occ = occs[0] + # Prioritize appearance properties, but it could be null + appearance = None + if occ.appearance: + appearance = occ.appearance + elif occ.bRepBodies: + for body in occ.bRepBodies: + if body.appearance: + appearance = body.appearance + break + elif occ.component.material: + appearance = occ.component.material.appearance + self.material_dict[utils.format_name(occ_name)] = self.__get_material(appearance) + + def _get_merge(self, occ: adsk.fusion.Occurrence) -> Tuple[str, List[str], List[adsk.fusion.Occurrence]]: + name = self.get_name(occ) + if name in self.merged_links_by_link: + return self.merged_links_by_link[name] + return name, [name], [occ] + + def _build(self) -> None: + ''' create links and joints by setting parent and child relationships and constructing + the XML formats to be exported later''' + + # Location and XYZ of the URDF link origin w.r.t Fusion global frame in Fusion units + self.link_origins: Dict[str, adsk.core.Matrix3D] = {} + + occurrences: Dict[str, List[str]] = OrderedDict() + for joint_name, joint_info in self.joints_dict.items(): + occurrences.setdefault(joint_info.parent, []) + occurrences.setdefault(joint_info.child, []) + occurrences[joint_info.parent].append(joint_name) + occurrences[joint_info.child].append(joint_name) + for link_name, joints in occurrences.items(): + utils.log(f"DEBUG: {link_name} touches joints {joints}") + assert self.base_link is not None + self.base_link_name, base_link_names, base_link_occs = self._get_merge(self.base_link) + grounded_occ = set(base_link_names) + for name in [self.base_link_name] + base_link_names: + # URDF origin at base link origin "by definition" + self.link_origins[name] = base_link_occs[0].transform2 + self.__add_link(self.base_link_name, base_link_occs) + boundary = grounded_occ.copy() + fixed_links: Dict[Tuple[str,str], str] = {} + while boundary: + new_boundary : Set[str] = set() + for occ_name in boundary: + for joint_name in occurrences.get(occ_name, ()): + joint = self.joints_dict[joint_name] + if joint.parent == occ_name: + child_name = joint.child + if child_name in grounded_occ: + continue + flip_axis = True + else: + assert joint.child == occ_name + if joint.parent in grounded_occ: + continue + # Parent is further away from base_link than the child, swap them + child_name = joint.parent + flip_axis = False + + + parent_name, _, _ = self._get_merge(self.links_by_name[occ_name]) + child_name, child_link_names, child_link_occs = self._get_merge(self.links_by_name[child_name]) + + child_origin = child_link_occs[0].transform2 + parent_origin = self.link_origins[parent_name] + + if utils.LOG_DEBUG and self.close_enough(parent_origin.getAsCoordinateSystem()[1:], adsk.core.Matrix3D.create().getAsCoordinateSystem()[1:]) and not self.close_enough(child_origin.getAsCoordinateSystem()[1:], adsk.core.Matrix3D.create().getAsCoordinateSystem()[1:]): + utils.log(f"***** !!!!! rotating off the global frame's orientation") + utils.log(f" Child axis: {[v.asArray() for v in child_origin.getAsCoordinateSystem()[1:]]}") + + t = parent_origin.copy() + assert t.invert() + + axis = joint.axis - # Make the actual urdf names accessible - self.body_dict_urdf = body_dict_urdf - - - base_link = self.base_links.pop() - center_of_mass = self.inertial_dict[base_link]['center_of_mass'] - link = parts.Link(name=base_link, - xyz=[0,0,0], - center_of_mass=center_of_mass, - sub_folder=mesh_folder, - mass=self.inertial_dict[base_link]['mass'], - inertia_tensor=self.inertial_dict[base_link]['inertia'], - body_dict = body_dict_urdf, - sub_mesh = self.sub_mesh) - - self.links_xyz_dict[link.name] = link.xyz - self.links[link.name] = link - - for k, joint in self.joints_dict.items(): - name = joint['child'] - - center_of_mass = [ i-j for i, j in zip(self.inertial_dict[name]['center_of_mass'], joint['xyz'])] - link = parts.Link(name=name, - xyz=(joint['xyz'][0], joint['xyz'][1], joint['xyz'][2]), - center_of_mass=center_of_mass, - sub_folder=mesh_folder, - mass=self.inertial_dict[name]['mass'], - inertia_tensor=self.inertial_dict[name]['inertia'], - body_dict = body_dict_urdf, - sub_mesh = self.sub_mesh) - - self.links_xyz_dict[link.name] = (link.xyz[0], link.xyz[1], link.xyz[2]) - - self.links[link.name] = link - - - - def _build_joints(self): - ''' create joints by setting parent and child relationships and constructing - the XML formats to be exported later ''' - - for k, j in self.joints_dict.items(): + if joint.type == "fixed": + fixed_links[(child_name, parent_name)] = joint.name + fixed_links[(parent_name, child_name)] = joint.name + else: + utils.log(f"DEBUG: for non-fixed joint {joint.name}, updating child origin from {utils.ct_to_str(child_origin)} to {joint.origin.asArray()}") + child_origin = child_origin.copy() + child_origin.translation = joint.origin + # The joint axis is specified in the joint (==child) frame + tt = child_origin.copy() + tt.translation = adsk.core.Vector3D.create() + assert tt.invert() + axis = axis.copy() + assert axis.transformBy(tt) + if flip_axis: + assert axis.scaleBy(-1) + utils.log(f"DEBUG: and using {utils.ct_to_str(tt)} and {flip_axis=} to update axis from {joint.axis.asArray()} to {axis.asArray()}") + + for name in [child_name] + child_link_names: + self.link_origins[name] = child_origin + + ct = child_origin.copy() + assert ct.transformBy(t) + + xyz = [c * self.cm for c in ct.translation.asArray()] + rpy = transforms.so3_to_euler(ct) + + utils.log( + f"DEBUG: joint {joint.name} (type {joint.type})" + f" from {parent_name} at {utils.vector_to_str(parent_origin.translation)}" + f" to {child_name} at {utils.vector_to_str(child_origin.translation)}" + f" -> xyz={utils.vector_to_str(xyz,5)} rpy={utils.rpy_to_str(rpy)}") + + self.joints[joint.name] = parts.Joint(name=joint.name , joint_type=joint.type, + xyz=xyz, rpy=rpy, axis=axis.asArray(), + parent=parent_name, child=child_name, + upper_limit=joint.upper_limit, lower_limit=joint.lower_limit) + + self.__add_link(child_name, child_link_occs) + new_boundary.update(child_link_names) + grounded_occ.update(child_link_names) - xyz = [] - for p,c in zip(self.links_xyz_dict[j['parent']], self.links_xyz_dict[j['child']]): - xyz.append(p-c) + boundary = new_boundary + + disconnected_external = [] + for name in self.extra_links: + if name in self.merge_links: + name2, _, occs = self.merged_links_by_name[name] + if name2 == self.base_link_name: + utils.fatal(f"Link '{name2}' is the root link, but declared as an extra (that is, not a part of the main URDF)") + for oc in occs: + self.link_origins[self.get_name(oc)] = occs[0].transform2 + else: + if name == self.base_link_name: + utils.fatal(f"Link '{name}' is the root link, but declared as an extra (that is, not a part of the main URDF)") + elif name not in self.links_by_name: + utils.fatal(f"Link '{name}' from the 'Extras:' section of the configuration file is not known") + occs = [self.links_by_name[name]] + self.link_origins[name] = occs[0].transform2 + self.__add_link(name, occs) + disconnected_external.append(name) - joint = parts.Joint(name=k , joint_type=j['type'], - xyz=xyz, axis=j['axis'], - parent=j['parent'], child=j['child'], - upper_limit=j['upper_limit'], lower_limit=j['lower_limit']) - self.joints[k] = joint + joint_children: Dict[str, List[parts.Joint]] = OrderedDict() + for joint in self.joints.values(): + joint_children.setdefault(joint.parent, []) + joint_children[joint.parent].append(joint) + tree_str = [] + def get_tree(level: int, link_name: str, exclude: Set[str]): + extra = f" {self.merge_links[link_name]}" if link_name in self.merge_links else "" + tree_str.append(" "*level + f" - Link: {link_name}{extra}") + for j in joint_children.get(link_name, ()): + if j.child not in exclude: + tree_str.append(" " * (level + 1) + f" - Joint [{j.type}]: {j.name}") + get_tree(level+2, j.child, exclude) + get_tree(1, self.base_link_name, set(disconnected_external)) + if disconnected_external: + tree_str.append(" - \"Extras\" links:") + for extra in disconnected_external: + get_tree(2, extra, set(self.link_origins).difference(disconnected_external)) + + # Sanity checks + not_in_joints = set() + unreachable = set() + for occ in self._iterate_through_occurrences(): + if occ.isVisible and self.body_dict.get(self.name) is not None: + if occ.fullPathName not in self.links_by_token: + not_in_joints.add(occ.fullPathName) + elif self.links_by_token[occ.fullPathName] not in grounded_occ: + unreachable.add(occ.fullPathName) + for occ in self.root.allOccurrences: + if any (b.isVisible and not b.entityToken in self.bodies_collected for b in occ.bRepBodies): + unreachable.add(occ.fullPathName) + if not_in_joints or unreachable: + error = "FATAL ERROR: Not all occurrences were included in the export:" + if not_in_joints: + error += "Not a part of any joint or rigid group: " + ", ".join(not_in_joints) + "." + if unreachable: + error += "Unreacheable from the grounded occurrence via joints+links: " + ", ".join(unreachable) + "." + utils.log(error) + missing_joints = set(self.joints_dict).difference(self.joints) + for joint_name in missing_joints.copy(): + joint = self.joints_dict[joint_name] + if joint.type == "fixed": + parent_name, _, _ = self._get_merge(self.links_by_name[joint.parent]) + child_name, _, _ = self._get_merge(self.links_by_name[joint.child]) + if parent_name == child_name: + utils.log(f"DEBUG: Skipped Fixed Joint '{joint_name}' that is internal for merged link {self.merged_links_by_link[joint.parent][0]}") + missing_joints.remove(joint_name) + elif (parent_name, child_name) in fixed_links: + utils.log(f"DEBUG: Skipped Fixed Joint '{joint_name}' that is duplicative of `{fixed_links[(parent_name, child_name)]}") + missing_joints.remove(joint_name) + if missing_joints: + utils.log("\n\t".join(["FATAL ERROR: Lost joints: "] + [f"{self.joints_dict[joint].name} of type {self.joints_dict[joint].type} between {self.joints_dict[joint].parent} and {self.joints_dict[joint].child}" for joint in missing_joints])) + extra_joints = set(self.joints).difference(self.joints_dict) + if extra_joints: + utils.log("FATAL ERROR: Extra joints: '" + "', '".join(sorted(extra_joints)) + "'") + if not_in_joints or unreachable or missing_joints or extra_joints: + utils.log("Reachable from the root:") + utils.log("\n".join(tree_str)) + utils.fatal("Fusion structure is broken or misunderstoon by the exporter, giving up! See the full output in Text Commands console for more information.") + self.tree_str = tree_str + + for link, locations in self.locations.items(): + if link not in self.link_origins: + utils.fatal(f"Link {link} specified in the config file 'Locations:' section does not exist. Make sure to use the URDF name (e.g. as set via MergeLink) rather than the Fusion one") + origin = self.link_origins[link] + t = origin.copy() + assert t.invert() + self.locs[link] = [] + for loc_name, loc_occurrence in locations.items(): + if loc_occurrence in self.merge_links: + ct = self.link_origins[loc_occurrence].copy() + else: + ct = self._resolve_name(loc_occurrence).transform2.copy() + assert ct.transformBy(t) + self.locs[link].append(parts.Location(loc_name, [c * self.cm for c in ct.translation.asArray()], rpy = transforms.so3_to_euler(ct))) diff --git a/Descriptor/core/parts.py b/Descriptor/core/parts.py index 0cc073c..1abbb39 100644 --- a/Descriptor/core/parts.py +++ b/Descriptor/core/parts.py @@ -7,17 +7,41 @@ Modified by cadop Dec 19 2021 """ +import math +from typing import List, Optional, Sequence from xml.etree.ElementTree import Element, SubElement from xml.etree import ElementTree from xml.dom import minidom +from . import utils -class Joint: +MAX_ROUND_TO_ZERO = 5e-11 # in mm, radians +HALF_PI = math.pi * 0.5 +def _round(val: float, unit: float) -> float: + units = val/unit + r = round(units) * unit + if abs(val - r) <= MAX_ROUND_TO_ZERO: + return r + return val - # Defaults for all joints - effort_limit = 100 - vel_limit = 100 +def round_mm(val:float) -> float: + return _round(val, 0.0001) - def __init__(self, name, xyz, axis, parent, child, joint_type, upper_limit, lower_limit): +def round_rads(val:float) -> float: + return _round(val, HALF_PI) + +class Location: + def __init__(self, name: str, xyz: Sequence[float], rpy: Sequence[float]): + self.name = name + self.xyz = [round_mm(x) for x in xyz] + self.rpy = [round_rads(r) for r in rpy] + +class Joint(Location): + + # Defaults for all joints. Need be be floats, not ints + effort_limit = 100.0 + vel_limit = 100.0 + + def __init__(self, name: str, xyz: Sequence[float], rpy: Sequence[float], axis: Sequence[float], parent: str, child:str, joint_type: str, upper_limit: float, lower_limit: float): """ Attributes ---------- @@ -38,35 +62,33 @@ def __init__(self, name, xyz, axis, parent, child, joint_type, upper_limit, lowe tran_xml: str generated xml describing about the transmission """ - self.name = name + super().__init__(name, xyz, rpy) self.type = joint_type - self.xyz = xyz self.parent = parent self.child = child self._joint_xml = None self._tran_xml = None - self.axis = axis # for 'revolute' and 'continuous' + self.axis = [round_mm(a) for a in axis] # for 'revolute' and 'continuous' self.upper_limit = upper_limit # for 'revolute' and 'prismatic' self.lower_limit = lower_limit # for 'revolute' and 'prismatic' - - @property - def joint_xml(self): + + def joint_xml(self) -> Element: """ - Generate the joint_xml and hold it by self.joint_xml + Generate the joint xml """ joint = Element('joint') - joint.attrib = {'name':self.name.replace(':','_').replace(' ',''), 'type':self.type} + joint.attrib = {'name':utils.format_name(self.name), 'type':self.type} origin = SubElement(joint, 'origin') - origin.attrib = {'xyz':' '.join([str(_) for _ in self.xyz]), 'rpy':'0 0 0'} + origin.attrib = {'xyz':' '.join([str(_) for _ in self.xyz]), 'rpy':' '.join([str(_) for _ in self.rpy])} parent = SubElement(joint, 'parent') - self.parent = self.parent.replace(':','_').replace(' ','') + self.parent = utils.format_name(self.parent) parent.attrib = {'link':self.parent} child = SubElement(joint, 'child') - self.child = self.child.replace(':','_').replace(' ','') + self.child = utils.format_name(self.child) child.attrib = {'link':self.child} if self.type == 'revolute' or self.type == 'continuous' or self.type == 'prismatic': @@ -77,14 +99,9 @@ def joint_xml(self): limit.attrib = {'upper': str(self.upper_limit), 'lower': str(self.lower_limit), 'effort': f'{Joint.effort_limit}', 'velocity': f'{Joint.vel_limit}'} - rough_string = ElementTree.tostring(joint, 'utf-8') - reparsed = minidom.parseString(rough_string) - self._joint_xml = "\n".join(reparsed.toprettyxml(indent=" ").split("\n")[1:]) + return joint - return self._joint_xml - - @property - def transmission_xml(self): + def transmission_xml(self) -> Element: """ Generate the tran_xml and hold it by self.tran_xml @@ -97,34 +114,30 @@ def transmission_xml(self): """ tran = Element('transmission') - tran.attrib = {'name':self.name.replace(':','_').replace(' ','') + '_tran'} + tran.attrib = {'name':utils.format_name(self.name) + '_tran'} joint_type = SubElement(tran, 'type') joint_type.text = 'transmission_interface/SimpleTransmission' joint = SubElement(tran, 'joint') - joint.attrib = {'name':self.name.replace(':','_').replace(' ','')} + joint.attrib = {'name':utils.format_name(self.name)} hardwareInterface_joint = SubElement(joint, 'hardwareInterface') hardwareInterface_joint.text = 'hardware_interface/EffortJointInterface' actuator = SubElement(tran, 'actuator') - actuator.attrib = {'name':self.name.replace(':','_').replace(' ','') + '_actr'} + actuator.attrib = {'name':utils.format_name(self.name) + '_actr'} hardwareInterface_actr = SubElement(actuator, 'hardwareInterface') hardwareInterface_actr.text = 'hardware_interface/EffortJointInterface' mechanicalReduction = SubElement(actuator, 'mechanicalReduction') mechanicalReduction.text = '1' - rough_string = ElementTree.tostring(tran, 'utf-8') - reparsed = minidom.parseString(rough_string) - self._tran_xml = "\n".join(reparsed.toprettyxml(indent=" ").split("\n")[1:]) - - return self._tran_xml + return tran -class Link: +class Link (Location): - mesh_scale = '0.001' + scale = '0.001' - def __init__(self, name, xyz, center_of_mass, sub_folder, mass, inertia_tensor, body_dict, sub_mesh): + def __init__(self, name, xyz, rpy, center_of_mass, sub_folder, mass, inertia_tensor, bodies, sub_mesh, material_dict, visible): """ Parameters ---------- @@ -142,78 +155,74 @@ def __init__(self, name, xyz, center_of_mass, sub_folder, mass, inertia_tensor, mass of the link inertia_tensor: [ixx, iyy, izz, ixy, iyz, ixz] tensor of the inertia - body_lst = [body1, body2, body3] + bodies = [body1, body2, body3] list of visible bodies - body_dict = {body.entityToken: name of occurrence} - dictionary of body entity tokens to the occurrence name """ - self.name = name - # xyz for visual - self.xyz = [-(_) for _ in xyz] # reverse the sign of xyz + super().__init__(name, xyz, rpy) # xyz for center of mass - self.center_of_mass = [x for x in center_of_mass] + self.center_of_mass = [round_mm(x) for x in center_of_mass] self._link_xml = None self.sub_folder = sub_folder self.mass = mass self.inertia_tensor = inertia_tensor - self.body_dict = body_dict + self.bodies = bodies self.sub_mesh = sub_mesh # if we want to export each body as a separate mesh + self.material_dict = material_dict + self.visible = visible - @property - def link_xml(self): + def link_xml(self) -> Optional[Element]: """ Generate the link_xml and hold it by self.link_xml """ - link = Element('link') - self.name = self.name.replace(':','_').replace(' ','') link.attrib = {'name':self.name} - + rpy = ' '.join([str(_) for _ in self.rpy]) + scale = ' '.join([self.scale]*3) + #inertial inertial = SubElement(link, 'inertial') origin_i = SubElement(inertial, 'origin') - origin_i.attrib = {'xyz':' '.join([str(_) for _ in self.center_of_mass]), 'rpy':'0 0 0'} + origin_i.attrib = {'xyz':' '.join([str(_) for _ in self.center_of_mass]), 'rpy':rpy} mass = SubElement(inertial, 'mass') mass.attrib = {'value':str(self.mass)} inertia = SubElement(inertial, 'inertia') inertia.attrib = {'ixx':str(self.inertia_tensor[0]), 'iyy':str(self.inertia_tensor[1]), - 'izz':str(self.inertia_tensor[2]), 'ixy':str(self.inertia_tensor[3]), - 'iyz':str(self.inertia_tensor[4]), 'ixz':str(self.inertia_tensor[5])} + 'izz':str(self.inertia_tensor[2]), 'ixy':str(self.inertia_tensor[3]), + 'iyz':str(self.inertia_tensor[4]), 'ixz':str(self.inertia_tensor[5])} + if not self.visible: + return link + # visual - if self.sub_mesh: # if we want to export each as a separate mesh - for body_name in self.body_dict[self.name]: - # body_name = body_name.replace(':','_').replace(' ','') + if self.sub_mesh and len(self.bodies) > 1: # if we want to export each as a separate mesh + for body_name in self.bodies: visual = SubElement(link, 'visual') origin_v = SubElement(visual, 'origin') - origin_v.attrib = {'xyz':' '.join([str(_) for _ in self.xyz]), 'rpy':'0 0 0'} + origin_v.attrib = {'xyz':' '.join([str(_) for _ in self.xyz]), 'rpy':rpy} geometry_v = SubElement(visual, 'geometry') mesh_v = SubElement(geometry_v, 'mesh') - mesh_v.attrib = {'filename':f'package://{self.sub_folder}{body_name}.stl','scale':f'{Link.mesh_scale} {Link.mesh_scale} {Link.mesh_scale}'} + mesh_v.attrib = {'filename':f'package://{self.sub_folder}{body_name}.stl','scale':scale} material = SubElement(visual, 'material') - material.attrib = {'name':'silver'} + material.attrib = {'name': self.material_dict[body_name]} else: visual = SubElement(link, 'visual') origin_v = SubElement(visual, 'origin') - origin_v.attrib = {'xyz':' '.join([str(_) for _ in self.xyz]), 'rpy':'0 0 0'} + origin_v.attrib = {'xyz':' '.join([str(_) for _ in self.xyz]), 'rpy':rpy} geometry_v = SubElement(visual, 'geometry') mesh_v = SubElement(geometry_v, 'mesh') - mesh_v.attrib = {'filename':f'package://{self.sub_folder}{self.name}.stl','scale':f'{Link.mesh_scale} {Link.mesh_scale} {Link.mesh_scale}'} + mesh_v.attrib = {'filename':f'package://{self.sub_folder}{self.name}.stl','scale':scale} material = SubElement(visual, 'material') - material.attrib = {'name':'silver'} + material.attrib = {'name': self.material_dict[self.name]} # collision collision = SubElement(link, 'collision') origin_c = SubElement(collision, 'origin') - origin_c.attrib = {'xyz':' '.join([str(_) for _ in self.xyz]), 'rpy':'0 0 0'} + origin_c.attrib = {'xyz':' '.join([str(_) for _ in self.xyz]), 'rpy':rpy} geometry_c = SubElement(collision, 'geometry') mesh_c = SubElement(geometry_c, 'mesh') - mesh_c.attrib = {'filename':'package://' + self.sub_folder + self.name + '.stl','scale':'0.001 0.001 0.001'} + mesh_c.attrib = {'filename':f'package://{self.sub_folder}{self.name}.stl','scale':scale} - rough_string = ElementTree.tostring(link, 'utf-8') - reparsed = minidom.parseString(rough_string) - self._link_xml = "\n".join(reparsed.toprettyxml(indent=" ").split("\n")[1:]) - return self._link_xml + return link \ No newline at end of file diff --git a/Descriptor/core/transforms.py b/Descriptor/core/transforms.py index b8ab5f0..fdf1aa2 100644 --- a/Descriptor/core/transforms.py +++ b/Descriptor/core/transforms.py @@ -1,4 +1,27 @@ +from typing import Tuple +import adsk.core +import numpy as np +from scipy.spatial.transform import Rotation + +def so3_to_euler(mat: adsk.core.Matrix3D) -> Tuple[float, float, float]: + """Converts an SO3 rotation matrix to Euler angles + + Args: + so3: Matrix3D coordinate transform + + Returns: + tuple of Euler angles (size 3) + + """ + so3 = np.zeros((3,3)) + for i in range(3): + for j in range(3): + so3[i,j] = mat.getCell(i,j) + ### first transform the matrix to euler angles + r = Rotation.from_matrix(so3) + yaw, pitch, roll = r.as_euler("ZYX", degrees=False) + return (float(roll), float(pitch), float(yaw)) def origin2center_of_mass(inertia, center_of_mass, mass): """ diff --git a/Descriptor/core/ui.py b/Descriptor/core/ui.py index d6ea8e5..b4de68c 100644 --- a/Descriptor/core/ui.py +++ b/Descriptor/core/ui.py @@ -1,12 +1,32 @@ ''' module: user interface''' -import adsk +from typing import Optional import adsk.core, adsk.fusion, traceback +try: + from yaml import SafeLoader # Test whether it's there + _ = SafeLoader # Noop for import to not be unused + from scipy.spatial.transform import Rotation # Test whether it's there + _ = Rotation # Noop for import to not be unused +except ModuleNotFoundError: + import sys + import subprocess + import os.path + exec = sys.executable + if "python" not in os.path.basename(sys.executable).lower(): + # There is a crazy thing on Mac, where sys.executable is Fusion iteself, not Python :( + exec = subprocess.__file__ + for i in range(3): + exec = os.path.dirname(exec) + exec = os.path.join(exec, "bin", "python") + subprocess.check_call([exec, '-m', 'ensurepip']) + subprocess.check_call([exec, '-m', 'pip', 'install', 'scipy', 'pyyaml']) + +from . import utils from . import manager -def file_dialog(ui): - '''display the dialog to save the file +def save_dir_dialog(ui: adsk.core.UserInterface) -> Optional[str]: + '''display the dialog to pick the save directory Parameters ---------- @@ -21,76 +41,155 @@ def file_dialog(ui): # Set styles of folder dialog. folderDlg = ui.createFolderDialog() - folderDlg.title = 'Fusion Folder Dialog' + folderDlg.title = 'URDF Save Folder Dialog' # Show folder dialog dlgResult = folderDlg.showDialog() if dlgResult == adsk.core.DialogResults.DialogOK: return folderDlg.folder - return False + return None +def yaml_file_dialog(ui: adsk.core.UserInterface) -> Optional[str]: + '''display the dialog to pick a yaml file + + Parameters + ---------- + ui : [type] + [description] + + Returns + ------- + [type] + [description] + ''' + + # Set styles of folder dialog. + fileDlg = ui.createFileDialog() + fileDlg.filter = "Configuration File (*.yaml)" + fileDlg.title = 'Configuration File Dialog' + fileDlg.isMultiSelectEnabled = False + + # Show folder dialog + dlgResult = fileDlg.showOpen() + if dlgResult == adsk.core.DialogResults.DialogOK: + return fileDlg.filename + return None class MyInputChangedHandler(adsk.core.InputChangedEventHandler): - def __init__(self, ui): + def __init__(self, ui: adsk.core.UserInterface): self.ui = ui super().__init__() - def notify(self, args): + def notify(self, eventArgs: adsk.core.InputChangedEventArgs) -> None: try: - cmd = args.firingEvent.sender + cmd = eventArgs.firingEvent.sender + assert isinstance(cmd, adsk.core.Command) inputs = cmd.commandInputs - cmdInput = args.input + cmdInput = eventArgs.input # Get settings of UI directory_path = inputs.itemById('directory_path') + config = inputs.itemById('config') + robot_name = inputs.itemById('robot_name') save_mesh = inputs.itemById('save_mesh') sub_mesh = inputs.itemById('sub_mesh') mesh_resolution = inputs.itemById('mesh_resolution') inertia_precision = inputs.itemById('inertia_precision') - document_units = inputs.itemById('document_units') target_units = inputs.itemById('target_units') - joint_order = inputs.itemById('joint_order') target_platform = inputs.itemById('target_platform') + preview_group = inputs.itemById('preview_group') + + utils.log(f"DEBUG: UI: processing command: {cmdInput.id}") + + assert isinstance(directory_path, adsk.core.TextBoxCommandInput) + assert isinstance(config, adsk.core.TextBoxCommandInput) + assert isinstance(robot_name, adsk.core.TextBoxCommandInput) + assert isinstance(save_mesh, adsk.core.BoolValueCommandInput) + assert isinstance(sub_mesh, adsk.core.BoolValueCommandInput) + assert isinstance(mesh_resolution, adsk.core.DropDownCommandInput) + assert isinstance(inertia_precision, adsk.core.DropDownCommandInput) + assert isinstance(target_units, adsk.core.DropDownCommandInput) + assert isinstance(target_platform, adsk.core.DropDownCommandInput) + assert isinstance(preview_group, adsk.core.GroupCommandInput) if cmdInput.id == 'generate': # User asked to generate using current settings # print(f'{directory_path.text}, {save_mesh.value}, {mesh_resolution.selectedItem.name},\ - # {inertia_precision.selectedItem.name}, {document_units.selectedItem.name},\ - # {target_units.selectedItem.name}, {joint_order.selectedItem.name}' ) + # {inertia_precision.selectedItem.name},\ + # {target_units.selectedItem.name} ) - document_manager = manager.Manager(directory_path.text, save_mesh.value, sub_mesh.value, + document_manager = manager.Manager(directory_path.text, robot_name.text, + save_mesh.value, sub_mesh.value, mesh_resolution.selectedItem.name, inertia_precision.selectedItem.name, - document_units.selectedItem.name, target_units.selectedItem.name, - joint_order.selectedItem.name, - target_platform.selectedItem.name) + target_platform.selectedItem.name, + config.text) # Generate document_manager.run() elif cmdInput.id == 'preview': # Generate Hierarchy and Preview in panel - document_manager = manager.Manager(directory_path.text, save_mesh.value, sub_mesh.value, mesh_resolution.selectedItem.name, - inertia_precision.selectedItem.name, document_units.selectedItem.name, target_units.selectedItem.name, - joint_order.selectedItem.name, target_platform.selectedItem.name) + document_manager = manager.Manager(directory_path.text, robot_name.text, save_mesh.value, sub_mesh.value, mesh_resolution.selectedItem.name, + inertia_precision.selectedItem.name, target_units.selectedItem.name, + target_platform.selectedItem.name, config.text) # # Generate _joints = document_manager.preview() joints_text = inputs.itemById('jointlist') + assert isinstance(joints_text, adsk.core.TextBoxCommandInput) _txt = 'joint name: parent link-> child link\n' for k, j in _joints.items(): - _txt += f'{k} : {j["parent"]} -> {j["child"]}\n' + _txt += f'{k} : {j.parent} -> {j.child}\n' joints_text.text = _txt + preview_group.isExpanded = True elif cmdInput.id == 'save_dir': # User set the save directory - save_dir = file_dialog(self.ui) - directory_path.text = save_dir + config_file = save_dir_dialog(self.ui) + if config_file is not None: + directory_path.text = config_file + directory_path.numRows = 2 - return True + elif cmdInput.id == 'set_config': + # User set the save directory + config_file = yaml_file_dialog(self.ui) + if config_file is not None: + try: + import yaml + with open(config_file, "rb") as yml: + configuration = yaml.load(yml, yaml.SafeLoader) + if 'RobotName' in configuration: + if not isinstance(configuration['RobotName'], str): + raise ValueError ("RobotName should be a string") + robot_name.text = configuration['RobotName'] + if 'SaveMesh' in configuration: + if not isinstance(configuration['SaveMesh'], bool): + raise ValueError ("SaveMesh should be a boolean") + save_mesh.value = configuration['SaveMesh'] + if 'SubMesh' in configuration: + if not isinstance(configuration['SubMesh'], bool): + raise ValueError ("SubMesh should be a boolean") + sub_mesh.value = configuration['SubMesh'] + for key, selector in [ + ('MeshResolution', mesh_resolution), + ('InertiaPrecision', inertia_precision), + ('TargetUnits', target_units), + ('TargetPlatform', target_platform), + ]: + if key in configuration: + names = [item.name for item in selector.listItems] + if configuration[key] not in names: + raise ValueError (f"{key} should be one of {names}") + for item in selector.listItems: + item.isSelected = (item.name == configuration[key]) + config.text = config_file + config.numRows = 2 + except Exception as e: + utils.log(f"ERROR: error loading configuration file {config_file}: {e}") except: if self.ui: self.ui.messageBox('Failed:\n{}'.format(traceback.format_exc())) @@ -99,7 +198,7 @@ class MyDestroyHandler(adsk.core.CommandEventHandler): def __init__(self, ui): self.ui = ui super().__init__() - def notify(self, args): + def notify(self, eventArgs: adsk.core.CommandEventArgs) -> None: try: adsk.terminate() except: @@ -114,7 +213,7 @@ class MyCreatedHandler(adsk.core.CommandCreatedEventHandler): adsk : adsk.core.CommandCreatedEventHandler Main handler for callbacks ''' - def __init__(self, ui, handlers): + def __init__(self, ui: adsk.core.UserInterface, handlers): '''[summary] Parameters @@ -128,7 +227,7 @@ def __init__(self, ui, handlers): self.handlers = handlers super().__init__() - def notify(self, args): + def notify(self, eventArgs: adsk.core.CommandCreatedEventArgs) -> None: ''' Construct the GUI and set aliases to be referenced later Parameters @@ -137,7 +236,7 @@ def notify(self, args): UI information ''' try: - cmd = args.command + cmd = eventArgs.command onDestroy = MyDestroyHandler(self.ui) cmd.destroy.add(onDestroy) @@ -148,18 +247,26 @@ def notify(self, args): self.handlers.append(onInputChanged) inputs = cmd.commandInputs + assert manager.Manager.root is not None # Show path to save - inputs.addTextBoxCommandInput('directory_path', 'Save Directory', 'C:', 2, True) + directory_path = inputs.addTextBoxCommandInput('directory_path', 'Save Directory', 'C:', 2, True) # Button to set the save directory btn = inputs.addBoolValueInput('save_dir', 'Set Save Directory', False) btn.isFullWidth = True + config_file = inputs.addTextBoxCommandInput('config', 'Configuration File (Optional)', '', 2, True) + # Button to set the save directory + btn = inputs.addBoolValueInput('set_config', 'Select Configuration File', False) + btn.isFullWidth = True + + inputs.addTextBoxCommandInput('robot_name', 'Robot Name', manager.Manager.root.name.split()[0], 1, False) + # Add checkbox to generate/export the mesh or not inputs.addBoolValueInput('save_mesh', 'Save Mesh', True) # Add checkbox to generate/export sub meshes or not - inputs.addBoolValueInput('sub_mesh', 'Sub Mesh', True) + inputs.addBoolValueInput('sub_mesh', 'Per-Body Visual Mesh', True) # Add dropdown to determine mesh export resolution di = inputs.addDropDownCommandInput('mesh_resolution', 'Mesh Resolution', adsk.core.DropDownStyles.TextListDropDownStyle) @@ -175,13 +282,6 @@ def notify(self, args): di.add('Medium', False, '') di.add('High', False, '') - # Add dropdown to set current document units - di = inputs.addDropDownCommandInput('document_units', 'Document Units', adsk.core.DropDownStyles.TextListDropDownStyle) - di = di.listItems - di.add('mm', False, '') - di.add('cm', True, '') - di.add('m', False, '') - # Add dropdown to set target export units di = inputs.addDropDownCommandInput('target_units', 'Target Units', adsk.core.DropDownStyles.TextListDropDownStyle) di = di.listItems @@ -189,18 +289,14 @@ def notify(self, args): di.add('cm', False, '') di.add('m', True, '') - # Add dropdown to define the order that joints were defined (parent as component 1 or component 2) - di = inputs.addDropDownCommandInput('joint_order', 'Joint Component 1', adsk.core.DropDownStyles.TextListDropDownStyle) - di = di.listItems - di.add('Parent', False, '') - di.add('Child', True, '') - # Set the type of platform to target for building XML di = inputs.addDropDownCommandInput('target_platform', 'Target Platform', adsk.core.DropDownStyles.TextListDropDownStyle) di = di.listItems di.add('None', True, '') di.add('pyBullet', False, '') - # di.add('m', False, '') # TODO Add other methods if needed + di.add('rviz', False, '') + di.add('Gazebo', False, '') + di.add('MoveIt', False, '') # Make a button to preview the hierarchy btn = inputs.addBoolValueInput('preview', 'Preview Links', False) @@ -210,8 +306,9 @@ def notify(self, args): tab_input = inputs.addTabCommandInput('tab_preview', 'Preview Tabs') tab_input_child = tab_input.children # Create group - input_group = tab_input_child.addGroupCommandInput("preview_group", "Preview") - textbox_group = input_group.children + preview_group = tab_input_child.addGroupCommandInput("preview_group", "Preview") + preview_group.isExpanded = False + textbox_group = preview_group.children # Create a textbox. txtbox = textbox_group.addTextBoxCommandInput('jointlist', 'Joint List', '', 8, True) @@ -221,13 +318,18 @@ def notify(self, args): btn = inputs.addBoolValueInput('generate', 'Generate', False) btn.isFullWidth = True + cmd.setDialogSize(500,0) + + # After setDialogSize to accomodate longer dir and config paths, set them to 1 row each + directory_path.numRows = 1 + config_file.numRows = 1 except: if self.ui: self.ui.messageBox('Failed:\n{}'.format(traceback.format_exc())) -def config_settings(ui, ui_handlers): +def config_settings(ui: adsk.core.UserInterface, ui_handlers) -> bool: '''[summary] Parameters @@ -263,7 +365,9 @@ def config_settings(ui, ui_handlers): return True except: + exn = traceback.format_exc() + utils.log(f"FATAL: {exn}") if ui: - ui.messageBox('Failed:\n{}'.format(traceback.format_exc())) + ui.messageBox(f'Failed:\n{exn}') - return False + return False diff --git a/Descriptor/core/utils.py b/Descriptor/core/utils.py new file mode 100644 index 0000000..2ff3c11 --- /dev/null +++ b/Descriptor/core/utils.py @@ -0,0 +1,84 @@ +import math +import time +from typing import Any, Dict, List, NoReturn, Optional, Sequence, Tuple, Union +import adsk.core +from .transforms import so3_to_euler + +LOG_DEBUG = False +REFRESH_DELAY = 2.0 + +start_time: Optional[float] = None +last_refresh = 0.0 + +def start_log_timer() -> None: + global start_time + start_time = time.time() + +def time_elapsed() -> float: + assert start_time is not None + return time.time() - start_time + +def format_name(input: str): + translation_table = str.maketrans({':':'_', '-':'_', '.':'_', ' ':'', '(':'{', ')':'}'}) + return input.translate(translation_table) + +def rename_if_duplicate(input: str, in_dict: Dict[str, Any]) -> str: + count = 0 + new_name = input + while in_dict.get(new_name) is not None: + new_name = f"{input}_{count}" + count += 1 + return new_name + +def convert_german(str_in): + translation_table = str.maketrans({'ä': 'ae', 'ö': 'oe', 'ü': 'ue', 'Ä': 'Ae', 'Ö': 'Oe', 'Ü': 'Ue', 'ß': 'ss'}) + return str_in.translate(translation_table) + +viewport: Optional[adsk.core.Viewport] = None + +all_warnings: List[str] = [] + +def log(msg: str, level: Optional[adsk.core.LogLevels] = None) -> None: + if not LOG_DEBUG and msg.startswith("DEBUG"): + return + if level is None: + level = adsk.core.LogLevels.InfoLogLevel + if msg.startswith("WARN"): + level = adsk.core.LogLevels.WarningLogLevel + elif msg.startswith("FATAL") or msg.startswith("ERR"): + level = adsk.core.LogLevels.ErrorLogLevel + adsk.core.Application.log(msg, level) + if level == adsk.core.LogLevels.WarningLogLevel: + all_warnings.append(msg) + if viewport is not None: + global last_refresh + if time.time() >= last_refresh + REFRESH_DELAY: + viewport.refresh() + last_refresh = time.time() + print(msg) + +def fatal(msg: str) -> NoReturn: + log_msg = "FATAL ERROR: " + msg + if start_time is not None: + log_msg += f"\n\tTime Elapsed: {time_elapsed():.1f}s." + log(log_msg) + raise RuntimeError(msg) + +def mat_str(m: adsk.core.Matrix3D) -> str: + return f"xyz={m.translation.asArray()} rpy={so3_to_euler(m)}" + +def vector_to_str(v: Union[adsk.core.Vector3D, adsk.core.Point3D, Sequence[float]], prec: int = 2) -> str: + """Turn a verctor into a debug printout""" + if isinstance(v, (adsk.core.Vector3D, adsk.core.Point3D)): + v = v.asArray() + return f"[{', '.join(f'{x:.{prec}f}' for x in v)}]" + +def rpy_to_str(rpy: Tuple[float, float, float]) -> str: + return f"({', '.join(str(int(x/math.pi*180)) for x in rpy)})" + +def ct_to_str(ct: adsk.core.Matrix3D) -> str: + """Turn a coordinate transform matrix into a debug printout""" + rpy = so3_to_euler(ct) + return (f"@{vector_to_str(ct.translation)}" + f" rpy={rpy_to_str(rpy)}" + f" ({' '.join(('[' + ','.join(str(int(x)) for x in v.asArray()) + ']') for v in ct.getAsCoordinateSystem()[1:])})") \ No newline at end of file diff --git a/Descriptor/gazebo_package/CMakeLists.txt b/Descriptor/gazebo_package/CMakeLists.txt new file mode 100644 index 0000000..434fdce --- /dev/null +++ b/Descriptor/gazebo_package/CMakeLists.txt @@ -0,0 +1,31 @@ +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 3.5) +project(fusion2urdf) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +################################################################################ +# Find ament packages and libraries for ament and system dependencies +################################################################################ +find_package(ament_cmake REQUIRED) +find_package(urdf REQUIRED) + +################################################################################ +# Install +################################################################################ +install(DIRECTORY meshes urdf launch + DESTINATION share/${PROJECT_NAME} +) + +################################################################################ +# Macro for ament package +################################################################################ +ament_package() \ No newline at end of file diff --git a/Descriptor/gazebo_package/LICENSE b/Descriptor/gazebo_package/LICENSE new file mode 100644 index 0000000..314c3af --- /dev/null +++ b/Descriptor/gazebo_package/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2018 Toshinori Kitamura + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/Descriptor/gazebo_package/launch/gazebo.launch.py b/Descriptor/gazebo_package/launch/gazebo.launch.py new file mode 100644 index 0000000..740178d --- /dev/null +++ b/Descriptor/gazebo_package/launch/gazebo.launch.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 + +from launch import LaunchDescription +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch.actions import IncludeLaunchDescription, AppendEnvironmentVariable +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory, get_package_prefix + +def generate_launch_description(): + ###### ROBOT DESCRIPTION ###### + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("fusion2urdf"), + "urdf", + "fusion2urdf.xacro", + ] + ), + + ] + ) + + robot_description = {"robot_description": robot_description_content} + + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[robot_description] + ) + + joint_state_publisher_gui_node = Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + name='joint_state_publisher_gui' + ) + + ###### RVIZ ###### + rviz_config_file = PathJoinSubstitution( + [ + FindPackageShare("fusion2urdf"), + "launch", + "urdf.rviz", + ] + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_file], + output='screen' + ) + + ###### GAZEBO ###### + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution( + [ + get_package_share_directory("gazebo_ros"), + "launch", + "gazebo.launch.py", + ] + ) + ]), + ) + + spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', + arguments=['-topic', 'robot_description', + '-entity', 'fusion2urdf'], + output='both') + + # Add the install path model path + gazebo_env = AppendEnvironmentVariable("GAZEBO_MODEL_PATH", PathJoinSubstitution([get_package_prefix("fusion2urdf"), "share"])) + + return LaunchDescription([ + gazebo_env, + robot_state_publisher_node, + joint_state_publisher_gui_node, + rviz_node, + gazebo, + spawn_entity + ]) \ No newline at end of file diff --git a/Descriptor/gazebo_package/launch/robot_description.launch.py b/Descriptor/gazebo_package/launch/robot_description.launch.py new file mode 100644 index 0000000..a80a53b --- /dev/null +++ b/Descriptor/gazebo_package/launch/robot_description.launch.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 + +from launch import LaunchDescription +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("fusion2urdf"), + "urdf", + "fusion2urdf.xacro", + ] + ), + + ] + ) + + robot_description = {"robot_description": robot_description_content} + + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[robot_description] + ) + + joint_state_publisher_gui_node = Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + name='joint_state_publisher_gui' + ) + + rviz_config_file = PathJoinSubstitution( + [ + FindPackageShare("fusion2urdf"), + "launch", + "urdf.rviz", + ] + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_file], + output='screen' + ) + + return LaunchDescription([ + robot_state_publisher_node, + joint_state_publisher_gui_node, + rviz_node + ]) \ No newline at end of file diff --git a/Descriptor/gazebo_package/launch/urdf.rviz b/Descriptor/gazebo_package/launch/urdf.rviz new file mode 100644 index 0000000..bb9b3b8 --- /dev/null +++ b/Descriptor/gazebo_package/launch/urdf.rviz @@ -0,0 +1,158 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /TF1 + Splitter Ratio: 0.5 + Tree Height: 617 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: dummy_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 0.8402727246284485 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6097970008850098 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.5103846192359924 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 201 + Y: 69 diff --git a/Descriptor/gazebo_package/package.xml b/Descriptor/gazebo_package/package.xml new file mode 100644 index 0000000..f36e3a2 --- /dev/null +++ b/Descriptor/gazebo_package/package.xml @@ -0,0 +1,14 @@ + + + + fusion2urdf + 0.0.0 + The fusion2urdf package + Spacemaster85 + MIT + ament_cmake + xacro + + ament_cmake + + diff --git a/Descriptor/moveit_package/CMakeLists.txt b/Descriptor/moveit_package/CMakeLists.txt new file mode 100644 index 0000000..434fdce --- /dev/null +++ b/Descriptor/moveit_package/CMakeLists.txt @@ -0,0 +1,31 @@ +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 3.5) +project(fusion2urdf) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +################################################################################ +# Find ament packages and libraries for ament and system dependencies +################################################################################ +find_package(ament_cmake REQUIRED) +find_package(urdf REQUIRED) + +################################################################################ +# Install +################################################################################ +install(DIRECTORY meshes urdf launch + DESTINATION share/${PROJECT_NAME} +) + +################################################################################ +# Macro for ament package +################################################################################ +ament_package() \ No newline at end of file diff --git a/Descriptor/moveit_package/LICENSE b/Descriptor/moveit_package/LICENSE new file mode 100644 index 0000000..314c3af --- /dev/null +++ b/Descriptor/moveit_package/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2018 Toshinori Kitamura + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/Descriptor/moveit_package/launch/setup_assistant.launch.py b/Descriptor/moveit_package/launch/setup_assistant.launch.py new file mode 100644 index 0000000..f2826e8 --- /dev/null +++ b/Descriptor/moveit_package/launch/setup_assistant.launch.py @@ -0,0 +1,29 @@ +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils.launch_utils import ( + add_debuggable_node, + DeclareBooleanLaunchArg, +) + +def generate_launch_description(): + ld = LaunchDescription() + + ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False)) + add_debuggable_node( + ld, + package="moveit_setup_assistant", + executable="moveit_setup_assistant", + arguments=[ + "--urdf_path", + PathJoinSubstitution( + [ + FindPackageShare("fusion2urdf"), + "urdf", + "fusion2urdf.xacro", + ] + ) + ] + ) + + return ld diff --git a/Descriptor/moveit_package/launch/urdf.rviz b/Descriptor/moveit_package/launch/urdf.rviz new file mode 100644 index 0000000..bb9b3b8 --- /dev/null +++ b/Descriptor/moveit_package/launch/urdf.rviz @@ -0,0 +1,158 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /TF1 + Splitter Ratio: 0.5 + Tree Height: 617 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: dummy_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 0.8402727246284485 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6097970008850098 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.5103846192359924 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 201 + Y: 69 diff --git a/Descriptor/moveit_package/package.xml b/Descriptor/moveit_package/package.xml new file mode 100644 index 0000000..f36e3a2 --- /dev/null +++ b/Descriptor/moveit_package/package.xml @@ -0,0 +1,14 @@ + + + + fusion2urdf + 0.0.0 + The fusion2urdf package + Spacemaster85 + MIT + ament_cmake + xacro + + ament_cmake + + diff --git a/Descriptor/package_ros2/CMakeLists.txt b/Descriptor/package_ros2/CMakeLists.txt new file mode 100644 index 0000000..434fdce --- /dev/null +++ b/Descriptor/package_ros2/CMakeLists.txt @@ -0,0 +1,31 @@ +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 3.5) +project(fusion2urdf) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +################################################################################ +# Find ament packages and libraries for ament and system dependencies +################################################################################ +find_package(ament_cmake REQUIRED) +find_package(urdf REQUIRED) + +################################################################################ +# Install +################################################################################ +install(DIRECTORY meshes urdf launch + DESTINATION share/${PROJECT_NAME} +) + +################################################################################ +# Macro for ament package +################################################################################ +ament_package() \ No newline at end of file diff --git a/Descriptor/package_ros2/LICENSE b/Descriptor/package_ros2/LICENSE new file mode 100644 index 0000000..314c3af --- /dev/null +++ b/Descriptor/package_ros2/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2018 Toshinori Kitamura + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/Descriptor/package_ros2/launch/robot_description.launch.py b/Descriptor/package_ros2/launch/robot_description.launch.py new file mode 100644 index 0000000..a80a53b --- /dev/null +++ b/Descriptor/package_ros2/launch/robot_description.launch.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 + +from launch import LaunchDescription +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("fusion2urdf"), + "urdf", + "fusion2urdf.xacro", + ] + ), + + ] + ) + + robot_description = {"robot_description": robot_description_content} + + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[robot_description] + ) + + joint_state_publisher_gui_node = Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + name='joint_state_publisher_gui' + ) + + rviz_config_file = PathJoinSubstitution( + [ + FindPackageShare("fusion2urdf"), + "launch", + "urdf.rviz", + ] + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_file], + output='screen' + ) + + return LaunchDescription([ + robot_state_publisher_node, + joint_state_publisher_gui_node, + rviz_node + ]) \ No newline at end of file diff --git a/Descriptor/package_ros2/launch/urdf.rviz b/Descriptor/package_ros2/launch/urdf.rviz new file mode 100644 index 0000000..bb9b3b8 --- /dev/null +++ b/Descriptor/package_ros2/launch/urdf.rviz @@ -0,0 +1,158 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /TF1 + Splitter Ratio: 0.5 + Tree Height: 617 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: dummy_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 0.8402727246284485 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6097970008850098 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.5103846192359924 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 201 + Y: 69 diff --git a/Descriptor/package_ros2/package.xml b/Descriptor/package_ros2/package.xml new file mode 100644 index 0000000..f36e3a2 --- /dev/null +++ b/Descriptor/package_ros2/package.xml @@ -0,0 +1,14 @@ + + + + fusion2urdf + 0.0.0 + The fusion2urdf package + Spacemaster85 + MIT + ament_cmake + xacro + + ament_cmake + + diff --git a/Getting-Started.md b/Getting-Started.md index 3d01645..3ebe82b 100644 --- a/Getting-Started.md +++ b/Getting-Started.md @@ -9,6 +9,8 @@ Installation & Running - Click on the green + to add the script into Fusion 360. - Add the **Descriptor** folder from the zip file to the scripts. You may have to unzip the file after downloading from Github - Click on **Descriptor** then click on **Run**. The GUI will appear. +- Rather than configuring the export manually in the GUI, you can create a YAML configuration file to control the export. A number of more + advanced features are only available via the configuraiton file. See `configuration_sample.yaml` for more information. To open the URDF file in PyBullet, do the following: diff --git a/README.md b/README.md index 4301f72..7aca45a 100644 --- a/README.md +++ b/README.md @@ -22,13 +22,16 @@ It provides a simple GUI for running the script and choosing a few different set **Features** - GUI interface for settings -- Uses the grounded base as root node +- Uses the grounded base as root node, that organizes the UDRF tree by joint/link distance from root. +- Handles rigid groups, coordinate transforms between origins, nested assemblies, etc - Allows switching between units - WYSIWYG stl exporting (exports model as you see on the screen) without needing to save design history or copy to a new file - Preview joint relationship before exporting - Export only URDF without Mesh (for fast viewing) -- Set Component 1 of joint to mean Parent or Child - Can export component bodies separately as a visual element of URDF and a combined component as the collision +- Can load export configuration from a Yaml file to speed up configuring the export. +- Additional advanced options can be specified in the Yaml file, such as automatically mapping component and/or joint names to custom names in URDF. + This is helpful to e.g. keep the URDF names constant as the Fusion design is updated, so that ROS configuration/code does not have to be changed. drawing @@ -116,7 +119,11 @@ Step-by-Step Guide - Add the **Descriptor** folder from the zip file to the scripts. - Click on **Descriptor** then click on **Run**. The GUI will appear. - Add a save directory for the output and select the desired options. +- Note: rather than selecting the desired export options manually in the GUI (and having to re-enter them every time you want to re-export), + you can create a YAML configuration file to control the export. Also, a number of more advanced features are only available via the + configuraiton file. See `configuration_sample.yaml` for more information. drawing - Click on **Generate**. The output can be found where the save directory is. + diff --git a/configuration_sample.yaml b/configuration_sample.yaml new file mode 100644 index 0000000..8a5c9dc --- /dev/null +++ b/configuration_sample.yaml @@ -0,0 +1,62 @@ +### Values to fill into the UI dialog +RobotName: my_great_robot +SaveMesh: true +SubMesh: true +MeshResolution: Low +InertiaPrecision: Low +TargetUnits: m +TargetPlatform: MoveIt + +### Additional URDF conversion options +# Rename links and/or joints. +# Maps Fusion names for occurrences and joints into URDF names for links and joints +# For links, MergeLinks chould also be used instead (and would probably be better). +NameMap: + my_big_slider: Slider + Rigid-Pulley-Spindle: Rev + +# If specified, serves as the root of the URDF regardless of what's grounded in Fusion +# Can be either a name of the Fusion occurance (has to be a literal match), or a name +# of a link from the MergeLinks section below. +Root: Robot_Base + +# Merge several Fusion occurrences into a single URDF link +# Can name either indivicual occurrences, or complete assemblies. +# When an assembly occurrence is mentioned, all components in that assmembly will be included, +# but it's not an error and would not change the result if some of the occurrences for components +# and/or subassemblies are also specified explicitly. +# The origin of the first occurrence specified would be used as the origin for the URDF link +# (except for links immedeately downstream from a non-fixed joint - there the joint origin +# would be the link origin, but the orientation would still follow that of the first occurrence). +# The name of occurrences/assemblies can be specifies as follows: +# - Literal name +# - Occurence name pattern with exactly one wildcard symbol "*" (wildcard matches any substring) +# - as a +-deliminated full path suffix, each element potentially containing a single wildcard +# In each case, there should be exactly one occurrence matching the specification +MergeLinks: + Robot_Base: + - Optical Breadboard - 1000x1000 v* + - Post:1 + - Second optical breadboard v* + - Slider Assembly v*+SliderBase v* + AddOnComponent1: + - Slider Assembly v*+*+AddOn v* + +# Use this to specify parts of the Design that should be exported, but not included in the +# main urdf/.xacro. If present, that these links and the joints that connect +# them to the rest of the robot would not be included in urdf/.xacro, and +# a urdf/-full.xacro would be created containing the full set. This is useful +# e.g. when these extra links are meant to be used as an AttachedCollisionObject with MoveIT +# These links should not have any downstream joints/links. +Extras: + - AddOnComponent1 + - AddOnComponent2 + +# When present and non-empty, will generate a urdf/locations.yaml containing specified locations w.r.t +# specified links. For each UDFL link, specify the locations.yaml and the Fusion occurrence to take the origin from +# This is helpful if you want to e.g. use MoveIt and specify some parts of your Fusion model as "attached" objects +# in your scene, rather than a part of your robot (note: you will need to write your own code to set that up). +Locations: + AddOnComponent1: + Base: Slider Assembly v*+*+AddOn v*+Base* + Tip: Slider Assembly v*+*+AddOn v*+Tip* \ No newline at end of file diff --git a/pyrightconfig.json b/pyrightconfig.json new file mode 100644 index 0000000..f2d726d --- /dev/null +++ b/pyrightconfig.json @@ -0,0 +1,3 @@ +{ + "reportMissingImports": "error" +}