Import upstream snapshot 80e710700aac9573a2230f74f7ce9e094833a0bc

Upstream: https://github.com/Rhoban/onshape-to-robot
Upstream-Commit: 80e710700aac9573a2230f74f7ce9e094833a0bc
Upstream-Branch: master

onshape_to_robot/__init__.py

@@ -0,0 +1,3 @@
+"""Export models from Onshape to other robotics formats."""
+
+from . import export, bullet, mujoco, clear_cache, edit_shape, pure_sketch

onshape_to_robot/assembly.py

@@ -0,0 +1,951 @@
+from __future__ import annotations
+import numpy as np
+from .config import Config
+from .message import error, info, bright, success, warning
+from .onshape_api.client import Client
+from .robot import Joint
+from .expression import ExpressionParser
+
+INSTANCE_IGNORE = -1
+
+
+class Frame:
+    """
+    Represents a frame attached
+    """
+
+    def __init__(self, body_id: int, name: str, T_world_frame: np.ndarray):
+        self.body_id: int = body_id
+        self.name: str = name
+        self.T_world_frame: np.ndarray = T_world_frame
+
+
+class DOF:
+    """
+    Represents a DOF
+    """
+
+    def __init__(
+        self,
+        body1_id: int,
+        body2_id: int,
+        name: str,
+        joint_type: str,
+        T_world_mate: np.ndarray,
+        limits: tuple | None,
+        axis: np.ndarray = np.array([0.0, 0.0, 1.0]),
+    ):
+        self.body1_id: int = body1_id
+        self.body2_id: int = body2_id
+        self.name: str = name
+        self.joint_type: str = joint_type
+        self.T_world_mate: np.ndarray = T_world_mate
+        self.limits: tuple | None = limits
+        self.axis: np.ndarray = axis
+
+    def flip(self, flip_limits: bool = True):
+        if flip_limits and self.limits is not None:
+            self.limits = (-self.limits[1], -self.limits[0])
+
+        # Flipping the joint around X axis
+        flip = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]])
+        self.T_world_mate[:3, :3] = self.T_world_mate[:3, :3] @ flip
+
+    def other_body(self, body_id: int):
+        if body_id == self.body1_id:
+            return self.body2_id
+        elif body_id == self.body2_id:
+            return self.body1_id
+        else:
+            raise Exception(f"ERROR: body {body_id} is not part of this DOF")
+
+
+class Assembly:
+    """
+    Main entry point to process an assembly
+    """
+
+    def __init__(self, config: Config):
+        self.config: Config = config
+
+        # Creating Onshape API client
+        self.client = Client(logging=False, creds=self.config.config_file)
+        self.expression_parser = ExpressionParser()
+        self.expression_parser.variables_lazy_loading = self.load_variables
+
+        self.document_id: str = config.document_id
+        self.workspace_id: str | None = config.workspace_id
+        self.version_id: str | None = config.version_id
+
+        # All (raw) data from assembly
+        self.assembly_data: dict = {}
+        # Map a (top-level) instance id to a body id
+        self.current_body_id: int = 0
+        self.instance_body: dict[str, int] = {}
+        # Frames object
+        self.frames: list[Frame] = []
+        # Loop closure constraints
+        self.closures: list = []
+        # Degrees of freedom
+        self.dofs: list[DOF] = []
+        # Features data
+        self.features: dict = {}
+        # Configuration values
+        self.configuration_parameters: dict = {}
+        # Dictionnary mapping items to their children in the tree
+        self.tree_children: dict = {}
+        # Root nodes
+        self.root_nodes: list = []
+        # Overriden link names
+        self.link_names: dict[int, str] = {}
+        # Relation indexed by target joints, values are [source joint, ratio]
+        self.relations: dict = {}
+
+        self.ensure_workspace_or_version()
+        self.find_assembly()
+        self.check_configuration()
+        self.retrieve_assembly()
+        self.find_instances()
+        self.load_features()
+        self.load_configuration()
+        self.process_mates()
+        self.build_trees()
+        self.find_relations()
+        print("")
+
+    def ensure_workspace_or_version(self):
+        """
+        Ensure either a workspace id or a version id is set
+        If none, try to retrieve the current workspace ID from API
+        """
+        if self.version_id:
+            print(bright(f"* Using configuration version ID {self.version_id} ..."))
+        elif self.workspace_id:
+            print(bright(f"* Using configuration workspace ID {self.workspace_id} ..."))
+        else:
+            print(
+                bright(
+                    "* Not workspace ID specified, retrieving the current workspace ..."
+                )
+            )
+            document = self.client.get_document(self.config.document_id)
+            self.workspace_id = document["defaultWorkspace"]["id"]
+            print(success(f"+ Using workspace id: {self.workspace_id}"))
+
+    def find_assembly(self):
+        """
+        Find the wanted assembly from the document
+        """
+        if self.config.element_id:
+            print(
+                bright(f"* Using configuration element ID {self.config.element_id} ...")
+            )
+            self.element_id = self.config.element_id
+            return
+
+        print(
+            bright(
+                "\n* Retrieving elements in the document, searching for the assembly..."
+            )
+        )
+
+        elements = self.client.list_elements(
+            self.document_id,
+            self.version_id if self.version_id else self.workspace_id,
+            "v" if self.version_id else "w",
+        )
+
+        self.element_id = None
+        assemblies: dict = {}
+        for element in elements:
+            if element["type"] == "Assembly":
+                assemblies[element["name"]] = element["id"]
+
+        if self.config.assembly_name:
+            if self.config.assembly_name in assemblies:
+                self.element_id = assemblies[self.config.assembly_name]
+            else:
+                raise Exception(
+                    f"ERROR: Unable to find required assembly {self.config.assembly_name} in this document"
+                )
+        else:
+            if len(assemblies) == 0:
+                raise Exception("ERROR: No assembly found in this document\n")
+            elif len(assemblies) == 1:
+                self.element_id = list(assemblies.values())[0]
+            else:
+                raise Exception(
+                    f"ERROR: Multiple assemblies found, please specify the assembly name\n"
+                    + '       to export (use "assemblyName" in the configuration file)\n'
+                    + f"       Available assemblies: {', '.join(assemblies.keys())}"
+                )
+
+        if self.element_id == None:
+            raise Exception(f"ERROR: Unable to find assembly in this document")
+
+    def check_configuration(self):
+        """
+        Retrieve configuration items for given assembly and parsing config configuration
+        """
+
+        if self.config.configuration != "default":
+            # Retrieving available config parameters
+            elements = self.client.elements_configuration(
+                self.document_id,
+                self.version_id if self.version_id else self.workspace_id,
+                self.element_id,
+                wmv=("v" if self.version_id else "w"),
+            )
+
+            parameters = {}
+            for entry in elements["configurationParameters"]:
+                type_name = entry["typeName"]
+                message = entry["message"]
+
+                if type_name.startswith("BTMConfigurationParameterEnum"):
+                    # The very first label typed is kept as the internal name for the enum, under the "option"
+                    # key. However, the user label that can be changed later is "optionName"
+                    option_names = [
+                        option["message"]["optionName"] for option in message["options"]
+                    ]
+                    options = [
+                        option["message"]["option"] for option in message["options"]
+                    ]
+                    parameters[message["parameterName"]] = [
+                        "enum",
+                        message["parameterId"],
+                        option_names,
+                        options,
+                    ]
+                elif type_name.startswith("BTMConfigurationParameterBoolean"):
+                    parameters[message["parameterName"]] = ["bool"]
+                elif type_name.startswith("BTMConfigurationParameterQuantity"):
+                    parameters[message["parameterName"]] = ["quantity"]
+
+            # Parsing configuration
+            parts = self.config.configuration.split(";")
+            processed_configuration = []
+            for part in parts:
+                kv = part.split("=")
+                if len(kv) == 2:
+                    key, value = kv
+                    if key not in parameters:
+                        raise Exception(
+                            f'ERROR: Unknown configuration parameter "{key}" in the configuration'
+                        )
+                    if parameters[key][0] == "enum":
+                        if value not in parameters[key][2]:
+                            raise Exception(
+                                f'ERROR: Unknown value "{value}" for configuration parameter "{key}"'
+                            )
+
+                        value = parameters[key][3][parameters[key][2].index(value)]
+                        key = parameters[key][1]
+                    processed_configuration.append(f"{key}={value.replace(' ', '+')}")
+
+            # Re-writing the configuration
+            self.config.configuration = ";".join(processed_configuration)
+
+    def retrieve_assembly(self):
+        """
+        Retrieve all assembly data
+        """
+        print(bright(f"* Retrieving assembly with id {self.element_id}"))
+
+        self.assembly_data: dict = self.client.get_assembly(
+            self.document_id,
+            self.version_id if self.version_id else self.workspace_id,
+            self.element_id,
+            wmv=("v" if self.version_id else "w"),
+            configuration=self.config.configuration,
+        )
+
+        self.microversion_id: str = self.assembly_data["rootAssembly"][
+            "documentMicroversion"
+        ]
+        self.occurrences: dict = {}
+        for occurrence in self.assembly_data["rootAssembly"]["occurrences"]:
+            self.occurrences[tuple(occurrence["path"])] = occurrence
+
+    def find_instances(self, prefix: list = [], instances=None):
+        """
+        Walking all the instances and associating them with their occurrences
+        """
+        if instances is None:
+            instances = self.assembly_data["rootAssembly"]["instances"]
+
+        for instance in instances:
+            if "type" in instance:
+                path = prefix + [instance["id"]]
+                self.get_occurrence(path)["instance"] = instance
+
+                if instance["type"] == "Assembly":
+                    if not instance["suppressed"]:
+                        d = instance["documentId"]
+                        m = instance["documentMicroversion"]
+                        e = instance["elementId"]
+                        c = instance["configuration"]
+                        for sub_assembly in self.assembly_data["subAssemblies"]:
+                            if (
+                                sub_assembly["documentId"] == d
+                                and sub_assembly["documentMicroversion"] == m
+                                and sub_assembly["elementId"] == e
+                                and sub_assembly["configuration"] == c
+                            ):
+                                self.find_instances(
+                                    prefix + [instance["id"]], sub_assembly["instances"]
+                                )
+
+    def load_features(self):
+        """
+        Load features
+        """
+
+        self.features = self.client.get_features(
+            self.document_id,
+            self.microversion_id,
+            self.element_id,
+            wmv="m",
+            configuration=self.config.configuration,
+        )
+
+        self.matevalues = self.client.matevalues(
+            self.document_id,
+            self.version_id if self.version_id else self.workspace_id,
+            self.element_id,
+            wmv="v" if self.version_id else "w",
+            configuration=self.config.configuration,
+        )
+
+    def load_configuration(self):
+        """
+        Load configuration parameters
+        """
+
+        self.variable_values = None
+
+        # Extracting configuration variables
+        parts = self.assembly_data["rootAssembly"]["fullConfiguration"].split(";")
+        for part in parts:
+            key_value = part.split("=")
+            if len(key_value) == 2:
+                key, value = key_value
+                value = value.replace("+", " ")
+                self.configuration_parameters[key] = value
+                try:
+                    param_value = self.expression_parser.eval_expr(value)
+                    self.expression_parser.variables[key] = param_value
+                except ValueError:
+                    pass
+
+    def load_variables(self):
+        """
+        Load variables values (only if needed) in the expression parser
+        """
+        variables = self.client.get_variables(
+            self.document_id,
+            self.version_id if self.version_id else self.workspace_id,
+            self.element_id,
+            wmv="v" if self.version_id else "w",
+            configuration=self.config.configuration,
+        )
+        for entry in variables:
+            for variable in entry["variables"]:
+                if variable["value"] is not None:
+                    self.expression_parser.variables[variable["name"]] = (
+                        self.expression_parser.eval_expr(variable["value"])
+                    )
+
+    def get_occurrence(self, path: list):
+        """
+        Retrieve occurrence from its path
+        """
+        return self.occurrences[tuple(path)]
+
+    def get_occurrence_transform(self, path: list) -> np.ndarray:
+        """
+        Retrieve occurrence transform from its path
+        """
+        T_world_part = np.array(self.get_occurrence(path)["transform"]).reshape(4, 4)
+
+        return T_world_part
+
+    def cs_to_transformation(self, cs: dict) -> np.ndarray:
+        """
+        Convert a coordinate system to a transformation matrix
+        """
+        T = np.eye(4)
+        T[:3, :3] = np.stack(
+            (
+                np.array(cs["xAxis"]),
+                np.array(cs["yAxis"]),
+                np.array(cs["zAxis"]),
+            )
+        ).T
+        T[:3, 3] = cs["origin"]
+
+        return T
+
+    def get_mate_transform(self, mated_entity: dict):
+        return self.cs_to_transformation(mated_entity["matedCS"])
+
+    def make_body(self, id: str):
+        """
+        Make the given instance id a body
+        """
+        self.instance_body[id] = self.current_body_id
+        self.current_body_id += 1
+
+    def merge_bodies(self, occurrence_A: str, occurrence_B: str):
+        # Ensure occurrences are body
+        if occurrence_A not in self.instance_body:
+            self.make_body(occurrence_A)
+        if occurrence_B not in self.instance_body:
+            self.make_body(occurrence_B)
+
+        # Merging bodies
+        body1_id = self.instance_body[occurrence_A]
+        body2_id = self.instance_body[occurrence_B]
+        if body1_id > body2_id:
+            body1_id, body2_id = body2_id, body1_id
+
+        for occurrence in self.instance_body:
+            if self.instance_body[occurrence] == body2_id:
+                self.instance_body[occurrence] = body1_id
+
+        for dof in self.dofs:
+            if dof.body1_id == body2_id:
+                dof.body1_id = body1_id
+            if dof.body2_id == body2_id:
+                dof.body2_id = body1_id
+
+    def translation(self, x: float, y: float, z: float) -> np.ndarray:
+        return np.array(
+            [
+                [1, 0, 0, x],
+                [0, 1, 0, y],
+                [0, 0, 1, z],
+                [0, 0, 0, 1],
+            ]
+        )
+
+    def process_mates(self):
+        """
+        Pre-assign all top-level instances to a separate body id
+        """
+        top_level_instances = self.assembly_data["rootAssembly"]["instances"]
+        self.make_body(top_level_instances[0]["id"])
+
+        # We first search for DOFs
+        for data, occurrence_A, occurrence_B in self.feature_mating_two_occurrences():
+            if data["name"].startswith("dof_"):
+                # Process the DOF name, removing dof prefix and inv suffix
+                parts = data["name"].split("_")
+                del parts[0]
+                data["inverted"] = False
+                if parts[-1] == "inv" or parts[-1] == "inverted":
+                    data["inverted"] = True
+                    del parts[-1]
+                name = "_".join(parts)
+
+                if name == "":
+                    raise Exception(
+                        f"ERROR: the following dof should have a name {data['name']}"
+                    )
+
+                # Finding joint type and limits
+                limits = None
+                if data["mateType"] == "REVOLUTE" or data["mateType"] == "CYLINDRICAL":
+                    if "wheel" in parts or "continuous" in parts:
+                        joint_type = Joint.CONTINUOUS
+                    else:
+                        joint_type = Joint.REVOLUTE
+
+                    if not self.config.ignore_limits:
+                        limits = self.get_limits(joint_type, data["name"])
+                elif data["mateType"] == "SLIDER":
+                    joint_type = Joint.PRISMATIC
+                    if not self.config.ignore_limits:
+                        limits = self.get_limits(joint_type, data["name"])
+                elif data["mateType"] == "FASTENED":
+                    joint_type = Joint.FIXED
+                elif data["mateType"] == "BALL":
+                    joint_type = Joint.BALL
+                    if not self.config.ignore_limits:
+                        limits = self.get_limits(joint_type, data["name"])
+                else:
+                    raise Exception(
+                        f"ERROR: {name} is declared as a DOF but the mate type is {data['mateType']}\n"
+                        + "       Only REVOLUTE, CYLINDRICAL, SLIDER and FASTENED are supported"
+                    )
+
+                # We compute the axis in the world frame
+                mated_entity = data["matedEntities"][0]
+                T_world_part = self.get_occurrence_transform(
+                    mated_entity["matedOccurrence"]
+                )
+
+                # jointToPart is the (rotation only) matrix from joint to the part
+                # it is attached to
+                T_part_mate = self.get_mate_transform(mated_entity)
+
+                T_world_mate = T_world_part @ T_part_mate
+
+                limits_str = ""
+                if limits is not None:
+                    limits_str = f"[{round(limits[0], 3)}: {round(limits[1], 3)}]"
+                print(success(f"+ Found DOF: {name} ({joint_type}) {limits_str}"))
+
+                # Ensure occurrences are body
+                if occurrence_A not in self.instance_body:
+                    self.make_body(occurrence_A)
+                if occurrence_B not in self.instance_body:
+                    self.make_body(occurrence_B)
+
+                dof = DOF(
+                    self.instance_body[occurrence_A],
+                    self.instance_body[occurrence_B],
+                    name,
+                    joint_type,
+                    T_world_mate,
+                    limits,
+                )
+
+                if data["inverted"]:
+                    dof.flip()
+
+                self.dofs.append(dof)
+
+        # Merging fixed links
+        for data, occurrence_A, occurrence_B in self.feature_mating_two_occurrences():
+            if data["name"].startswith("fix_") or (
+                data["mateType"] == "FASTENED"
+                and not data["name"].startswith("dof_")
+                and not data["name"].startswith("closing_")
+                and not data["name"].startswith("frame_")
+            ):
+                self.merge_bodies(occurrence_A, occurrence_B)
+
+        # Merging mate gorups
+        for group in self.feature_mate_groups():
+            for k in range(1, len(group)):
+                occurrence_A = group[0]
+                occurrence_B = group[k]
+
+                self.merge_bodies(occurrence_A, occurrence_B)
+
+        # Processing frame mates
+        for data, occurrence_A, occurrence_B in self.feature_mating_two_occurrences():
+            if data["name"].startswith("frame_"):
+                name = "_".join(data["name"].split("_")[1:])
+                if (
+                    occurrence_A not in self.instance_body
+                    and occurrence_B in self.instance_body
+                ):
+                    parent, child = occurrence_B, occurrence_A
+                    mated_entity = data["matedEntities"][0]
+                elif (
+                    occurrence_B not in self.instance_body
+                    and occurrence_A in self.instance_body
+                ):
+                    parent, child = occurrence_A, occurrence_B
+                    mated_entity = data["matedEntities"][1]
+                else:
+                    raise Exception(
+                        f"Frame {name} should mate an orphan body to a body in the kinematics tree"
+                    )
+
+                T_world_part = self.get_occurrence_transform(
+                    mated_entity["matedOccurrence"]
+                )
+
+                self.frames.append(
+                    Frame(self.instance_body[parent], name, T_world_part)
+                )
+
+                if self.config.draw_frames:
+                    self.merge_bodies(parent, child)
+                else:
+                    self.instance_body[child] = INSTANCE_IGNORE
+
+        # Checking that all intances are assigned to a body
+        for instance in self.assembly_data["rootAssembly"]["instances"]:
+            if instance["id"] not in self.instance_body and not instance["suppressed"]:
+                self.make_body(instance["id"])
+
+        # Processing loop closing frames
+        for data, occurrence_A, occurrence_B in self.feature_mating_two_occurrences():
+            is_hinge_closure = data["mateType"] == "REVOLUTE"
+
+            if data["name"].startswith("closing_"):
+                for k in 0, 1:
+                    mated_entity = data["matedEntities"][k]
+                    occurrence = mated_entity["matedOccurrence"][0]
+
+                    T_world_part = self.get_occurrence_transform(
+                        mated_entity["matedOccurrence"]
+                    )
+                    T_part_mate = self.get_mate_transform(mated_entity)
+                    T_world_mate = T_world_part @ T_part_mate
+
+                    self.frames.append(
+                        Frame(
+                            self.instance_body[occurrence],
+                            f"{data['name']}_{k+1}",
+                            T_world_mate,
+                        )
+                    )
+
+                    if is_hinge_closure:
+                        self.frames.append(
+                            Frame(
+                                self.instance_body[occurrence],
+                                f"{data['name']}_{k+1}_z",
+                                T_world_mate @ self.translation(0, 0, 0.1),
+                            )
+                        )
+
+                closure_types = {
+                    "FASTENED": "fixed",
+                    "REVOLUTE": "revolute",
+                    "BALL": "ball",
+                    "SLIDER": "slider",
+                }
+
+                self.closures.append(
+                    [
+                        closure_types.get(data["mateType"], "unknown"),
+                        f"{data['name']}_1",
+                        f"{data['name']}_2",
+                    ]
+                )
+                if is_hinge_closure:
+                    self.closures.append(
+                        [
+                            closure_types.get(data["mateType"], "unknown"),
+                            f"{data['name']}_1_z",
+                            f"{data['name']}_2_z",
+                        ]
+                    )
+
+        # Search for mate connector named "link_..." to override link names
+        for feature in self.assembly_data["rootAssembly"]["features"]:
+            # Suppressed mate connectors reference occurrences that may no longer
+            # exist in the assembly, so skip them like mates and mate groups do.
+            if feature.get("suppressed"):
+                continue
+
+            if feature["featureType"] == "mateConnector" and feature["featureData"][
+                "name"
+            ].startswith("link_"):
+                link_name = "_".join(feature["featureData"]["name"].split("_")[1:])
+                body_id = self.instance_body[feature["featureData"]["occurrence"][0]]
+                self.link_names[body_id] = link_name
+
+            if feature["featureType"] == "mateConnector" and feature["featureData"][
+                "name"
+            ].startswith("frame_"):
+                name = "_".join(feature["featureData"]["name"].split("_")[1:])
+                occurrence = feature["featureData"]["occurrence"]
+                T_world_occurrence = self.get_occurrence_transform(occurrence)
+                body_id = self.instance_body[occurrence[0]]
+                T_occurrence_mate = self.cs_to_transformation(
+                    feature["featureData"]["mateConnectorCS"]
+                )
+                T_world_mate = T_world_occurrence @ T_occurrence_mate
+                self.frames.append(Frame(body_id, name, T_world_mate))
+
+        print(success(f"* Found total {len(self.dofs)} degrees of freedom"))
+
+    def build_trees(self):
+        """
+        Perform checks on the produced tree
+        """
+        self.body_in_tree = []
+        for body_id in self.instance_body.values():
+            if body_id != INSTANCE_IGNORE and body_id not in self.body_in_tree:
+                self.build_tree(body_id)
+
+        print(success(f"* Found {len(self.root_nodes)} root nodes:"))
+        for root_node in self.root_nodes:
+            print(success(f"  - {self.body_instance(root_node)['name']}"))
+
+    def build_tree(self, root_node: int):
+        """
+        Building a tree starting a root_node
+        """
+        # Append the root node
+        self.root_nodes.append(root_node)
+
+        # Checking that the graph is actually a tree (no loop)
+        exploring = [root_node]
+        dofs = self.dofs.copy()
+        while len(exploring) > 0:
+            current = exploring.pop()
+            self.body_in_tree.append(current)
+
+            children = []
+            dofs_to_remove = []
+            for dof in dofs:
+                if dof.body1_id == current:
+                    dof.flip(flip_limits=False)
+                    children.append(dof.body2_id)
+                    dofs_to_remove.append(dof)
+                elif dof.body2_id == current:
+                    children.append(dof.body1_id)
+                    dofs_to_remove.append(dof)
+            for dof in dofs_to_remove:
+                dofs.remove(dof)
+
+            self.tree_children[current] = children
+            for child in children:
+                if child in self.body_in_tree:
+                    raise Exception(
+                        "The DOF graph is not a tree, check for loops in your DOFs"
+                    )
+                elif child not in exploring:
+                    exploring.append(child)
+
+    def feature_mating_two_occurrences(self):
+        """
+        Iterate over all valid mating feature with two occurrences
+        """
+        for feature in self.assembly_data["rootAssembly"]["features"]:
+            if feature["featureType"] == "mate" and not feature["suppressed"]:
+                data = feature["featureData"]
+
+                if (
+                    "matedEntities" not in data
+                    or len(data["matedEntities"]) != 2
+                    or len(data["matedEntities"][0]["matedOccurrence"]) == 0
+                    or len(data["matedEntities"][1]["matedOccurrence"]) == 0
+                ):
+                    continue
+
+                occurrence_A = data["matedEntities"][0]["matedOccurrence"][0]
+                occurrence_B = data["matedEntities"][1]["matedOccurrence"][0]
+
+                yield data, occurrence_A, occurrence_B
+
+    def feature_mate_groups(self):
+        """
+        Find mate groups in the assembly
+        """
+        groups = []
+
+        for feature in self.assembly_data["rootAssembly"]["features"]:
+            group = []
+            if feature["featureType"] == "mateGroup" and not feature["suppressed"]:
+                data = feature["featureData"]
+
+                for occurrence in data["occurrences"]:
+                    group.append(occurrence["occurrence"][0])
+            groups.append(group)
+
+        return groups
+
+    def get_feature_by_id(self, feature_id: str):
+        """
+        Find a specific feature by its ID
+        """
+        for feature in self.features["features"]:
+            if feature["message"]["featureId"] == feature_id:
+                return feature
+
+        return None
+
+    def find_relations(self):
+        """
+        Finding relations features in the assembly
+        """
+        for feature in self.features["features"]:
+            if feature["typeName"] == "BTMMateRelation":
+                relation_name = feature["message"]["name"]
+
+                mated_dofs = None
+                ratio = None
+                reverse = None
+                for parameter in feature["message"]["parameters"]:
+                    if parameter["message"]["parameterId"] == "matesQuery":
+                        queries = parameter["message"]["queries"]
+                        if len(queries) == 2:
+                            dof1_feature = self.get_feature_by_id(
+                                queries[0]["message"]["featureId"]
+                            )
+                            dof2_feature = self.get_feature_by_id(
+                                queries[1]["message"]["featureId"]
+                            )
+                            if dof1_feature is not None and dof2_feature is not None:
+                                dof1 = dof1_feature["message"]["name"]
+                                dof2 = dof2_feature["message"]["name"]
+                                if dof1.startswith("dof_") and dof2.startswith("dof_"):
+                                    mated_dofs = [dof1[4:], dof2[4:]]
+                    elif parameter["message"]["parameterId"] == "relationRatio":
+                        ratio = self.read_expression(parameter["message"]["expression"])
+                    elif parameter["message"]["parameterId"] == "reverseDirection":
+                        reverse = parameter["message"]["value"]
+
+                if mated_dofs is not None and ratio is not None and reverse is not None:
+                    if not reverse:
+                        ratio = -ratio
+
+                    print(
+                        success(
+                            f"+ Found relation {relation_name} mating {mated_dofs} with ratio {ratio}"
+                        )
+                    )
+                    if mated_dofs[1] in self.relations:
+                        print(
+                            warning(
+                                f"Multiple relations found with {mated_dofs[1]} as target"
+                            )
+                        )
+
+                    self.relations[mated_dofs[1]] = [mated_dofs[0], ratio]
+
+    def read_parameter_value(self, parameter: str, name: str):
+        """
+        Try to read a parameter value from Onshape
+        """
+
+        # This is an expression
+        if parameter["typeName"] == "BTMParameterNullableQuantity":
+            return self.read_expression(parameter["message"]["expression"])
+        if parameter["typeName"] == "BTMParameterConfigured":
+            message = parameter["message"]
+            parameterValue = self.configuration_parameters[
+                message["configurationParameterId"]
+            ]
+
+            for value in message["values"]:
+                if value["typeName"] == "BTMConfiguredValueByBoolean":
+                    booleanValue = parameterValue == "true"
+                    if value["message"]["booleanValue"] == booleanValue:
+                        return self.read_expression(
+                            value["message"]["value"]["message"]["expression"]
+                        )
+                elif value["typeName"] == "BTMConfiguredValueByEnum":
+                    if value["message"]["enumValue"] == parameterValue:
+                        return self.read_expression(
+                            value["message"]["value"]["message"]["expression"]
+                        )
+                else:
+                    raise Exception(
+                        "Can't read value of parameter {name} configured with {value['typeName']}"
+                    )
+
+            print(error(f"Coud not find the value for {name}"))
+        else:
+            raise Exception(f"Unknown feature type for {name}: {parameter['typeName']}")
+
+    def read_expression(self, expression: str):
+        """
+        Reading an expression from Onshape
+        """
+        return self.expression_parser.eval_expr(expression)
+
+    def get_offset(self, name: str):
+        """
+        Retrieve the offset from current joint position in the assembly
+        Currently, this only works with workspace in the API
+        """
+        if self.matevalues is None:
+            return None
+
+        for entry in self.matevalues["mateValues"]:
+            if entry["mateName"] == name:
+                if "rotationZ" in entry:
+                    return entry["rotationZ"]
+                elif "translationZ" in entry:
+                    return entry["translationZ"]
+                else:
+                    print(warning(f"Unknown offset type for {name}"))
+        return None
+
+    def get_limits(self, joint_type: str, name: str):
+        """
+        Retrieve (low, high) limits for a given joint, if any
+        """
+        enabled = False
+        minimum, maximum = 0, 0
+        for feature in self.features["features"]:
+            # Find coresponding joint
+            if name == feature["message"]["name"]:
+                # Find min and max values
+                for parameter in feature["message"]["parameters"]:
+                    if parameter["message"]["parameterId"] == "limitsEnabled":
+                        enabled = parameter["message"]["value"]
+
+                if enabled:
+                    for parameter in feature["message"]["parameters"]:
+                        if joint_type == Joint.REVOLUTE:
+                            if parameter["message"]["parameterId"] == "limitAxialZMin":
+                                minimum = self.read_parameter_value(parameter, name)
+                            if parameter["message"]["parameterId"] == "limitAxialZMax":
+                                maximum = self.read_parameter_value(parameter, name)
+                        elif joint_type == Joint.PRISMATIC:
+                            if parameter["message"]["parameterId"] == "limitZMin":
+                                minimum = self.read_parameter_value(parameter, name)
+                            if parameter["message"]["parameterId"] == "limitZMax":
+                                maximum = self.read_parameter_value(parameter, name)
+                        elif joint_type == Joint.BALL:
+                            if (
+                                parameter["message"]["parameterId"]
+                                == "limitEulerConeAngleMax"
+                            ):
+                                minimum = 0
+                                maximum = self.read_parameter_value(parameter, name)
+                        else:
+                            print(
+                                warning(
+                                    f"WARNING: Can't read limits for a joint of type {joint_type}"
+                                )
+                            )
+                            print(parameter)
+        if enabled:
+            if joint_type != Joint.BALL:
+                offset = self.get_offset(name)
+                if offset is not None:
+                    minimum -= offset
+                    maximum -= offset
+            return (minimum, maximum)
+        else:
+            if joint_type != Joint.CONTINUOUS:
+                print(
+                    warning(f"WARNING: joint {name} of type {joint_type} has no limits")
+                )
+            return None
+
+    def body_instance(self, body_id: int):
+        """
+        Get the (first) instance associated with a given body
+        """
+        for instance in self.assembly_data["rootAssembly"]["instances"]:
+            if (
+                instance["id"] in self.instance_body
+                and self.instance_body[instance["id"]] == body_id
+            ):
+                return instance
+
+        return None
+
+    def body_occurrences(self, body_id: int):
+        """
+        Retrieve all occurrences associated to a given body id
+        """
+        for occurrence in self.assembly_data["rootAssembly"]["occurrences"]:
+            key = occurrence["path"][0]
+            if key in self.instance_body and self.instance_body[key] == body_id:
+                yield occurrence
+
+    def get_dof(self, body1_id: int, body2_id: int):
+        """
+        Get a DOF for given bodies
+        """
+        for dof in self.dofs:
+            if (dof.body1_id == body1_id and dof.body2_id == body2_id) or (
+                dof.body1_id == body2_id and dof.body2_id == body1_id
+            ):
+                return dof
+
+        raise Exception(f"ERROR: no DOF found between {body1_id} and {body2_id}")

onshape_to_robot/assets/scene.xml

@@ -0,0 +1,24 @@
+<mujoco model="scene">
+    <include file="{robot_filename}" />
+
+    <visual>
+        <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
+        <rgba haze="0.15 0.25 0.35 1" />
+        <global azimuth="160" elevation="-20" />
+    </visual>
+
+    <asset>
+        <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512"
+            height="3072" />
+        <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4"
+            rgb2="0.1 0.2 0.3"
+            markrgb="0.8 0.8 0.8" width="300" height="300" />
+        <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5"
+            reflectance="0.2" />
+    </asset>
+
+    <worldbody>
+        <light pos="0 0 3.5" dir="0 0 -1" directional="true" />
+        <geom name="floor" size="0 0 0.05" pos="0 0 0" type="plane" material="groundplane" />
+    </worldbody>
+</mujoco>

onshape_to_robot/bullet.py

@@ -0,0 +1,71 @@
+def main():
+    import math
+    import sys
+    import os
+    import time
+    import argparse
+    import pybullet as p
+    from .simulation import Simulation
+
+    parser = argparse.ArgumentParser(prog="onshape-to-robot-bullet")
+    parser.add_argument("-f", "--fixed", action="store_true")
+    parser.add_argument("-n", "--no-self-collisions", action="store_true")
+    parser.add_argument("-x", "--x", type=float, default=0)
+    parser.add_argument("-y", "--y", type=float, default=0)
+    parser.add_argument("-z", "--z", type=float, default=0)
+    parser.add_argument("directory")
+    args = parser.parse_args()
+
+    robotPath = args.directory
+    if not robotPath.endswith(".urdf"):
+        robotPath += "/robot.urdf"
+
+    sim = Simulation(
+        robotPath,
+        gui=True,
+        panels=True,
+        fixed=args.fixed,
+        ignore_self_collisions=args.no_self_collisions,
+    )
+    pos, rpy = sim.getRobotPose()
+    _, orn = p.getBasePositionAndOrientation(sim.robot)
+    sim.setRobotPose([pos[0] + args.x, pos[1] + args.y, pos[2] + args.z], orn)
+
+    controls = {}
+    for name in sim.getJoints():
+        if name.endswith("_speed"):
+            controls[name] = p.addUserDebugParameter(name, -math.pi * 3, math.pi * 3, 0)
+        else:
+            infos = sim.getJointsInfos(name)
+            low = -math.pi
+            high = math.pi
+            if "lowerLimit" in infos:
+                low = infos["lowerLimit"]
+            if "upperLimit" in infos:
+                high = infos["upperLimit"]
+            controls[name] = p.addUserDebugParameter(name, low, high, 0)
+
+    lastPrint = 0
+    while True:
+        targets = {}
+        for name in controls.keys():
+            targets[name] = p.readUserDebugParameter(controls[name])
+        sim.setJoints(targets)
+
+        if time.time() - lastPrint > 0.05:
+            lastPrint = time.time()
+            os.system("clear")
+            frames = sim.getFrames()
+            for frame in frames:
+                print(frame)
+                print("- x=%f\ty=%f\tz=%f" % frames[frame][0])
+                print("- r=%f\tp=%f\ty=%f" % frames[frame][1])
+                print("")
+            print("Center of mass:")
+            print(sim.getCenterOfMassPosition())
+
+        sim.tick()
+
+
+if __name__ == "__main__":
+    main()

onshape_to_robot/bullet/plane.obj

@@ -0,0 +1,18 @@
+# Blender v2.66 (sub 1) OBJ File: ''
+# www.blender.org
+mtllib plane.mtl
+o Plane
+v 15.000000 -15.000000 0.000000
+v 15.000000 15.000000 0.000000
+v -15.000000 15.000000 0.000000
+v -15.000000  -15.000000 0.000000
+
+vt 15.000000 0.000000
+vt 15.000000 15.000000
+vt 0.000000 15.000000
+vt 0.000000 0.000000
+
+usemtl Material
+s off
+f 1/1 2/2 3/3
+f 1/1 3/3 4/4

onshape_to_robot/bullet/plane.urdf

@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="plane">
+  <link name="planeLink">
+  <contact>
+      <lateral_friction value="1"/>
+  </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".0"/>
+       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="plane.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -5"/>
+      <geometry>
+	 	<box size="30 30 10"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

onshape_to_robot/clear_cache.py

@@ -0,0 +1,16 @@
+"""Clear the onshape-to-robot cache."""
+
+
+def main():
+    import shutil
+
+    from .onshape_api.cache import get_cache_path
+
+    """Clear the onshape-to-robot cache."""
+    cache_dir = get_cache_path()
+    print("Removing cache directory: {}".format(cache_dir))
+    shutil.rmtree(cache_dir, ignore_errors=True)
+
+
+if __name__ == "__main__":
+    main()

onshape_to_robot/config.py

@@ -0,0 +1,209 @@
+from __future__ import annotations
+import numpy as np
+import re
+import os
+import commentjson as json
+
+
+class Config:
+    def __init__(self, robot_path: str, safe: bool = False):
+        self.safe: bool = safe
+        self.config_file: str = robot_path
+
+        if os.path.isdir(robot_path):
+            self.config_file += os.path.sep + "config.json"
+
+        # Loading JSON configuration
+        if not os.path.exists(self.config_file):
+            raise Exception(f"ERROR: The file {self.config_file} can't be found")
+        with open(self.config_file, "r", encoding="utf8") as stream:
+            self.config: dict = json.load(stream)
+
+        # Loaded processors
+        self.processors: list = []
+
+        self.read_configuration()
+
+        # Output directory, making it if it doesn't exists
+        self.output_directory: str = os.path.dirname(os.path.abspath(self.config_file))
+
+        if self.robot_name is None:
+            self.robot_name = os.path.dirname(os.path.abspath(self.config_file)).split(
+                "/"
+            )[-1]
+
+        try:
+            os.makedirs(self.output_directory)
+        except OSError:
+            pass
+
+    def to_camel_case(self, snake_str: str) -> str:
+        """
+        Converts a string to camel case
+        """
+        components = snake_str.split("_")
+        return components[0] + "".join(x.title() for x in components[1:])
+
+    def get(self, name: str, default=None, required: bool = True, values_list=None):
+        """
+        Gets an entry from the configuration
+
+        Args:
+            name (str): entry name
+            default: default fallback value if the entry is not present. Defaults to None.
+            required (bool, optional): whether the configuration entry is required. Defaults to False.
+            values_list: list of allowed values. Defaults to None.
+        """
+        camel_name = self.to_camel_case(name)
+
+        if name in self.config or camel_name in self.config:
+            if name in self.config:
+                value = self.config[name]
+            else:
+                value = self.config[camel_name]
+
+            if values_list is not None and value not in values_list:
+                raise Exception(
+                    f"Value for {name} should be onf of: {','.join(values_list)}"
+                )
+            return value
+        elif required and default is None:
+            raise Exception(f"ERROR: missing required key {name} in config")
+
+        return default
+
+    def printable_version(self) -> str:
+        if self.url is not None:
+            return self.url
+        else:
+            version = f"document_id: {self.document_id}"
+            if self.version_id:
+                version += f" / version_id: {self.version_id}"
+            elif self.workspace_id:
+                version += f" / workspace_id: {self.workspace_id}"
+
+            return version
+
+    def parse_url(self):
+        pattern = "https://(.*)/(.*)/([wv])/(.*)/e/(.*)"
+        match = re.match(pattern, self.url)
+
+        if match is None:
+            raise Exception(f"Invalid URL: {self.url}")
+
+        match_groups = match.groups()
+        self.document_id = match_groups[1]
+        if match_groups[2] == "w":
+            self.workspace_id = match_groups[3]
+        elif match_groups[2] == "v":
+            self.version_id = match_groups[3]
+        self.element_id = match_groups[4]
+
+    def asset_path(self, asset_name: str) -> str:
+        return f"{self.output_directory}/{self.assets_directory}/{asset_name}"
+
+    def read_configuration(self):
+        """
+        Load and check configuration entries
+        """
+
+        # Robot name
+        self.robot_name: str = self.get("robot_name", None, required=False)
+        self.output_filename: str = self.get("output_filename", "robot")
+        # Securing filename
+        self.output_filename = "".join(
+            c for c in self.output_filename if c.isalnum() or c in ("_", "-")
+        ).rstrip()
+        self.assets_directory: str = self.get("assets_directory", "assets")
+
+        # Main settings
+        self.document_id: str = self.get("document_id", required=False)
+        self.version_id: str | None = self.get("version_id", required=False)
+        self.workspace_id: str | None = self.get("workspace_id", required=False)
+        self.element_id: str | None = self.get("element_id", required=False)
+
+        if self.version_id and self.workspace_id:
+            raise Exception("You can't specify workspace_id and version_id")
+
+        self.url: str = self.get("url", None, required=False)
+        if self.url is not None:
+            self.parse_url()
+
+        if self.url is None and self.document_id is None:
+            raise Exception("You need to specify either a url or a document_id")
+
+        self.draw_frames: bool = self.get("draw_frames", False)
+
+        self.assembly_name: str = self.get("assembly_name", required=False)
+        self.output_format: str = self.get("output_format")
+        self.configuration: str | dict = self.get("configuration", "default")
+        self.ignore_limits: bool = self.get("ignore_limits", False)
+
+        if isinstance(self.configuration, dict):
+            self.configuration = ";".join(
+                [f"{k}={v}" for k, v in self.configuration.items()]
+            )
+
+        # Joint specs
+        self.joint_properties: dict = self.get("joint_properties", {})
+        self.geom_properties: dict = self.get("geom_properties", {})
+        self.no_dynamics: bool = self.get("no_dynamics", False)
+
+        # Ignore / whitelists
+        self.ignore: list[str] = self.get("ignore", {})
+        if isinstance(self.ignore, list):
+            self.ignore = {entry: "all" for entry in self.ignore}
+
+        # Color override
+        self.color: str | None = self.get("color", required=False)
+
+        # Post-import commands
+        self.post_import_commands: list[str] = self.get("post_import_commands", [])
+
+        # Whether to include configuration suffix in part names
+        self.include_configuration_suffix: bool = self.get(
+            "include_configuration_suffix", True
+        )
+
+        # Number of decimals to keep for small numbers
+        self.round_decimals = self.get("round_decimals", 12)
+
+        # Loading processors
+        from . import processors
+
+        loaded_modules = {}
+        processors_list: list[str] | None = self.get("processors", None, required=False)
+        if processors_list is None or self.safe:
+            self.processors = [
+                processor(self)
+                for processor in processors.default_processors
+                if (processor.is_safe or not self.safe)
+            ]
+        else:
+            for entry in processors_list:
+                parts = entry.split(":")
+
+                if len(parts) == 1:
+                    processor = eval(f"processors.{entry}")
+                else:
+                    module, cls = parts
+                    if module not in loaded_modules:
+                        loaded_modules[module] = __import__(module, fromlist=[cls])
+                    processor = getattr(loaded_modules[module], cls)
+
+                if processor is None:
+                    raise Exception(f"ERROR: Processor {entry} not found")
+
+                self.processors.append(processor(self))
+
+    def round(self, object: float | list | tuple | np.ndarray):
+        """
+        Round the given number or list of numbers using the configuration decimals
+        """
+        if isinstance(object, float):
+            return round(object, self.round_decimals)
+        elif isinstance(object, np.ndarray):
+            return object.round(self.round_decimals)
+        else:
+            original_type = type(object)
+            return original_type(np.array(object).round(self.round_decimals))

onshape_to_robot/csg.py

@@ -0,0 +1,123 @@
+import json
+import re
+import os
+import numpy as np
+
+"""
+These functions are responsible for parsing CSG files (constructive solid geometry), which are files
+produced by OpenSCAD, containing no loop, variables etc.
+"""
+
+
+def multmatrix_parse(parameters):
+    matrix = np.array(json.loads(parameters), dtype=float)
+    matrix[0, 3] /= 1000.0
+    matrix[1, 3] /= 1000.0
+    matrix[2, 3] /= 1000.0
+    return matrix
+
+
+def cube_parse(parameters, dilatation):
+    results = re.findall(r'^size = (.+), center = (.+)$', parameters)
+    if len(results) != 1:
+        print("! Can't parse CSG cube parameters: "+parameters)
+        exit()
+    extra = np.array([dilatation]*3)
+    return (extra + np.array(json.loads(results[0][0]), dtype=float)/1000.0), results[0][1] == 'true'
+
+
+def cylinder_parse(parameters, dilatation):
+    results = re.findall(
+        r'h = (.+), r1 = (.+), r2 = (.+), center = (.+)', parameters)
+    if len(results) != 1:
+        print("! Can't parse CSG cylinder parameters: "+parameters)
+        exit()
+    result = results[0]
+    extra = np.array([dilatation/2, dilatation])
+    return (extra + np.array([result[0], result[1]], dtype=float)/1000.0), result[3] == 'true'
+
+
+def sphere_parse(parameters, dilatation):
+    results = re.findall(r'r = (.+)$', parameters)
+    if len(results) != 1:
+        print("! Can't parse CSG sphere parameters: "+parameters)
+        exit()
+    return dilatation + float(results[0])/1000.0
+
+
+def extract_node_parameters(line):
+    line = line.strip()
+    parts = line.split('(', 1)
+    node = parts[0]
+    parameters = parts[1]
+    if parameters[-1] == ';':
+        parameters = parameters[:-2]
+    if parameters[-1] == '{':
+        parameters = parameters[:-3]
+    return node, parameters
+
+
+def T(x, y, z):
+    m = np.eye(4)
+    m.T[3, :3] = [x, y, z]
+
+    return m
+
+
+def parse_csg(data, dilatation):
+    shapes = []
+    lines = data.split("\n")
+    matrices = []
+    for line in lines:
+        line = line.strip()
+        if line != '':
+            if line[-1] == '{':
+                node, parameters = extract_node_parameters(line)
+                if node == 'multmatrix':
+                    matrix = multmatrix_parse(parameters)
+                else:
+                    matrix = np.array(np.identity(4))
+                matrices.append(matrix)
+            elif line[-1] == '}':
+                matrices.pop()
+            else:
+                node, parameters = extract_node_parameters(line)
+                transform = np.array(np.identity(4))
+                for entry in matrices:
+                    transform = transform@entry
+                if node == 'cube':
+                    size, center = cube_parse(parameters, dilatation)
+                    if not center:
+                        transform = transform @ \
+                            T(size[0]/2.0, size[1]/2.0, size[2]/2.0)
+                    shapes.append({
+                        'type': 'cube',
+                        'parameters': size,
+                        'transform': transform
+                    })
+                if node == 'cylinder':
+                    size, center = cylinder_parse(parameters, dilatation)
+                    if not center:
+                        transform = transform @ T(0, 0, size[0]/2.0)
+                    shapes.append({
+                        'type': 'cylinder',
+                        'parameters': size,
+                        'transform': transform
+                    })
+                if node == 'sphere':
+                    shapes.append({
+                        'type': 'sphere',
+                        'parameters': sphere_parse(parameters, dilatation),
+                        'transform': transform
+                    })
+    return shapes
+
+
+def process(filename, dilatation):
+    tmp_data = os.getcwd()+'/_tmp_data.csg'
+    os.system('openscad '+filename+' -o '+tmp_data)
+    with open(tmp_data, "r", encoding="utf-8") as stream:
+        data = stream.read()
+    os.system('rm '+tmp_data)
+
+    return parse_csg(data, dilatation)

onshape_to_robot/edit_shape.py

@@ -0,0 +1,26 @@
+def main():
+    import os
+    import sys
+
+    if len(sys.argv) < 2:
+        print("Usage: onshape-to-robot-edit-shape {STL file}")
+    else:
+        fileName = sys.argv[1]
+        parts = fileName.split(".")
+        parts[-1] = "scad"
+        fileName = ".".join(parts)
+        if not os.path.exists(fileName):
+            scad = '% scale(1000) import("' + os.path.basename(sys.argv[1]) + '");\n'
+            scad += "\n"
+            scad += "// Append pure shapes (cube, cylinder and sphere), e.g:\n"
+            scad += "// cube([10, 10, 10], center=true);\n"
+            scad += "// cylinder(r=10, h=10, center=true);\n"
+            scad += "// sphere(10);\n"
+            with open(fileName, "w", encoding="utf-8") as stream:
+                stream.write(scad)
+        directory = os.path.dirname(fileName)
+        os.system("cd " + directory + "; openscad " + os.path.basename(fileName))
+
+
+if __name__ == "__main__":
+    main()

onshape_to_robot/export.py

@@ -0,0 +1,117 @@
+def main():
+    import os
+    import sys
+    import pickle
+    import argparse
+    from dotenv import load_dotenv, find_dotenv
+    from .config import Config
+    from .message import error, info
+    from .robot_builder import RobotBuilder
+    from .exporter_urdf import ExporterURDF
+    from .exporter_sdf import ExporterSDF
+    from .exporter_mujoco import ExporterMuJoCo
+
+    """
+    This is the entry point of the export script, i.e the "onshape-to-robot" command.
+    """
+    load_dotenv(find_dotenv(usecwd=True))
+
+    def get_version():
+        # Get version from package
+        try:
+            from importlib.metadata import version, PackageNotFoundError
+
+            return version("onshape-to-robot")
+        except PackageNotFoundError:
+            return "unknown"
+
+    try:
+        # Retrieving robot path
+        arg_parser = argparse.ArgumentParser()
+        arg_parser.add_argument(
+            "robot_path", type=str, help="Path to the robot directory"
+        )
+        arg_parser.add_argument(
+            "--version", action="version", version=f"onshape-to-robot {get_version()}"
+        )
+        arg_parser.add_argument(
+            "--retrieve",
+            action="store_true",
+            help="Only retrieve data and produce robot.pkl",
+        )
+        arg_parser.add_argument(
+            "--save-pickle",
+            action="store_true",
+            help="Save the robot data to robot.pkl",
+        )
+        arg_parser.add_argument(
+            "--convert",
+            action="store_true",
+            help="Only convert robot.pkl to the desired format",
+        )
+        arg_parser.add_argument(
+            "--safe",
+            action="store_true",
+            help="Disable features involving custom commands or imports",
+        )
+        args = arg_parser.parse_args()
+
+        print(info(f"* onshape-to-robot version {get_version()}"))
+
+        robot_path: str = args.robot_path
+
+        # Loading configuration
+        config = Config(robot_path, safe=args.safe)
+
+        # Building exporter beforehand, so that the configuration gets checked
+        if config.output_format == "urdf":
+            exporter = ExporterURDF(config)
+        elif config.output_format == "sdf":
+            exporter = ExporterSDF(config)
+        elif config.output_format == "mujoco":
+            exporter = ExporterMuJoCo(config)
+        else:
+            raise Exception(f"Unsupported output format: {config.output_format}")
+
+        if not args.convert:
+            # Building the robot
+            robot_builder = RobotBuilder(config)
+            robot = robot_builder.robot
+
+        # Can be used for debugging
+        pkl_filename = config.output_directory + "/robot.pkl"
+        if args.retrieve or args.save_pickle:
+            pickle.dump(robot, open(pkl_filename, "wb"))
+            print(info(f"* Robot data saved to {pkl_filename}"))
+
+        if args.convert:
+            print(info(f"* Loading robot data from {pkl_filename}"))
+            robot = pickle.load(open(pkl_filename, "rb"))
+
+        if not args.retrieve:
+            # Applying processors
+            for processor in config.processors:
+                processor.process(robot)
+
+            exporter.write_xml(
+                robot,
+                config.output_directory
+                + "/"
+                + config.output_filename
+                + "."
+                + exporter.ext,
+            )
+
+            if not args.safe:
+                # Executing post-import commands
+                for command in config.post_import_commands:
+                    print(info(f"* Running command: {command}"))
+                    os.system(command)
+
+    except Exception as e:
+        print(error(f"ERROR: {e}"))
+        raise e
+
+
+if __name__ == "__main__":
+    main()

onshape_to_robot/exporter.py

@@ -0,0 +1,40 @@
+import os
+from .message import success
+import xml.dom.minidom
+from .robot import Robot
+
+
+class Exporter:
+    def __init__(self):
+        self.xml: str = ""
+        self.ext: str = "xml"
+
+    def build(self):
+        raise Exception("This exporter should implement build() method")
+
+    def get_xml(self, robot: Robot) -> str:
+        self.build(robot)
+        return self.xml
+    
+    def remove_empty_text_nodes(self, node: xml.dom.Node):
+        to_delete = []
+
+        for child_node in node.childNodes:
+            if isinstance(child_node, xml.dom.minidom.Text):
+                child_node.data = child_node.data.strip()
+                if child_node.data == "":
+                    to_delete.append(child_node)
+            else:
+                self.remove_empty_text_nodes(child_node)
+
+        for child_node in to_delete:
+            node.childNodes.remove(child_node)
+
+    def write_xml(self, robot: Robot, filename: str) -> str:
+        with open(filename, "w") as file:
+            self.build(robot)
+            dom = xml.dom.minidom.parseString(self.xml)
+            self.remove_empty_text_nodes(dom)
+            xml_output = dom.toprettyxml(indent="  ")
+            file.write(xml_output)
+            print(success(f"* Writing {os.path.basename(filename)}"))

onshape_to_robot/exporter_mujoco.py

@@ -0,0 +1,414 @@
+from __future__ import annotations
+import numpy as np
+import os
+import fnmatch
+from .message import success, warning, info
+from .robot import Robot, Link, Part, Joint, Closure
+from .config import Config
+from .geometry import Box, Cylinder, Sphere, Mesh, Shape
+from .exporter import Exporter
+from .exporter_utils import xml_escape, rotation_matrix_to_rpy
+from transforms3d.quaternions import mat2quat
+
+
+class ExporterMuJoCo(Exporter):
+    def __init__(self, config: Config | None = None):
+        super().__init__()
+        self.config: Config = config
+
+        self.no_dynamics: bool = False
+        self.additional_xml: str = ""
+        self.meshes: list = []
+        self.materials: dict = {}
+
+        if config is not None:
+            self.equalities = self.config.get("equalities", {})
+            self.no_dynamics = config.no_dynamics
+            additional_xml_file = config.get("additional_xml", None, required=False)
+            if isinstance(additional_xml_file, str):
+                self.add_additional_xml(additional_xml_file)
+            elif isinstance(additional_xml_file, list):
+                for filename in additional_xml_file:
+                    self.add_additional_xml(filename)
+
+    def add_additional_xml(self, xml_file: str):
+        self.additional_xml += f"<!-- Additional {xml_file} -->"
+        with open(self.config.output_directory + "/" + xml_file, "r") as file:
+            self.additional_xml += file.read()
+
+    def append(self, line: str):
+        self.xml += line
+
+    def build(self, robot: Robot):
+        self.xml = ""
+        self.append('<?xml version="1.0" ?>')
+        self.append("<!-- Generated using onshape-to-robot -->")
+        if self.config:
+            self.append(f"<!-- Onshape {self.config.printable_version()} -->")
+        self.append(f'<mujoco model="{robot.name}">')
+        self.append(
+            f'<compiler angle="radian" meshdir="{self.config.assets_directory}" autolimits="true" />'
+        )
+
+        # Boilerplate
+        self.default_class = robot.name
+        self.append("<default>")
+        self.append(f'<default class="{self.default_class}">')
+        self.append('<joint frictionloss="0.1" armature="0.005"/>')
+        self.append('<position kp="50" dampratio="1" />')
+        self.append('<default class="visual">')
+        self.append('<geom type="mesh" contype="0" conaffinity="0" group="2"/>')
+        self.append("</default>")
+        self.append('<default class="collision">')
+        self.append('<geom group="3"/>')
+        self.append("</default>")
+        self.append("</default>")
+        self.append("</default>")
+
+        if self.additional_xml:
+            self.append(self.additional_xml)
+
+        # Adding robot links
+        self.append("<worldbody>")
+        for base_link in robot.base_links:
+            self.add_link(robot, base_link)
+        self.append("</worldbody>")
+
+        # Asset (mesh & materials)
+        self.append("<asset>")
+        for mesh_file in set(self.meshes):
+            self.append(f'<mesh file="{mesh_file}" />')
+        for material_name, color in self.materials.items():
+            color_str = "%g %g %g %g" % self.config.round(tuple(color))
+            self.append(f'<material name="{material_name}" rgba="{color_str}" />')
+        self.append("</asset>")
+
+        # Adding actuators
+        self.add_actuators(robot)
+
+        # Adding equalities (loop closure)
+        self.add_equalities(robot)
+
+        self.append("</mujoco>")
+
+        return self.xml
+
+    def add_actuators(self, robot: Robot):
+        self.append("<actuator>")
+
+        for joint in robot.joints:
+            if joint.joint_type == "fixed":
+                continue
+
+            # Suppose joints with relation equality is not actuated, unless specified
+            guess_actuated = joint.relation is None
+
+            if (
+                joint.properties.get("actuated", guess_actuated)
+                and joint.joint_type != Joint.BALL
+            ):
+                type = joint.properties.get("type", "position")
+                actuator_class = joint.properties.get("class", self.default_class)
+                actuator: str = f'<{type} class="{actuator_class}" name="{joint.name}" joint="{joint.name}" '
+
+                for key in "kp", "kv", "dampratio":
+                    if key in joint.properties:
+                        actuator += f'{key}="{joint.properties[key]}" '
+
+                if "forcerange" in joint.properties:
+                    actuator += f'forcerange="-{joint.properties["forcerange"]} {joint.properties["forcerange"]}" '
+
+                joint_limits = joint.properties.get("limits", joint.limits)
+                limits_are_set = joint.properties.get("limits", False) != False
+                if joint_limits and (type == "position" or limits_are_set):
+                    if joint.properties.get("range", True) and type == "position":
+                        actuator += f'inheritrange="1" '
+                    else:
+                        actuator += f'ctrlrange="{joint_limits[0]} {joint_limits[1]}" '
+
+                actuator += "/>"
+                self.append(actuator)
+
+        self.append("</actuator>")
+
+    def get_equality_attributes(self, closure: Closure) -> str:
+        all_attributes = {}
+        for name, attributes in self.equalities.items():
+            if fnmatch.fnmatch(closure.frame1, name) and fnmatch.fnmatch(
+                closure.frame2, name
+            ):
+                all_attributes.update(attributes)
+
+        if len(all_attributes) > 0:
+            return (
+                " ".join([f'{key}="{value}"' for key, value in all_attributes.items()])
+                + " "
+            )
+
+        return ""
+
+    def add_equalities(self, robot: Robot):
+        self.append("<equality>")
+        for closure in robot.closures:
+            attributes = self.get_equality_attributes(closure)
+
+            if closure.closure_type == Closure.FIXED:
+                self.append(
+                    f'<weld site1="{closure.frame1}" site2="{closure.frame2}" {attributes}/>'
+                )
+            elif closure.closure_type == Closure.REVOLUTE:
+                self.append(
+                    f'<connect site1="{closure.frame1}" site2="{closure.frame2}" {attributes}/>'
+                )
+            elif closure.closure_type == Closure.BALL:
+                self.append(
+                    f'<connect site1="{closure.frame1}" site2="{closure.frame2}" {attributes}/>'
+                )
+            else:
+                print(
+                    warning(
+                        f"Closure type: {closure.closure_type} is not supported with MuJoCo equality constraints"
+                    )
+                )
+
+        for joint in robot.joints:
+            if joint.relation is not None:
+                self.append(
+                    f'<joint joint1="{joint.name}" joint2="{joint.relation.source_joint}" polycoef="0 {joint.relation.ratio} 0 0 0" />'
+                )
+
+        self.append("</equality>")
+
+    def add_inertial(self, mass: float, com: np.ndarray, inertia: np.ndarray):
+        # Ensuring epsilon masses and inertias
+        mass = max(1e-9, mass)
+        inertia[0, 0] = max(1e-9, inertia[0, 0])
+        inertia[1, 1] = max(1e-9, inertia[1, 1])
+        inertia[2, 2] = max(1e-9, inertia[2, 2])
+
+        # Populating body inertial properties
+        # https://mujoco.readthedocs.io/en/stable/XMLreference.html#body-inertial
+        inertial: str = "<inertial "
+        inertial += 'pos="%g %g %g" ' % self.config.round(tuple(com))
+        inertial += 'mass="%g" ' % self.config.round(mass)
+        inertial += 'fullinertia="%g %g %g %g %g %g" ' % self.config.round(
+            (
+                inertia[0, 0],
+                inertia[1, 1],
+                inertia[2, 2],
+                inertia[0, 1],
+                inertia[0, 2],
+                inertia[1, 2],
+            )
+        )
+        inertial += " />"
+        self.append(inertial)
+
+    def add_mesh(self, part: Part, class_: str, T_world_link: np.ndarray, mesh: Mesh):
+        """
+        Add a mesh node (e.g. STL) to the MuJoCo file
+        """
+        # Retrieving mesh file and material name
+        mesh_file = os.path.relpath(self.config.output_directory + "/" + mesh.filename, self.config.asset_path(""))
+        mesh_file_no_ext = ".".join(os.path.basename(mesh_file).split(".")[:-1])
+        material_name = mesh_file_no_ext + "_material"
+
+        # Relative frame
+        T_link_part = np.linalg.inv(T_world_link) @ part.T_world_part
+
+        # Adding the geom node
+        geom = f'<geom type="mesh" class="{class_}" '
+        geom += self.pos_quat(T_link_part) + " "
+        geom += f'mesh="{xml_escape(mesh_file_no_ext)}" '
+        geom += f'material="{xml_escape(material_name)}" '
+
+        # Apply properties based on class (visual or collision)
+        properties = mesh.visual_properties if class_ == "visual" else mesh.collision_properties
+        for key, value in properties.items():
+            geom += f'{key}="{xml_escape(str(value))}" '
+
+        geom += " />"
+
+        # Adding the mesh and material to appear in the assets section
+        self.meshes.append(mesh_file)
+        self.materials[material_name] = mesh.color
+
+        self.append(geom)
+
+    def add_shape(
+        self, part: Part, class_: str, T_world_link: np.ndarray, shape: Shape
+    ):
+        """
+        Add pure shape geometry.
+        """
+        geom = f'<geom class="{class_}" '
+
+        T_link_shape = (
+            np.linalg.inv(T_world_link) @ part.T_world_part @ shape.T_part_shape
+        )
+        geom += self.pos_quat(T_link_shape) + " "
+
+        if isinstance(shape, Box):
+            geom += 'type="box" size="%g %g %g" ' % self.config.round(tuple(shape.size / 2))
+        elif isinstance(shape, Cylinder):
+            geom += 'type="cylinder" size="%g %g" ' % (
+                shape.radius,
+                shape.length / 2,
+            )
+        elif isinstance(shape, Sphere):
+            geom += 'type="sphere" size="%g" ' % self.config.round(shape.radius)
+
+        if class_ == "visual":
+            material_name = f"{part.name}_material"
+            self.materials[material_name] = shape.color
+            geom += f'material="{xml_escape(material_name)}" '
+
+        # Apply properties based on class (visual or collision)
+        properties = shape.visual_properties if class_ == "visual" else shape.collision_properties
+        for key, value in properties.items():
+            geom += f'{key}="{xml_escape(str(value))}" '
+
+        geom += " />"
+        self.append(geom)
+
+    def add_geometries(self, part: Part, T_world_link: np.ndarray):
+        """
+        Add a part geometries
+        """
+        for shape in part.shapes:
+            if shape.visual:
+                self.add_shape(part, "visual", T_world_link, shape)
+            if shape.collision:
+                self.add_shape(part, "collision", T_world_link, shape)
+
+        for mesh in part.meshes:
+            if mesh.visual:
+                self.add_mesh(part, "visual", T_world_link, mesh)
+            if mesh.collision:
+                self.add_mesh(part, "collision", T_world_link, mesh)
+
+    def add_joint(self, joint: Joint):
+        self.append(f"<!-- Joint from {joint.parent.name} to {joint.child.name} -->")
+        if joint.joint_type == "fixed":
+            self.append(f'<!-- Joint is "fixed", skipping it -->')
+            return
+
+        joint_xml: str = "<joint "
+        joint_xml += 'axis="%g %g %g" ' % self.config.round(tuple(joint.axis))
+        joint_xml += f'name="{joint.name}" '
+        if joint.joint_type == Joint.REVOLUTE:
+            joint_xml += 'type="hinge" '
+        elif joint.joint_type == Joint.PRISMATIC:
+            joint_xml += 'type="slide" '
+        elif joint.joint_type == Joint.BALL:
+            joint_xml += 'type="ball" '
+        else:
+            print(warning(f"Unknown joint type: {joint.joint_type}"))
+
+        joint_limits = joint.properties.get("limits", joint.limits)
+        if joint_limits is not None and joint.properties.get("range", True):
+            joint_xml += f'range="{joint_limits[0]} {joint_limits[1]}" '
+
+        for key in (
+            "class",
+            "frictionloss",
+            "armature",
+            "damping",
+            "stiffness",
+        ):
+            if key in joint.properties:
+                joint_xml += f'{key}="{joint.properties[key]}" '
+
+        joint_xml += " />"
+        self.append(joint_xml)
+
+    def add_frame(
+        self,
+        frame: str,
+        T_world_link: np.ndarray,
+        T_world_frame: np.ndarray,
+        group: int = 0,
+    ):
+        self.append(f"<!-- Frame {frame} -->")
+        T_link_frame = np.linalg.inv(T_world_link) @ T_world_frame
+
+        site: str = f'<site group="{group}" name="{frame}" '
+        site += self.pos_quat(T_link_frame) + " "
+        site += " />"
+        self.append(site)
+
+    def add_link(
+        self,
+        robot: Robot,
+        link: Link,
+        parent_joint: Joint | None = None,
+        T_world_parent: np.ndarray = np.eye(4),
+    ):
+        """
+        Adds a link recursively to the URDF file
+        """
+        if parent_joint is None:
+            T_world_link = np.eye(4)
+        else:
+            T_world_link = parent_joint.T_world_joint
+
+        childclass = ""
+        if parent_joint is None:
+            childclass = f'childclass="{self.default_class}" '
+        self.append(f"<!-- Link {link.name} -->")
+        T_parent_link = np.linalg.inv(T_world_parent) @ T_world_link
+        self.append(
+            f'<body name="{link.name}" {self.pos_quat(T_parent_link)} {childclass}>'
+        )
+
+        if parent_joint is None:
+            if not link.fixed:
+                self.append(f'<freejoint name="{link.name}_freejoint" />')
+        else:
+            self.add_joint(parent_joint)
+
+        # Adding inertial properties
+        mass, com, inertia = link.get_dynamics(T_world_link)
+        self.add_inertial(mass, com, inertia)
+
+        # Adding geometry objects
+        for part in link.parts:
+            self.append(f"<!-- Part {part.name} -->")
+            self.add_geometries(part, T_world_link)
+
+        # Adding frames attached to current link
+        for frame, T_world_frame in link.frames.items():
+            self.add_frame(frame, T_world_link, T_world_frame, group=3)
+
+        # Adding joints and children links
+        for joint in robot.get_link_joints(link):
+            self.add_link(robot, joint.child, joint, T_world_link)
+
+        self.append("</body>")
+
+    def pos_quat(self, matrix: np.ndarray) -> str:
+        """
+        Turn a transformation matrix into 'pos="..." quat="..."' attributes
+        """
+        pos = matrix[:3, 3]
+        quat = mat2quat(matrix[:3, :3])
+        xml = 'pos="%g %g %g" quat="%g %g %g %g"' % self.config.round((*pos, *quat))
+
+        return xml
+
+    def write_xml(self, robot: Robot, filename: str) -> str:
+        super().write_xml(robot, filename)
+
+        dirname = os.path.dirname(filename)
+        scene_filename = dirname + "/scene.xml"
+        if not os.path.exists(scene_filename):
+            scene_xml: str = (
+                os.path.dirname(os.path.realpath(__file__)) + "/assets/scene.xml"
+            )
+            scene_xml = open(scene_xml, "r").read()
+            scene_xml = scene_xml.format(robot_filename=os.path.basename(filename))
+            with open(scene_filename, "w") as file:
+                file.write(scene_xml)
+                print(success(f"* Writing scene.xml"))
+        else:
+            print(info(f"* scene.xml already exists, not over-writing it"))

onshape_to_robot/exporter_sdf.py

@@ -0,0 +1,338 @@
+from __future__ import annotations
+import numpy as np
+import os
+from .message import success
+from .robot import Robot, Link, Part, Joint
+from .config import Config
+from .geometry import Box, Cylinder, Sphere, Mesh, Shape
+from .exporter import Exporter
+from .exporter_utils import xml_escape, rotation_matrix_to_rpy
+
+MODEL_CONFIG_XML = """
+<?xml version="1.0" ?>
+<model>
+    <name>%s</name>
+    <version>1.0</version>
+    <sdf version="1.7">%s</sdf>
+    <author>
+        <name></name>
+        <email></email>
+    </author>
+    <description></description>
+</model>
+"""
+
+
+class ExporterSDF(Exporter):
+    def __init__(self, config: Config | None = None):
+        super().__init__()
+        self.config: Config = config
+
+        self.ext: str = "sdf"
+        self.no_dynamics: bool = False
+        self.additional_xml: str = ""
+
+        if config is not None:
+            self.no_dynamics = config.no_dynamics
+            additional_xml_file = config.get("additional_xml", None, required=False)
+            if isinstance(additional_xml_file, str):
+                self.add_additional_xml(additional_xml_file)
+            elif isinstance(additional_xml_file, list):
+                for filename in additional_xml_file:
+                    self.add_additional_xml(filename)
+
+    def add_additional_xml(self, xml_file: str):
+        self.additional_xml += f"<!-- Additional {xml_file} -->"
+        with open(self.config.output_directory + "/" + xml_file, "r") as file:
+            self.additional_xml += file.read()
+
+    def append(self, line: str):
+        self.xml += line
+
+    def build(self, robot: Robot):
+        self.xml = ""
+        self.append('<?xml version="1.0" ?>')
+        self.append("<!-- Generated using onshape-to-robot -->")
+        if self.config:
+            self.append(f"<!-- Onshape {self.config.printable_version()} -->")
+        self.append('<sdf version="1.7">')
+        self.append(f'<model name="{xml_escape(robot.name)}">')
+
+        for base_link in robot.base_links:
+            self.add_link(robot, base_link)
+
+        if self.additional_xml:
+            self.append(self.additional_xml)
+
+        self.append("</model>")
+        self.append("</sdf>")
+
+        return self.xml
+
+    def add_inertial(
+        self, mass: float, com: np.ndarray, inertia: np.ndarray, frame: str = ""
+    ):
+        # Unless "no_dynamics" is set, we make sure that mass and inertia
+        # are not zero
+        if not self.no_dynamics:
+            mass = max(1e-9, mass)
+            inertia[0, 0] = max(1e-9, inertia[0, 0])
+            inertia[1, 1] = max(1e-9, inertia[1, 1])
+            inertia[2, 2] = max(1e-9, inertia[2, 2])
+
+        self.append("<inertial>")
+        self.append(
+            '<pose>%g %g %g 0 0 0</pose>'
+            % self.config.round((
+                com[0],
+                com[1],
+                com[2],
+            ))
+        )
+        self.append("<mass>%g</mass>" % self.config.round(mass))
+        self.append(
+            "<inertia><ixx>%g</ixx><ixy>%g</ixy><ixz>%g</ixz><iyy>%g</iyy><iyz>%g</iyz><izz>%g</izz></inertia>"
+            % self.config.round((
+                inertia[0, 0],
+                inertia[0, 1],
+                inertia[0, 2],
+                inertia[1, 1],
+                inertia[1, 2],
+                inertia[2, 2],
+            ))
+        )
+        self.append("</inertial>")
+
+    def append_material(self, color: np.ndarray):
+        self.append(f"<material>")
+        self.append("<ambient>%g %g %g %g</ambient>" % self.config.round((color[0], color[1], color[2], color[3])))
+        self.append("<diffuse>%g %g %g %g</diffuse>" % self.config.round((color[0], color[1], color[2], color[3])))
+        self.append("<specular>0.1 0.1 0.1 1</specular>")
+        self.append("<emissive>0 0 0 0</emissive>")
+        self.append("</material>")
+
+    def add_mesh(
+        self,
+        link: Link,
+        part: Part,
+        node: str,
+        T_world_link: np.ndarray,
+        mesh: Mesh,
+        mesh_n: int,
+    ):
+        """
+        Add a mesh node (e.g. STL) to the SDF file
+        """
+        self.append(f'<{node} name="{part.name}_{node}_mesh_{mesh_n}">')
+
+        T_link_part = np.linalg.inv(T_world_link) @ part.T_world_part
+        self.append(self.pose(T_link_part, relative_to=link.name))
+
+        mesh_file = mesh.filename
+
+        self.append("<geometry>")
+        self.append(
+            f"<mesh><uri>model://{self.config.robot_name}/{xml_escape(mesh_file)}</uri></mesh>"
+        )
+        self.append("</geometry>")
+
+        if node == "visual":
+            self.append_material(mesh.color)
+
+        # Apply properties based on node type (visual or collision)
+        properties = mesh.visual_properties if node == "visual" else mesh.collision_properties
+        for key, value in properties.items():
+            self.append(f'<{key}>{xml_escape(str(value))}</{key}>')
+
+        self.append(f"</{node}>")
+
+    def add_shape(
+        self,
+        link: Link,
+        part: Part,
+        node: str,
+        T_world_link: np.ndarray,
+        shape: Shape,
+        shape_n: int,
+    ):
+        """
+        Add shapes (box, sphere and cylinder) nodes to the SDF.
+        """
+        self.append(f'<{node} name="{part.name}_{node}_shapes_{shape_n}">')
+
+        T_link_shape = (
+            np.linalg.inv(T_world_link) @ part.T_world_part @ shape.T_part_shape
+        )
+        self.append(self.pose(T_link_shape, relative_to=link.name))
+
+        self.append("<geometry>")
+        if isinstance(shape, Box):
+            self.append("<box><size>%g %g %g</size></box>" % self.config.round(tuple(shape.size)))
+        elif isinstance(shape, Cylinder):
+            self.append(
+                "<cylinder><length>%g</length><radius>%g</radius></cylinder>"
+                % self.config.round((shape.length, shape.radius))
+            )
+        elif isinstance(shape, Sphere):
+            self.append("<sphere><radius>%g</radius></sphere>" % self.config.round((shape.radius,)))
+        self.append("</geometry>")
+
+        if node == "visual":
+            self.append_material(shape.color)
+
+        # Apply properties based on node type (visual or collision)
+        properties = shape.visual_properties if node == "visual" else shape.collision_properties
+        for key, value in properties.items():
+            self.append(f'<{key}>{xml_escape(str(value))}</{key}>')
+
+        self.append(f"</{node}>")
+
+    def add_geometries(self, link: Link, part: Part, T_world_link: np.ndarray):
+        """
+        Add a part geometries
+        """
+        shape_n = 0
+        for shape in part.shapes:
+            if shape.visual:
+                shape_n += 1
+                self.add_shape(link, part, "visual", T_world_link, shape, shape_n)
+            if shape.collision:
+                shape_n += 1
+                self.add_shape(link, part, "collision", T_world_link, shape, shape_n)
+
+        mesh_n = 0
+        for mesh in part.meshes:
+            if mesh.visual:
+                mesh_n += 1
+                self.add_mesh(link, part, "visual", T_world_link, mesh, mesh_n)
+            if mesh.collision:
+                mesh_n += 1
+                self.add_mesh(link, part, "collision", T_world_link, mesh, mesh_n)
+
+    def add_joint(self, joint: Joint, T_world_link: np.ndarray):
+        self.append(f"<!-- Joint from {joint.parent.name} to {joint.child.name} -->")
+
+        joint_type = joint.properties.get("type", joint.joint_type)
+        self.append(f'<joint name="{joint.name}" type="{joint_type}">')
+
+        T_link_joint = np.linalg.inv(T_world_link) @ joint.T_world_joint
+        self.append(self.pose(T_link_joint, relative_to=joint.parent.name))
+
+        self.append(f"<parent>{joint.parent.name}</parent>")
+        self.append(f"<child>{joint.child.name}</child>")
+        self.append("<axis>")
+        self.append("<xyz>%g %g %g</xyz>" % self.config.round(tuple(joint.axis)))
+
+        self.append("<limit>")
+        if "max_effort" in joint.properties:
+            self.append("<effort>%g</effort>" % self.config.round(joint.properties["max_effort"]))
+        else:
+            self.append("<effort>10</effort>")
+
+        if "max_velocity" in joint.properties:
+            self.append("<velocity>%g</velocity>" % self.config.round(joint.properties["max_velocity"]))
+        else:
+            self.append("<velocity>10</velocity>")
+
+        joint_limits = joint.properties.get("limits", joint.limits)
+        if joint_limits is None:
+            if joint_type == "revolute":
+                joint_limits = [-np.pi, np.pi]
+            if joint_type == "prismatic":
+                joint_limits = [-1, 1]
+
+        self.append(f"<lower>{joint_limits[0]}</lower>")
+        self.append(f"<upper>{joint_limits[1]}</upper>")
+        self.append("</limit>")
+
+        if joint.relation is not None:
+            self.append(
+                f'<mimic joint="{joint.relation.source_joint}"><multiplier>{joint.relation.ratio}</multiplier></mimic>'
+            )
+
+        self.append("<dynamics>")
+        self.append(f"<friction>{joint.properties.get('friction', 0.)}</friction>")
+        self.append(f"<damping>{joint.properties.get('damping', 0.)}</damping>")
+        self.append("</dynamics>")
+
+        self.append("</axis>")
+
+        self.append("</joint>")
+
+    def add_frame(
+        self,
+        link: Link,
+        frame: str,
+        T_world_link: np.ndarray,
+        T_world_frame: np.ndarray,
+    ):
+        self.append(f"<!-- Frame {frame} -->")
+        T_link_frame = np.linalg.inv(T_world_link) @ T_world_frame
+
+        self.append(f'<frame name="{frame}">')
+        self.append(self.pose(T_link_frame, relative_to=link.name))
+        self.append("</frame>")
+
+    def add_link(self, robot: Robot, link: Link, joint: Joint = None):
+        """
+        Adds a link recursively to the SDF file
+        """
+        self.append(f"<!-- Link {link.name} -->")
+        self.append(f'<link name="{link.name}">')
+
+        T_world_link = np.eye(4)
+        if joint is not None:
+            T_world_link = joint.T_world_joint
+
+        # Adding inertial properties
+        mass, com, inertia = link.get_dynamics(T_world_link)
+        self.add_inertial(mass, com, inertia, link.name)
+
+        relative_to = ""
+        if joint is not None:
+            relative_to = joint.name
+        self.append(self.pose(np.eye(4), relative_to=relative_to))
+
+        # Adding geometry objects
+        for part in link.parts:
+            self.append(f"<!-- Part {part.name} -->")
+            self.add_geometries(link, part, T_world_link)
+
+        self.append("</link>")
+
+        if link.fixed:
+            self.append(f'<joint name="{link.name}_fixed_world" type="fixed">')
+            self.append(f"<parent>world</parent>")
+            self.append(f"<child>{link.name}</child>")
+            self.append("</joint>")
+
+        # Adding frames attached to current link
+        for frame, T_world_frame in link.frames.items():
+            self.add_frame(link, frame, T_world_link, T_world_frame)
+
+        # Adding joints and children links
+        for joint in robot.get_link_joints(link):
+            self.add_link(robot, joint.child, joint)
+            self.add_joint(joint, T_world_link)
+
+    def pose(self, matrix: np.ndarray, relative_to: str = ""):
+        """
+        Transforms a transformation matrix into a SDF pose tag
+        """
+        relative = ""
+        if relative_to:
+            relative = f' relative_to="{relative_to}"'
+
+        sdf = "<pose%s>%g %g %g %g %g %g</pose>"
+
+        return sdf % (relative, *self.config.round((*matrix[:3, 3], *rotation_matrix_to_rpy(matrix))))
+
+    def write_xml(self, robot: Robot, filename: str) -> str:
+        model_config = MODEL_CONFIG_XML % (robot.name, os.path.basename(filename))
+
+        super().write_xml(robot, filename)
+
+        dirname = os.path.dirname(filename)
+        with open(dirname + "/model.config", "w") as file:
+            file.write(model_config)
+            print(success(f"* Writing model.config"))

onshape_to_robot/exporter_urdf.py

@@ -0,0 +1,326 @@
+from __future__ import annotations
+import numpy as np
+import os
+from .message import warning
+from .robot import Robot, Link, Part, Joint
+from .config import Config
+from .geometry import Box, Cylinder, Sphere, Shape, Mesh
+from .exporter import Exporter
+from .exporter_utils import xml_escape, rotation_matrix_to_rpy
+
+
+class ExporterURDF(Exporter):
+    def __init__(self, config: Config | None = None):
+        super().__init__()
+        self.config: Config = config
+
+        self.ext: str = "urdf"
+        self.no_dynamics: bool = False
+        self.package_name: str = ""
+        self.additional_xml: str = ""
+        self.set_zero_mass_to_fixed: bool = False
+
+        if config is not None:
+            self.no_dynamics = config.no_dynamics
+            self.package_name: str = config.get("package_name", "")
+            self.set_zero_mass_to_fixed: bool = config.get(
+                "set_zero_mass_to_fixed", False
+            )
+            additional_xml_file = config.get("additional_xml", None, required=False)
+            if isinstance(additional_xml_file, str):
+                self.add_additional_xml(additional_xml_file)
+            elif isinstance(additional_xml_file, list):
+                for filename in additional_xml_file:
+                    self.add_additional_xml(filename)
+
+    def add_additional_xml(self, xml_file: str):
+        self.additional_xml += f"<!-- Additional {xml_file} -->"
+        with open(self.config.output_directory + "/" + xml_file, "r") as file:
+            self.additional_xml += file.read()
+
+    def append(self, line: str):
+        self.xml += line
+
+    def build(self, robot: Robot):
+        self.xml = ""
+        self.append('<?xml version="1.0" ?>')
+        self.append("<!-- Generated using onshape-to-robot -->")
+        if self.config:
+            self.append(f"<!-- Onshape {self.config.printable_version()} -->")
+        self.append(f'<robot name="{robot.name}">')
+
+        if len(robot.base_links) > 1:
+            print(
+                warning(
+                    "WARNING: Multiple base links detected, which is not supported by URDF."
+                )
+            )
+            print(warning("Only the first base link will be considered."))
+
+        if len(robot.base_links) > 0:
+            self.add_link(robot, robot.base_links[0])
+
+        if self.additional_xml:
+            self.append(self.additional_xml)
+
+        self.append("</robot>")
+
+        return self.xml
+
+    def add_inertial(
+        self, mass: float, com: np.ndarray, inertia: np.ndarray, fixed: str = False
+    ):
+        # Unless "no_dynamics" is set, we make sure that mass and inertia
+        # are not zero
+        if not self.no_dynamics:
+            mass = max(1e-9, mass)
+            inertia[0, 0] = max(1e-9, inertia[0, 0])
+            inertia[1, 1] = max(1e-9, inertia[1, 1])
+            inertia[2, 2] = max(1e-9, inertia[2, 2])
+        if fixed and self.set_zero_mass_to_fixed:
+            # To mark an object as fixed in the world, sets its dynamics to zero
+            mass = 0
+            com = np.zeros(3)
+            inertia = np.zeros((3, 3))
+
+        self.append("<inertial>")
+        self.append(
+            '<origin xyz="%g %g %g" rpy="0 0 0"/>'
+            % self.config.round(
+                (
+                    com[0],
+                    com[1],
+                    com[2],
+                )
+            )
+        )
+        self.append('<mass value="%g" />' % self.config.round(mass))
+        self.append(
+            '<inertia ixx="%g" ixy="%g"  ixz="%g" iyy="%g" iyz="%g" izz="%g" />'
+            % self.config.round(
+                (
+                    inertia[0, 0],
+                    inertia[0, 1],
+                    inertia[0, 2],
+                    inertia[1, 1],
+                    inertia[1, 2],
+                    inertia[2, 2],
+                )
+            )
+        )
+        self.append("</inertial>")
+
+    def add_mesh(self, part: Part, node: str, T_world_link: np.ndarray, mesh: Mesh):
+        """
+        Add a mesh node (e.g. STL) to the URDF file
+        """
+        self.append(f"<{node}>")
+
+        T_link_part = np.linalg.inv(T_world_link) @ part.T_world_part
+        self.append(self.origin(T_link_part))
+
+        mesh_file = mesh.filename
+        if self.package_name:
+            mesh_file = self.package_name + "/" + mesh_file
+
+        self.append("<geometry>")
+        self.append(f'<mesh filename="package://{xml_escape(mesh_file)}" />')
+        self.append("</geometry>")
+
+        if node == "visual":
+            material_name = f"{part.name}_material"
+            self.append(f'<material name="{xml_escape(material_name)}">')
+            self.append(
+                '<color rgba="%g %g %g %g"/>'
+                % self.config.round(
+                    (mesh.color[0], mesh.color[1], mesh.color[2], mesh.color[3])
+                )
+            )
+            self.append("</material>")
+
+        # Apply properties based on node type (visual or collision)
+        properties = mesh.visual_properties if node == "visual" else mesh.collision_properties
+        for key, value in properties.items():
+            self.append(f'<{key}>{xml_escape(str(value))}</{key}>')
+
+        self.append(f"</{node}>")
+
+    def add_shape(self, part: Part, node: str, T_world_link: np.ndarray, shape: Shape):
+        """
+        Add shapes (box, sphere and cylinder) nodes to the URDF.
+        """
+        self.append(f"<{node}>")
+
+        T_link_shape = (
+            np.linalg.inv(T_world_link) @ part.T_world_part @ shape.T_part_shape
+        )
+        self.append(self.origin(T_link_shape))
+
+        self.append("<geometry>")
+        if isinstance(shape, Box):
+            self.append(
+                '<box size="%g %g %g" />' % self.config.round(tuple(shape.size))
+            )
+        elif isinstance(shape, Cylinder):
+            self.append(
+                '<cylinder length="%g" radius="%g" />'
+                % self.config.round((shape.length, shape.radius))
+            )
+        elif isinstance(shape, Sphere):
+            self.append('<sphere radius="%g" />' % self.config.round((shape.radius,)))
+        self.append("</geometry>")
+
+        if node == "visual":
+            material_name = f"{part.name}_material"
+            self.append(f'<material name="{xml_escape(material_name)}">')
+            self.append(
+                '<color rgba="%g %g %g %g"/>'
+                % self.config.round(
+                    (shape.color[0], shape.color[1], shape.color[2], shape.color[3])
+                )
+            )
+            self.append("</material>")
+
+        # Apply properties based on node type (visual or collision)
+        properties = shape.visual_properties if node == "visual" else shape.collision_properties
+        for key, value in properties.items():
+            self.append(f'<{key}>{xml_escape(str(value))}</{key}>')
+
+        self.append(f"</{node}>")
+
+    def add_geometries(self, part: Part, T_world_link: np.ndarray):
+        """
+        Add a part geometries
+        """
+        for shape in part.shapes:
+            if shape.visual:
+                self.add_shape(part, "visual", T_world_link, shape)
+            if shape.collision:
+                self.add_shape(part, "collision", T_world_link, shape)
+
+        for mesh in part.meshes:
+            if mesh.visual:
+                self.add_mesh(part, "visual", T_world_link, mesh)
+            if mesh.collision:
+                self.add_mesh(part, "collision", T_world_link, mesh)
+
+    def add_joint(self, joint: Joint, T_world_link: np.ndarray):
+        self.append(f"<!-- Joint from {joint.parent.name} to {joint.child.name} -->")
+
+        joint_type = joint.properties.get("type", joint.joint_type)
+        self.append(f'<joint name="{joint.name}" type="{joint_type}">')
+
+        T_link_joint = np.linalg.inv(T_world_link) @ joint.T_world_joint
+        self.append(self.origin(T_link_joint))
+
+        self.append(f'<parent link="{joint.parent.name}" />')
+        self.append(f'<child link="{joint.child.name}" />')
+        self.append('<axis xyz="%g %g %g"/>' % self.config.round(tuple(joint.axis)))
+
+        limits = ""
+        if "max_effort" in joint.properties:
+            limits += 'effort="%g" ' % self.config.round(joint.properties["max_effort"])
+        else:
+            limits += 'effort="10" '
+
+        if "max_velocity" in joint.properties:
+            limits += 'velocity="%g" ' % self.config.round(
+                joint.properties["max_velocity"]
+            )
+        else:
+            limits += 'velocity="10" '
+
+        joint_limits = joint.properties.get("limits", joint.limits)
+        if joint_limits is not None:
+            limits += 'lower="%g" upper="%g" ' % self.config.round(
+                (joint_limits[0], joint_limits[1])
+            )
+        elif joint_type == "revolute":
+            limits += f'lower="{-np.pi}" upper="{np.pi}" '
+        elif joint_type == "prismatic":
+            limits += 'lower="-1" upper="1" '
+
+        if limits:
+            self.append(f"<limit {limits}/>")
+
+        if "friction" in joint.properties:
+            self.append(
+                f'<joint_properties friction="{joint.properties["friction"]}"/>'
+            )
+
+        if joint.relation is not None:
+            self.append(
+                f'<mimic joint="{joint.relation.source_joint}" multiplier="{joint.relation.ratio}" />'
+            )
+
+        self.append("</joint>")
+
+    def add_frame(
+        self,
+        link: Link,
+        frame: str,
+        T_world_link: np.ndarray,
+        T_world_frame: np.ndarray,
+    ):
+        self.append(f"<!-- Frame {frame} (dummy link + fixed joint) -->")
+        T_link_frame = np.linalg.inv(T_world_link) @ T_world_frame
+
+        # Adding a dummy link to the assembly
+        self.append(f'<link name="{frame}">')
+        self.append(self.origin(np.eye(4)))
+
+        self.append("<inertial>")
+        self.append('<origin xyz="0 0 0" rpy="0 0 0" />')
+        if self.no_dynamics:
+            self.append('<mass value="0" />')
+        else:
+            self.append('<mass value="1e-9" />')
+        self.append('<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />')
+        self.append("</inertial>")
+
+        self.append("</link>")
+
+        # Attaching this dummy link to the parent frame using a fixed joint
+        self.append(f'<joint name="{frame}_frame" type="fixed">')
+        self.append(self.origin(T_link_frame))
+        self.append(f'<parent link="{link.name}" />')
+        self.append(f'<child link="{frame}" />')
+        self.append('<axis xyz="0 0 0"/>')
+        self.append("</joint>")
+
+    def add_link(self, robot: Robot, link: Link, T_world_link: np.ndarray = np.eye(4)):
+        """
+        Adds a link recursively to the URDF file
+        """
+        self.append(f"<!-- Link {link.name} -->")
+        self.append(f'<link name="{link.name}">')
+
+        # Adding inertial properties
+        mass, com, inertia = link.get_dynamics(T_world_link)
+        self.add_inertial(mass, com, inertia, link.fixed)
+
+        # Adding geometry objects
+        for part in link.parts:
+            self.append(f"<!-- Part {part.name} -->")
+            self.add_geometries(part, T_world_link)
+
+        self.append("</link>")
+
+        # Adding frames attached to current link
+        for frame, T_world_frame in link.frames.items():
+            self.add_frame(link, frame, T_world_link, T_world_frame)
+
+        # Adding joints and children links
+        for joint in robot.get_link_joints(link):
+            self.add_link(robot, joint.child, joint.T_world_joint)
+            self.add_joint(joint, T_world_link)
+
+    def origin(self, matrix: np.ndarray):
+        """
+        Transforms a transformation matrix into a URDF origin tag
+        """
+        urdf = '<origin xyz="%g %g %g" rpy="%g %g %g" />'
+
+        return urdf % self.config.round(
+            (*matrix[:3, 3], *rotation_matrix_to_rpy(matrix))
+        )

onshape_to_robot/exporter_utils.py

@@ -0,0 +1,30 @@
+import math
+import numpy as np
+from xml.sax.saxutils import escape
+
+
+def xml_escape(unescaped: str) -> str:
+    """
+    Escapes XML characters in a string so that it can be safely added to an XML file
+    """
+    return escape(unescaped, entities={"'": "'", '"': """})
+
+
+def rotation_matrix_to_rpy(R):
+    """
+    Converts a rotation matrix to rpy Euler angles
+    """
+    sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
+
+    singular = sy < 1e-6
+
+    if not singular:
+        x = math.atan2(R[2, 1], R[2, 2])
+        y = math.atan2(-R[2, 0], sy)
+        z = math.atan2(R[1, 0], R[0, 0])
+    else:
+        x = math.atan2(-R[1, 2], R[1, 1])
+        y = math.atan2(-R[2, 0], sy)
+        z = 0
+
+    return np.array([x, y, z])

onshape_to_robot/expression.py

@@ -0,0 +1,119 @@
+import numpy as np
+import ast
+import operator as op
+
+
+class ExpressionParser:
+    """
+    Evaluate Onshape expression
+    See https://cad.onshape.com/help/Content/numeric-fields.htm
+
+    This parser is based on ast module.
+    """
+
+    def __init__(self):
+        self.variables_lazy_loading = None
+        self.variables = {
+            "pi": np.pi, "Pi": np.pi, "PI": np.pi
+        }
+
+    # Supported operators
+    operators = {
+        ast.Add: op.add,
+        ast.Sub: op.sub,
+        ast.Mult: op.mul,
+        ast.Div: op.truediv,
+        ast.Pow: op.pow,
+        ast.BitXor: op.xor,
+        ast.USub: op.neg,
+        ast.Mod: op.mod,
+    }
+
+    # Supporter functions
+    functions = {
+        "cos": np.cos,
+        "sin": np.sin,
+        "tan": np.tan,
+        "acos": np.arccos,
+        "asin": np.arcsin,
+        "atan": np.arctan,
+        "atan2": np.arctan2,
+        "cosh": np.cosh,
+        "sinh": np.sinh,
+        "tanh": np.tanh,
+        "asinh": np.arcsinh,
+        "acosh": np.arccosh,
+        "atanh": np.arctanh,
+        "ceil": np.ceil,
+        "floor": np.floor,
+        "round": np.round,
+        "exp": np.exp,
+        "sqrt": np.sqrt,
+        "abs": np.abs,
+        "max": np.max,
+        "min": np.min,
+        "log": np.log,
+        "log10": np.log10,
+    }
+
+    def eval_expr(self, expr):
+        # Length units. Converting everything to meter / radian
+        units = {
+            "millimeter": 1e-3,
+            "mm": 1e-3,
+            "centimeter": 1e-2,
+            "cm": 1e-2,
+            "meter": 1.0,
+            "inch": 0.0254,
+            "in": 0.0254,
+            "foot": 0.3048,
+            "ft": 0.3048,
+            "yard": 0.9144,
+            "yd": 0.9144,
+            "radian": 1.0,
+            "rad": 1.0,
+            "degree": np.pi / 180,
+            "deg": np.pi / 180,
+            "m": 1.0
+        }
+        for unit, factor in units.items():
+            expr = expr.replace(f" {unit}", f"*{factor}")
+
+        expr = expr.replace("#", "")
+        expr = expr.replace("^", "**")
+
+        return self.eval_(ast.parse(expr, mode="eval").body)
+
+    def eval_(self, node):
+        if isinstance(node, ast.Constant):
+            return float(node.value)
+        elif isinstance(node, ast.BinOp):
+            return self.operators[type(node.op)](
+                self.eval_(node.left), self.eval_(node.right)
+            )
+        elif isinstance(node, ast.UnaryOp):  # e.g., -1
+            return self.operators[type(node.op)](self.eval_(node.operand))
+        elif isinstance(node, ast.Name):
+            if (
+                node.id not in self.variables
+                and self.variables_lazy_loading is not None
+            ):
+                self.variables_lazy_loading()
+                self.variables_lazy_loading = None
+            if node.id not in self.variables:
+                raise ValueError(f"Unknown variable in expression: {node.id}")
+            return self.variables[node.id]
+        elif isinstance(node, ast.Call):
+            if node.func.id not in self.functions:
+                raise ValueError(f"Unknown function in expression: {node.func.id}")
+            return self.functions[node.func.id](*[self.eval_(arg) for arg in node.args])
+        else:
+            raise TypeError(node)
+
+
+if __name__ == "__main__":
+    ep = ExpressionParser()
+    ep.variables["x"] = 5
+
+    print(ep.eval_expr("(cos(5 deg)) mm + #x inch"))
+    print(ep.eval_expr("-sin(3/(2^2) deg)"))

onshape_to_robot/geometry.py

@@ -0,0 +1,87 @@
+import numpy as np
+
+
+class Geometry:
+    def __init__(
+        self,
+        color: np.ndarray = np.array([0.5, 0.5, 0.5, 1.0]),
+        visual: bool = True,
+        collision: bool = True,
+    ):
+        self.color: np.ndarray = color
+        self.visual: bool = visual
+        self.collision: bool = collision
+        self.visual_properties: dict = {}
+        self.collision_properties: dict = {}
+
+    def is_type(self, what: str):
+        if what == "visual":
+            return self.visual
+        elif what == "collision":
+            return self.collision
+        return False
+
+
+class Mesh(Geometry):
+    def __init__(
+        self,
+        filename: str,
+        color: np.ndarray = np.array([0.5, 0.5, 0.5, 1.0]),
+        visual: bool = True,
+        collision: bool = True,
+    ):
+        super().__init__(color, visual, collision)
+        self.filename: str = filename
+
+
+class Shape(Geometry):
+    def __init__(
+        self,
+        T_part_shape: np.ndarray,
+        color: np.ndarray = np.array([0.5, 0.5, 0.5, 1.0]),
+        visual: bool = True,
+        collision: bool = True,
+    ):
+        super().__init__(color, visual, collision)
+        self.T_part_shape: np.ndarray = T_part_shape
+
+
+class Box(Shape):
+    def __init__(
+        self,
+        T_part_shape: np.ndarray,
+        size: np.ndarray,
+        color: np.ndarray = np.array([0.5, 0.5, 0.5, 1.0]),
+        visual: bool = True,
+        collision: bool = True,
+    ):
+        super().__init__(T_part_shape, color, visual, collision)
+        self.size: np.ndarray = size
+
+
+class Cylinder(Shape):
+    def __init__(
+        self,
+        T_part_shape: np.ndarray,
+        length: float,
+        radius: float,
+        color: np.ndarray = np.array([0.5, 0.5, 0.5, 1.0]),
+        visual: bool = True,
+        collision: bool = True,
+    ):
+        super().__init__(T_part_shape, color, visual, collision)
+        self.length: float = length
+        self.radius: float = radius
+
+
+class Sphere(Shape):
+    def __init__(
+        self,
+        T_part_shape: np.ndarray,
+        radius: float,
+        color: np.ndarray = np.array([0.5, 0.5, 0.5, 1.0]),
+        visual: bool = True,
+        collision: bool = True,
+    ):
+        super().__init__(T_part_shape, color, visual, collision)
+        self.radius: float = radius

onshape_to_robot/message.py

@@ -0,0 +1,21 @@
+from colorama import Fore, Back, Style, just_fix_windows_console
+
+just_fix_windows_console()
+
+def error(text: str):
+    return Fore.RED + text + Style.RESET_ALL
+
+def bright(text: str):
+    return Style.BRIGHT + text + Style.RESET_ALL
+
+def info(text: str):
+    return Fore.BLUE + text + Style.RESET_ALL
+
+def success(text: str):
+    return Fore.GREEN + text + Style.RESET_ALL
+
+def warning(text: str):
+    return Fore.YELLOW + text + Style.RESET_ALL
+
+def dim(text: str):
+    return Style.DIM + text + Style.RESET_ALL

onshape_to_robot/mujoco.py

@@ -0,0 +1,38 @@
+def main():
+    import time
+    import mujoco
+    import argparse
+    import mujoco.viewer
+
+    parser = argparse.ArgumentParser(prog="onshape-to-robot-mujoco")
+    parser.add_argument("--sim", action="store_true")
+    parser.add_argument("--x", type=float, default=0)
+    parser.add_argument("--y", type=float, default=0)
+    parser.add_argument("--z", type=float, default=0.5)
+    parser.add_argument("directory")
+    args = parser.parse_args()
+
+    robot_path = args.directory
+    if not robot_path.endswith(".xml"):
+        robot_path += "/scene.xml"
+
+    model: mujoco.MjModel = mujoco.MjModel.from_xml_path(robot_path)
+    data: mujoco.MjData = mujoco.MjData(model)
+
+    # Check for root existence
+    if len(model.jnt_type) and model.jnt_type[0] == mujoco.mjtJoint.mjJNT_FREE:
+        data.qpos[:3] = [args.x, args.y, args.z]
+
+    viewer = mujoco.viewer.launch_passive(model, data)
+    while viewer.is_running():
+        step_start = time.time()
+        mujoco.mj_step(model, data)
+        viewer.sync()
+
+        time_until_next_step = model.opt.timestep - (time.time() - step_start)
+        if time_until_next_step > 0:
+            time.sleep(time_until_next_step)
+
+
+if __name__ == "__main__":
+    main()

onshape_to_robot/onshape_api/__init__.py

@@ -0,0 +1,11 @@
+'''
+onshape_api, based on onshape "apikey"
+======
+
+Demonstrates usage of API keys for the Onshape REST API
+'''
+
+__copyright__ = 'Copyright (c) 2016 Onshape, Inc.'
+__license__ = 'All rights reserved.'
+__title__ = 'onshape_api'
+__all__ = ['onshape', 'client', 'utils']

onshape_to_robot/onshape_api/cache.py

@@ -0,0 +1,54 @@
+import os
+import inspect
+import hashlib
+from pathlib import Path
+import pickle
+
+
+def get_cache_path() -> Path:
+    """
+    Return the path to the user cache.
+    """
+    path = Path.home() / ".cache" / "onshape-to-robot"
+    path.mkdir(parents=True, exist_ok=True)
+    return path
+
+
+def can_cache(method, *args, **kwargs) -> bool:
+    """
+    Check if the cache can be used.
+    When using wmv=w, the current workspace is used, which make it impossible to cache.
+    """
+    signature = inspect.signature(method)
+    wmv = None
+    if "wmv" in signature.parameters:
+        wmv = signature.parameters["wmv"].default
+    if "wmv" in kwargs:
+        wmv = kwargs["wmv"]
+    return wmv != "w"
+
+
+def cache_response(method):
+    """
+    Decorator that caches the response of a method.
+    """
+
+    def cached_call(*args, **kwargs):
+        # Checking if the method can be cached
+        if not can_cache(method, *args, **kwargs):
+            return method(*args, **kwargs)
+
+        # Building filename that is unique for method/args combination
+        method_name = method.__qualname__
+        arguments = {"args": args, "kwargs": kwargs}
+        arguments_hash = hashlib.sha1(pickle.dumps(arguments)).hexdigest()
+        filename = f"{get_cache_path()}/{method_name}_{arguments_hash}.pkl"
+
+        if not os.path.exists(filename):
+            result = method(*args, **kwargs)
+            with open(filename, "wb") as f:
+                pickle.dump(result, f)
+
+        return pickle.load(open(filename, "rb"))
+
+    return cached_call

onshape_to_robot/onshape_api/client.py

@@ -0,0 +1,254 @@
+"""
+client
+======
+
+Convenience functions for working with the Onshape API
+"""
+
+from .onshape import Onshape
+from .cache import cache_response
+
+
+def escape(s):
+    return s.replace("/", "%2f").replace("+", "%2b")
+
+
+class Client:
+    """
+    Defines methods for testing the Onshape API. Comes with several methods:
+
+    - Create a document
+    - Delete a document
+    - Get a list of documents
+
+    Attributes:
+        - stack (str, default='https://cad.onshape.com'): Base URL
+        - logging (bool, default=True): Turn logging on or off
+    """
+
+    def __init__(
+        self, stack="https://cad.onshape.com", logging=True, creds="./config.json"
+    ):
+        """
+        Instantiates a new Onshape client.
+
+        Args:
+            - stack (str, default='https://cad.onshape.com'): Base URL
+            - logging (bool, default=True): Turn logging on or off
+        """
+
+        self._metadata_cache = {}
+        self._massproperties_cache = {}
+        self._stack = stack
+        self._api = Onshape(stack=stack, logging=logging, creds=creds)
+
+    def request(self, url, **kwargs):
+        return self._api.request("get", url, **kwargs).json()
+
+    def request_binary(self, url, **kwargs):
+        return self._api.request("get", url, **kwargs).content
+
+    @cache_response
+    def get_document(self, did):
+        """
+        Get details for a specified document.
+
+        Args:
+            - did (str): Document ID
+
+        Returns:
+            - requests.Response: Onshape response data
+        """
+        return self.request(f"/api/documents/{escape(did)}")
+
+    @cache_response
+    def list_elements(self, did, wid, wmv="w"):
+        """
+        Get the list of elements in a given document
+        """
+
+        return self.request(
+            f"/api/documents/d/{escape(did)}/{escape(wmv)}/{escape(wid)}/elements"
+        )
+
+    @cache_response
+    def get_assembly(self, did, wmvid, eid, wmv="w", configuration="default"):
+        """
+        Retrieve the assembly structure for a specified document / workspace / element.
+        """
+        return self.request(
+            f"/api/assemblies/d/{escape(did)}/{escape(wmv)}/{escape(wmvid)}/e/{escape(eid)}",
+            query={
+                "includeMateFeatures": "true",
+                "includeMateConnectors": "true",
+                "includeNonSolids": "true",
+                "configuration": configuration,
+            },
+        )
+
+    @cache_response
+    def get_features(self, did, wvid, eid, wmv="w", configuration="default"):
+        """
+        Gets the feature list for specified document / workspace / part studio.
+
+        Args:
+            - did (str): Document ID
+            - mid (str): Microversion
+            - eid (str): Element ID
+
+        Returns:
+            - requests.Response: Onshape response data
+        """
+
+        return self.request(
+            f"/api/assemblies/d/{escape(did)}/{escape(wmv)}/{escape(wvid)}/e/{escape(eid)}/features",
+            query={"configuration": configuration},
+        )
+
+    @cache_response
+    def get_sketches(self, did, mid, eid, configuration):
+        """
+        Get sketches for a given document / microversion / element.
+        """
+        return self.request(
+            f"/api/partstudios/d/{escape(did)}/m/{escape(mid)}/e/{escape(eid)}/sketches",
+            query={"includeGeometry": "true", "configuration": configuration},
+        )
+
+    @cache_response
+    def get_parts(self, did, mid, eid, configuration):
+        """
+        Get parts for a given document / microversion / element.
+        """
+        return self.request(
+            f"/api/parts/d/{escape(did)}/m/{escape(mid)}/e/{escape(eid)}",
+            query={"configuration": configuration},
+        )
+
+    def find_new_partid(
+        self, did, mid, eid, partid, configuration_before, configuration
+    ):
+        before = self.get_parts(did, mid, eid, configuration_before)
+        name = None
+        for entry in before:
+            if entry["partId"] == partid:
+                name = entry["name"]
+
+        if name is not None:
+            after = self.get_parts(did, mid, eid, configuration)
+            for entry in after:
+                if entry["name"] == name:
+                    return entry["partId"]
+        else:
+            print("Onshape ERROR: Can't find new partid for " + str(partid))
+
+        return partid
+
+    @cache_response
+    def part_studio_stl_m(
+        self,
+        did,
+        wmvid,
+        eid,
+        partid="",
+        wmv="m",
+        configuration="default",
+        linked_document_id=None,
+    ):
+        req_headers = {"Accept": "*/*"}
+        query = {
+            "mode": "binary",
+            "units": "meter",
+            "configuration": configuration,
+        }
+        if linked_document_id is not None:
+            query["linkDocumentId"] = linked_document_id
+        return self.request_binary(
+            f"/api/parts/d/{escape(did)}/{escape(wmv)}/{escape(wmvid)}/e/{escape(eid)}/partid/{escape(partid)}/stl",
+            query=query,
+            headers=req_headers,
+        )
+
+    @cache_response
+    def matevalues(self, did, wmvid, eid, wmv="w", configuration="default"):
+        return self.request(
+            f"/api/assemblies/d/{escape(did)}/{wmv}/{escape(wmvid)}/e/{escape(eid)}/matevalues",
+            query={"configuration": configuration},
+        )
+
+    @cache_response
+    def part_get_metadata(
+        self,
+        did,
+        wmvid,
+        eid,
+        partid,
+        wmv="m",
+        configuration="default",
+        linked_document_id=None,
+    ):
+        query = {"configuration": configuration}
+        if linked_document_id is not None:
+            query["linkDocumentId"] = linked_document_id
+        return self.request(
+            f"/api/metadata/d/{escape(did)}/{escape(wmv)}/{escape(wmvid)}/e/{escape(eid)}/p/{escape(partid)}",
+            query=query,
+        )
+
+    @cache_response
+    def part_mass_properties(
+        self,
+        did,
+        wmvid,
+        eid,
+        partid,
+        wmv="m",
+        configuration="default",
+        linked_document_id=None,
+    ):
+        query = {
+            "configuration": configuration,
+            "useMassPropertyOverrides": True,
+        }
+        if linked_document_id is not None:
+            query["linkDocumentId"] = linked_document_id
+        return self.request(
+            f"/api/parts/d/{escape(did)}/{escape(wmv)}/{escape(wmvid)}/e/{escape(eid)}/partid/{escape(partid)}/massproperties",
+            query=query,
+        )
+
+    @cache_response
+    def standard_cont_mass_properties(
+        self, did, vid, eid, partid, linked_document_id, configuration
+    ):
+        return self.request(
+            f"/api/parts/d/{escape(did)}/v/{escape(vid)}/e/{escape(eid)}/partid/{escape(partid)}/massproperties",
+            query={
+                "configuration": configuration,
+                "useMassPropertyOverrides": True,
+                "linkDocumentId": linked_document_id,
+                "inferMetadataOwner": True,
+            },
+        )
+
+    @cache_response
+    def elements_configuration(
+        self, did, wmvid, eid, wmv, linked_document_id=None, configuration=None
+    ):
+        query = {}
+        if linked_document_id is not None:
+            query["linkDocumentId"] = linked_document_id
+        return self.request(
+            f"/api/elements/d/{escape(did)}/{escape(wmv)}/{escape(wmvid)}/e/{escape(eid)}/configuration",
+            query=query,
+        )
+
+    @cache_response
+    def get_variables(self, did, wvid, eid, wmv, configuration):
+        return self.request(
+            f"/api/variables/d/{escape(did)}/{escape(wmv)}/{escape(wvid)}/e/{escape(eid)}/variables",
+            query={
+                "configuration": configuration,
+                "includeValuesAndReferencedVariables": True,
+            },
+        )

onshape_to_robot/onshape_api/onshape.py

@@ -0,0 +1,247 @@
+'''
+onshape
+======
+
+Provides access to the Onshape REST API
+'''
+
+from . import utils
+
+import os
+import random
+import string
+import commentjson as json
+import hmac
+import hashlib
+import base64
+import urllib
+import datetime
+import requests
+from colorama import Fore, Back, Style
+from urllib.parse import urlparse
+from urllib.parse import parse_qs
+
+__all__ = [
+    'Onshape'
+]
+
+
+class Onshape():
+    '''
+    Provides access to the Onshape REST API.
+
+    Attributes:
+        - stack (str): Base URL
+        - creds (str, default='./creds.json'): Credentials location
+        - logging (bool, default=True): Turn logging on or off
+    '''
+
+    def __init__(self, stack, creds='./config.json', logging=True):
+        '''
+        Instantiates an instance of the Onshape class. Reads credentials from a JSON file
+        of this format:
+
+            {
+                "http://cad.onshape.com": {
+                    "access_key": "YOUR KEY HERE",
+                    "secret_key": "YOUR KEY HERE"
+                },
+                etc... add new object for each stack to test on
+            }
+
+        The creds.json file should be stored in the root project folder; optionally,
+        you can specify the location of a different file.
+
+        Args:
+            - stack (str): Base URL
+            - creds (str, default='./config.json'): Credentials location
+        '''        
+
+        if not os.path.isfile(creds):
+            raise IOError('%s is not a file' % creds)
+
+        self._logging = logging
+
+        with open(creds, "r", encoding="utf-8") as stream:
+            try:
+                config = json.load(stream)
+            except TypeError as ex:
+                raise ValueError('%s is not valid json' % creds) from ex
+
+        try:
+            # Trying to retrieve from config.json, this is deprecated but kept for backwards compatibility
+            self._url = config["onshape_api"]
+            self._access_key = config['onshape_access_key'].encode('utf-8')
+            self._secret_key = config['onshape_secret_key'].encode('utf-8')
+
+            print(Fore.YELLOW + 'WARNING: Storing Onshape credentials in config.json is deprecated, please use environment variables instead' + Style.RESET_ALL)
+        except KeyError:
+            self._url = os.getenv('ONSHAPE_API')
+            self._secret_bearer = os.getenv('ONSHAPE_SECRET_BEARER')
+            self._access_key = os.getenv('ONSHAPE_ACCESS_KEY')
+            self._secret_key = os.getenv('ONSHAPE_SECRET_KEY')
+
+        if self._url and self._secret_bearer:
+            self._secret_bearer = self._secret_bearer.encode('utf-8')
+        elif self._url and self._access_key and self._secret_key:
+            self._access_key = self._access_key.encode('utf-8')
+            self._secret_key = self._secret_key.encode('utf-8')
+        else:
+            print(Fore.RED + 'ERROR: No Onshape API access key are set' + Style.RESET_ALL)
+            print()
+            print(Fore.BLUE + 'TIP: Connect to https://dev-portal.onshape.com/keys, and edit your .bashrc file:' + Style.RESET_ALL)
+            print(Fore.BLUE + 'export ONSHAPE_API=https://cad.onshape.com' + Style.RESET_ALL)
+            print(Fore.BLUE + 'export ONSHAPE_ACCESS_KEY=Your_Access_Key' + Style.RESET_ALL)
+            print(Fore.BLUE + 'export ONSHAPE_SECRET_KEY=Your_Secret_Key' + Style.RESET_ALL)
+            exit(1)
+
+        if self._logging:
+            utils.log('onshape instance created: url = %s, access key = %s' % (self._url, self._access_key))
+
+    def _make_nonce(self):
+        '''
+        Generate a unique ID for the request, 25 chars in length
+
+        Returns:
+            - str: Cryptographic nonce
+        '''
+
+        chars = string.digits + string.ascii_letters
+        nonce = ''.join(random.choice(chars) for i in range(25))
+
+        if self._logging:
+            utils.log('nonce created: %s' % nonce)
+
+        return nonce
+
+    def _append_auth(self, output_headers: dict, method, date, path, query={}, ctype='application/json'):
+        '''
+        Create the request signature to authenticate
+
+        Args:
+            - method (str): HTTP method
+            - date (str): HTTP date header string
+            - nonce (str): Cryptographic nonce
+            - path (str): URL pathname
+            - query (dict, default={}): URL query string in key-value pairs
+            - ctype (str, default='application/json'): HTTP Content-Type
+        '''
+
+        if self._secret_bearer:
+            # Access using simple bearer token
+            output_headers['Authorization'] = 'Bearer ' + self._secret_bearer.decode('utf-8')
+        else:
+            # Access using API key
+            nonce = self._make_nonce()
+            query = urllib.parse.urlencode(query)
+
+            hmac_str = (method + '\n' + nonce + '\n' + date + '\n' + ctype + '\n' + path +
+                        '\n' + query + '\n').lower().encode('utf-8')
+
+            signature = base64.b64encode(hmac.new(self._secret_key, hmac_str, digestmod=hashlib.sha256).digest())
+            auth = 'On ' + self._access_key.decode('utf-8') + ':HmacSHA256:' + signature.decode('utf-8')
+
+            if self._logging:
+                utils.log({
+                    'query': query,
+                    'hmac_str': hmac_str,
+                    'signature': signature,
+                    'auth': auth
+                })
+
+            output_headers['On-Nonce'] = nonce
+            output_headers['Authorization'] = auth
+
+    def _make_headers(self, method, path, query={}, headers={}):
+        '''
+        Creates a headers object to sign the request
+
+        Args:
+            - method (str): HTTP method
+            - path (str): Request path, e.g. /api/documents. No query string
+            - query (dict, default={}): Query string in key-value format
+            - headers (dict, default={}): Other headers to pass in
+
+        Returns:
+            - dict: Dictionary containing all headers
+        '''
+
+        date = datetime.datetime.utcnow().strftime('%a, %d %b %Y %H:%M:%S GMT')
+        ctype = headers.get('Content-Type') if headers.get('Content-Type') else 'application/json'
+
+        req_headers = {
+            'Content-Type': 'application/json',
+            'Date': date,
+            'User-Agent': 'Onshape Python Sample App',
+            'Accept': 'application/json'
+        }
+
+        self._append_auth(req_headers, method, date, path, query=query, ctype=ctype)
+
+        # add in user-defined headers
+        for h in headers:
+            req_headers[h] = headers[h]
+
+        return req_headers
+
+    def request(self, method, path, query={}, headers={}, body={}, base_url=None):
+        '''
+        Issues a request to Onshape
+
+        Args:
+            - method (str): HTTP method
+            - path (str): Path  e.g. /api/documents/:id
+            - query (dict, default={}): Query params in key-value pairs
+            - headers (dict, default={}): Key-value pairs of headers
+            - body (dict, default={}): Body for POST request
+            - base_url (str, default=None): Host, including scheme and port (if different from creds file)
+
+        Returns:
+            - requests.Response: Object containing the response from Onshape
+        '''
+
+        req_headers = self._make_headers(method, path, query, headers)
+        if base_url is None:
+            base_url = self._url
+        url = base_url + path + '?' + urllib.parse.urlencode(query)
+
+        if self._logging:
+            utils.log(body)
+            utils.log(req_headers)
+            utils.log('request url: ' + url)
+
+        # only parse as json string if we have to
+        body = json.dumps(body) if type(body) == dict else body
+
+        res = requests.request(method, url, headers=req_headers, data=body, allow_redirects=False, stream=True)
+
+        if res.status_code == 307:
+            location = urlparse(res.headers["Location"])
+            querystring = parse_qs(location.query)
+
+            if self._logging:
+                utils.log('request redirected to: ' + location.geturl())
+
+            new_query = {}
+            new_base_url = location.scheme + '://' + location.netloc
+
+            for key in querystring:
+                new_query[key] = querystring[key][0]  # won't work for repeated query params
+
+            return self.request(method, location.path, query=new_query, headers=headers, base_url=new_base_url)
+        elif not 200 <= res.status_code <= 206:
+            print(url)
+            print('! ERROR ('+str(res.status_code)+') while using Onshape API')
+            if res.text:
+                print('! '+res.text)
+
+            if res.status_code == 403:
+                print('HINT: Check that your access rights are correct, and that the clock on your computer is set correctly')
+            exit()
+            if self._logging:
+                utils.log('request failed, details: ' + res.text, level=1)
+        else:
+            if self._logging:
+                utils.log('request succeeded, details: ' + res.text)
+
+        return res

onshape_to_robot/onshape_api/utils.py

@@ -0,0 +1,74 @@
+'''
+utils
+=====
+
+Handy functions for API key sample app
+'''
+
+import logging
+from logging.config import dictConfig
+
+__all__ = [
+    'log'
+]
+
+
+def log(msg, level=0):
+    '''
+    Logs a message to the console, with optional level paramater
+
+    Args:
+        - msg (str): message to send to console
+        - level (int): log level; 0 for info, 1 for error (default = 0)
+    '''
+
+    red = '\033[91m'
+    endc = '\033[0m'
+
+    # configure the logging module
+    cfg = {
+        'version': 1,
+        'disable_existing_loggers': False,
+        'formatters': {
+            'stdout': {
+                'format': '[%(levelname)s]: %(asctime)s - %(message)s',
+                'datefmt': '%x %X'
+            },
+            'stderr': {
+                'format': red + '[%(levelname)s]: %(asctime)s - %(message)s' + endc,
+                'datefmt': '%x %X'
+            }
+        },
+        'handlers': {
+            'stdout': {
+                'class': 'logging.StreamHandler',
+                'level': 'DEBUG',
+                'formatter': 'stdout'
+            },
+            'stderr': {
+                'class': 'logging.StreamHandler',
+                'level': 'ERROR',
+                'formatter': 'stderr'
+            }
+        },
+        'loggers': {
+            'info': {
+                'handlers': ['stdout'],
+                'level': 'INFO',
+                'propagate': True
+            },
+            'error': {
+                'handlers': ['stderr'],
+                'level': 'ERROR',
+                'propagate': False
+            }
+        }
+    }
+
+    dictConfig(cfg)
+
+    lg = 'info' if level == 0 else 'error'
+    lvl = 20 if level == 0 else 40
+
+    logger = logging.getLogger(lg)
+    logger.log(lvl, msg)

onshape_to_robot/processor.py

@@ -0,0 +1,13 @@
+from .config import Config
+from .robot import Robot
+
+
+class Processor:
+    is_safe: bool = True
+
+    def __init__(self, config: Config):
+        self.config: Config = config
+        pass
+
+    def process(self, robot: Robot):
+        pass

onshape_to_robot/processor_ball_to_euler.py

@@ -0,0 +1,75 @@
+import numpy as np
+from .processor import Processor
+from .config import Config
+from .robot import Robot, Link, Joint
+from .message import info
+import fnmatch
+
+
+class ProcessorBallToEuler(Processor):
+    """
+    Turn ball joints into euler roll/pitch/yaw joints.
+    """
+
+    def __init__(self, config: Config):
+        super().__init__(config)
+
+        # Check if it is enabled in configuration
+        self.ball_to_euler: bool | list = config.get("ball_to_euler", False)
+        self.ball_to_euler_order: bool | list = config.get(
+            "ball_to_euler_order",
+            "xyz",
+            values_list=["xyz", "xzy", "zyx", "zxy", "yxz", "yzx"],
+        )
+
+    def should_replace(self, joint: Joint) -> bool:
+        if self.ball_to_euler == True:
+            return True
+        elif isinstance(self.ball_to_euler, list):
+            for entry in self.ball_to_euler:
+                if fnmatch.fnmatch(entry, joint.name):
+                    return True
+        return False
+
+    def process(self, robot: Robot):
+        if self.ball_to_euler:
+            print(info(f"Replacing balls to euler ({self.ball_to_euler})"))
+            replaced_joints: list[Joint] = []
+
+            # Searching for ball joints to replace
+            for joint in robot.joints:
+                if joint.joint_type == Joint.BALL and self.should_replace(joint):
+                    parent = joint.parent
+                    children = {}
+                    for letter in self.ball_to_euler_order[:-1]:
+                        body_name = f"{joint.name}_link_{letter}"
+                        new_link = Link(body_name)
+                        children[letter] = new_link
+                        robot.links.append(new_link)
+                    children[self.ball_to_euler_order[-1]] = joint.child
+
+                    for letter in self.ball_to_euler_order:
+                        if letter == "x":
+                            axis = np.array([1.0, 0.0, 0.0])
+                        elif letter == "y":
+                            axis = np.array([0.0, 1.0, 0.0])
+                        elif letter == "z":
+                            axis = np.array([0.0, 0.0, 1.0])
+
+                        new_joint = Joint(
+                            name=f"{joint.name}_{letter}",
+                            joint_type=Joint.REVOLUTE,
+                            parent=parent,
+                            child=children[letter],
+                            T_world_joint=joint.T_world_joint,
+                            properties=joint.properties.copy(),
+                            axis=axis,
+                        )
+                        robot.joints.append(new_joint)
+                        parent = children[letter]
+
+                    replaced_joints.append(joint)
+
+            # Removing replaced joints
+            for joint in replaced_joints:
+                robot.joints.remove(joint)

onshape_to_robot/processor_collision_as_visual.py

@@ -0,0 +1,35 @@
+from pathlib import Path
+from .message import bright, info, error, warning
+from .processor import Processor
+from .config import Config
+from .robot import Robot, Part
+from .geometry import Mesh
+import numpy as np
+
+
+class ProcessorCollisionAsVisual(Processor):
+    """
+    This processor will update the part mesh and shapes to turn every collision into visual.
+
+    Can be useful for debugging
+    """
+
+    def __init__(self, config: Config):
+        super().__init__(config)
+
+        # OpenSCAD pure shapes
+        self.collisions_as_visual: bool = config.get("collisions_as_visual", False)
+
+    def process(self, robot: Robot):
+        """
+        Runs the processor
+        """
+        if self.collisions_as_visual:
+            print(info("+ Converting collisions to visual"))
+            for link in robot.links:
+                for part in link.parts:
+                    for mesh in part.meshes:
+                        mesh.visual = mesh.collision
+                    for shape in part.shapes:
+                        shape.visual = shape.collision
+                    part.prune_unused_geometry()

onshape_to_robot/processor_convex_decomposition.py

@@ -0,0 +1,120 @@
+import hashlib
+import os
+from pathlib import Path
+from .message import bright, info, error, warning
+from .processor import Processor
+from .config import Config
+from .robot import Robot, Part
+from .geometry import Mesh
+import numpy as np
+import pickle
+
+
+class ProcessorConvexDecomposition(Processor):
+    """
+    Convex decomposition processor. Runs CoACD algorithm on collision meshes to use a convex approximation.
+    """
+
+    is_safe: bool = False
+
+    def __init__(self, config: Config):
+        super().__init__(config)
+
+        # Enable convex decomposition
+        self.convex_decomposition: bool = config.get("convex_decomposition", False)
+        self.rainbow_colors: bool = config.get("rainbow_colors", False)
+
+        self.check_coacd()
+
+    def get_cache_path(self) -> Path:
+        """
+        Return the path to the user cache.
+        """
+        path = Path.home() / ".cache" / "onshape-to-robot-convex-decomposition"
+        path.mkdir(parents=True, exist_ok=True)
+        return path
+
+    def check_coacd(self):
+        if self.convex_decomposition:
+            print(bright("* Checking CoACD presence..."))
+            try:
+                import coacd
+                import trimesh
+            except ImportError:
+                print(bright("Can't import CoACD, disabling convex decomposition."))
+                print(info("TIP: consider installing CoACD:"))
+                print(info("pip install coacd trimesh"))
+                self.convex_decomposition = False
+
+    def process(self, robot: Robot):
+        if self.convex_decomposition:
+            os.makedirs(self.config.asset_path("convex_decomposition"), exist_ok=True)
+            getcwd = os.getcwd()
+            os.chdir(self.config.output_directory)
+
+            for link in robot.links:
+                for part in link.parts:
+                    self.convex_decompose(part)
+
+            os.chdir(getcwd)
+
+    def convex_decompose(self, part: Part):
+        import coacd
+        import trimesh
+
+        collision_meshes = [mesh for mesh in part.meshes if mesh.collision]
+        if len(collision_meshes) > 0:
+            if len(collision_meshes) > 1:
+                print(
+                    warning(
+                        f"* Skipping convex decomposition for part {part.name} as it already has multiple collision meshes."
+                    )
+                )
+
+            collision_mesh = collision_meshes[0]
+
+            # Retrieving file SHA1
+            sha1 = hashlib.sha1(open(collision_mesh.filename, "rb").read()).hexdigest()
+            cache_filename = f"{self.get_cache_path()}/{sha1}.pkl"
+
+            if os.path.exists(cache_filename):
+                print(
+                    info(
+                        f"* Loading cached CoACD decomposition cache for part {part.name}"
+                    )
+                )
+                with open(cache_filename, "rb") as f:
+                    meshes = pickle.load(f)
+            else:
+                mesh = trimesh.load(collision_mesh.filename, force="mesh")
+                mesh = coacd.Mesh(mesh.vertices, mesh.faces)
+                meshes = coacd.run_coacd(mesh, max_convex_hull=16)
+                with open(cache_filename, "wb") as f:
+                    pickle.dump(meshes, f)
+
+            part.collision_meshes = []
+            filename = self.config.asset_path(
+                f"convex_decomposition/{part.name}_%05d.stl"
+            )
+            for k, mesh in enumerate(meshes):
+                mesh = trimesh.Trimesh(vertices=mesh[0], faces=mesh[1])
+                mesh.export(filename % k)
+                color = (
+                    np.concatenate([np.random.rand(3), [1.0]]) if self.rainbow_colors else collision_mesh.color
+                )
+                part.meshes.append(
+                    Mesh(
+                        filename % k,
+                        color,
+                        visual=False,
+                        collision=True,
+                    )
+                )
+                part.collision_meshes.append(filename % k)
+
+            collision_mesh.collision = False
+            part.prune_unused_geometry()
+
+            print(
+                info(f"* Decomposed part {part.name} into {len(meshes)} convex shapes.")
+            )

onshape_to_robot/processor_dummy_base_link.py

@@ -0,0 +1,36 @@
+import numpy as np
+from .processor import Processor
+from .config import Config
+from .robot import Robot, Link, Joint
+
+
+class ProcessorDummyBaseLink(Processor):
+    """
+    Fixed links processor.
+    When enabled, crawl all the links, and split them into sublinks containing all one part.
+    """
+
+    def __init__(self, config: Config):
+        super().__init__(config)
+
+        # Check if it is enabled in configuration
+        self.add_dummy_base_link: bool = config.get("add_dummy_base_link", False)
+
+    def process(self, robot: Robot):
+        if self.add_dummy_base_link:
+            new_base_link = Link("base_link")
+            new_base_link.fixed = True
+            robot.links.append(new_base_link)
+
+            for base_link in robot.base_links:
+                robot.joints.append(
+                    Joint(
+                        "base_link_to_" + base_link.name,
+                        "fixed",
+                        new_base_link,
+                        base_link,
+                        np.eye(4),
+                    )
+                )
+
+            robot.base_links = [new_base_link]

onshape_to_robot/processor_fixed_links.py

@@ -0,0 +1,51 @@
+from .processor import Processor
+from .config import Config
+from .robot import Robot, Link, Joint
+from .message import info
+import fnmatch
+
+
+class ProcessorFixedLinks(Processor):
+    """
+    Fixed links processor.
+    When enabled, crawl all the links, and split them into sublinks containing all one part.
+    """
+
+    def __init__(self, config: Config):
+        super().__init__(config)
+
+        # Check if it is enabled in configuration
+        self.use_fixed_links: bool | list = config.get("use_fixed_links", False)
+
+    def should_fix_links(self, link_name: str) -> bool:
+        if self.use_fixed_links == True:
+            return True
+        elif isinstance(self.use_fixed_links, list):
+            for entry in self.use_fixed_links:
+                if fnmatch.fnmatch(link_name, entry):
+                    return True
+        return False
+
+    def process(self, robot: Robot):
+        if self.use_fixed_links:
+            print(info(f"Using fixed links ({self.use_fixed_links})"))
+            new_links = []
+            for link in robot.links:
+                if self.should_fix_links(link.name):
+                    for part in link.parts:
+                        part_link = Link(f"{link.name}_{part.name}")
+                        part_link.parts = [part]
+                        new_links.append([link, part_link])
+                    link.parts = []
+
+            for parent_link, new_link in new_links:
+                robot.links.append(new_link)
+                robot.joints.append(
+                    Joint(
+                        f"{new_link.name}_fixed",
+                        Joint.FIXED,
+                        parent_link,
+                        new_link,
+                        new_link.parts[0].T_world_part,
+                    )
+                )

onshape_to_robot/processor_merge_parts.py

@@ -0,0 +1,159 @@
+import numpy as np
+import os
+from .config import Config
+from .robot import Robot, Link, Part
+from .processor import Processor
+from .geometry import Mesh
+from .message import bright, info, error
+from stl import mesh, Mode
+
+
+class ProcessorMergeParts(Processor):
+    """
+    This processor merge all parts into a single one, combining the STL
+    """
+
+    def __init__(self, config: Config):
+        super().__init__(config)
+        self.merge_stls = config.get("merge_stls", False)
+
+    def process(self, robot: Robot):
+        if self.merge_stls:
+            os.makedirs(self.config.asset_path("merged"), exist_ok=True)
+            getcwd = os.getcwd()
+            os.chdir(self.config.output_directory)
+            for link in robot.links:
+                self.merge_parts(link)
+            os.chdir(getcwd)
+
+    def load_mesh(self, stl_file: str) -> mesh.Mesh:
+        return mesh.Mesh.from_file(stl_file)
+
+    def save_mesh(self, mesh: mesh.Mesh, stl_file: str):
+        # Tweaking STL header to avoid timestamp
+        # This ensures that same process will result in same STL file
+        def get_header(name):
+            header = "onshape-to-robot"
+            return header[:80].ljust(80, " ")
+
+        mesh.get_header = get_header
+        mesh.save(stl_file, mode=Mode.BINARY)
+
+    def transform_mesh(self, mesh: mesh.Mesh, matrix: np.ndarray):
+        rotation = matrix[:3, :3]
+        translation = matrix[:3, 3]
+
+        def transform(points):
+            return (rotation @ points.T).T + translation
+
+        mesh.v0 = transform(mesh.v0)
+        mesh.v1 = transform(mesh.v1)
+        mesh.v2 = transform(mesh.v2)
+        mesh.normals = transform(mesh.normals)
+
+    def combine_meshes(self, m1: mesh.Mesh, m2: mesh.Mesh):
+        return mesh.Mesh(np.concatenate([m1.data, m2.data]))
+
+    def merge_parts(self, link: Link):
+        print(info(f"+ Merging parts for {link.name}"))
+
+        merge_everything = (
+            self.merge_stls != "collision" and self.merge_stls != "visual"
+        )
+
+        # Computing the frame where the new part will be located at
+        _, com, __ = link.get_dynamics()
+        T_world_com = np.eye(4)
+        T_world_com[:3, 3] = com
+
+        # Computing a new color, weighting by masses
+        color = np.zeros(4)
+        total_mass = 0
+        for part in link.parts:
+            if len(part.meshes):
+                meshes_color = np.mean([mesh.color for mesh in part.meshes], axis=0)
+                color += meshes_color * part.mass
+            total_mass += part.mass
+
+        color /= total_mass
+
+        # Changing shapes frame
+        merged_shapes = []
+        for part in link.parts:
+            if part.shapes is not None:
+                for shape in part.shapes:
+                    if merge_everything or shape.is_type(self.merge_stls):
+                        # Changing the shape frame
+                        T_world_shape = part.T_world_part @ shape.T_part_shape
+                        shape.T_part_shape = np.linalg.inv(T_world_com) @ T_world_shape
+                        merged_shapes.append(shape)
+
+        # Merging STL files
+        def accumulate_meshes(which: str):
+            mesh = None
+            for part in link.parts:
+                for part_mesh in part.meshes:
+                    if part_mesh.is_type(which):
+                        if which == "visual":
+                            part_mesh.visual = False
+                        else:
+                            part_mesh.collision = False
+
+                        # Retrieving meshes
+                        part_mesh = self.load_mesh(part_mesh.filename)
+
+                        # Expressing meshes in the merged frame
+                        T_com_part = np.linalg.inv(T_world_com) @ part.T_world_part
+                        self.transform_mesh(part_mesh, T_com_part)
+
+                        if mesh is None:
+                            mesh = part_mesh
+                        else:
+                            mesh = self.combine_meshes(mesh, part_mesh)
+            return mesh
+
+        merged_meshes = []
+
+        if self.merge_stls != "collision":
+            visual_mesh = accumulate_meshes("visual")
+            if visual_mesh is not None:
+                filename = self.config.asset_path(
+                    "merged/" + "/" + link.name + "_visual.stl"
+                )
+                self.save_mesh(visual_mesh, filename)
+                merged_meshes.append(
+                    Mesh(os.path.relpath(filename, self.config.output_directory), color, visual=True, collision=False)
+                )
+
+        if self.merge_stls != "visual":
+            collision_mesh = accumulate_meshes("collision")
+            if collision_mesh is not None:
+                filename = self.config.asset_path(
+                    "merged/" + "/" + link.name + "_collision.stl"
+                )
+                self.save_mesh(collision_mesh, filename)
+                merged_meshes.append(
+                    Mesh(os.path.relpath(filename, self.config.output_directory), color, visual=False, collision=True)
+                )
+
+        mass, com, inertia = link.get_dynamics(T_world_com)
+        if merge_everything:
+            # Remove all parts
+            link.parts = []
+        else:
+            # We keep the existing parts and add a massless part with merged meshes
+            mass = 0
+            inertia *= 0
+
+        # Replacing parts with a single one
+        link.parts.append(
+            Part(
+                f"{link.name}_parts",
+                T_world_com,
+                mass,
+                com,
+                inertia,
+                merged_meshes,
+                merged_shapes,
+            )
+        )

onshape_to_robot/processor_no_collision_meshes.py

@@ -0,0 +1,29 @@
+from pathlib import Path
+from .message import bright, info, error, warning
+from .processor import Processor
+from .config import Config
+from .robot import Robot, Part
+
+
+class ProcessorNoCollisionMeshes(Processor):
+    """
+    This processor ensures no collision meshes are present in the robot
+    """
+
+    def __init__(self, config: Config):
+        super().__init__(config)
+
+        # OpenSCAD pure shapes
+        self.no_collision_meshes: bool = config.get("no_collision_meshes", False)
+
+    def process(self, robot: Robot):
+        """
+        Runs the processor
+        """
+        if self.no_collision_meshes:
+            print(info("+ Removing collision meshes"))
+            for link in robot.links:
+                for part in link.parts:
+                    for mesh in part.meshes:
+                        mesh.collision = False
+                    part.prune_unused_geometry()

onshape_to_robot/processor_scad.py

@@ -0,0 +1,192 @@
+import subprocess
+import re
+import json
+import os
+from .message import bright, info, error
+from .processor import Processor
+from .config import Config
+from .robot import Robot
+from .geometry import Box, Cylinder, Sphere, Shape
+import numpy as np
+
+
+class ProcessorScad(Processor):
+    """
+    Scad processor. This processor will parse OpenSCAD files to create pure shapes when available.
+
+    The code is a naive parser of the intermediate CSG file produced by OpenSCAD, gathering Box, Sphere and Cylinders.
+    """
+
+    is_safe: bool = False
+
+    def __init__(self, config: Config):
+        super().__init__(config)
+
+        # OpenSCAD pure shapes
+        self.use_scads: bool = config.get("use_scads", False)
+        self.pure_shape_dilatation: float = config.get("pure_shape_dilatation", 0.0)
+
+        if self.use_scads:
+            self.check_openscad()
+
+    def check_openscad(self):
+        if self.use_scads:
+            print(bright("* Checking OpenSCAD presence..."))
+            try:
+                subprocess.run(["openscad", "-v"])
+            except FileNotFoundError:
+                print(bright("Can't run openscad -v, disabling OpenSCAD support"))
+                print(info("TIP: consider installing openscad:"))
+                print(info("Linux:"))
+                print(info("sudo add-apt-repository ppa:openscad/releases"))
+                print(info("sudo apt-get update"))
+                print(info("sudo apt-get install openscad"))
+                print(info("Windows:"))
+                print(info("go to: https://openscad.org/downloads.html "))
+                self.use_scads = False
+
+    def process(self, robot: Robot):
+        if self.use_scads:
+            getcwd = os.getcwd()
+            os.chdir(self.config.output_directory)
+            print(info("+ Parsing OpenSCAD files..."))
+            for link in robot.links:
+                for part in link.parts:
+                    converted_meshes = []
+                    for mesh in part.meshes:
+                        if mesh.collision:
+                            scad_file = mesh.filename.replace(".stl", ".scad")
+                            if os.path.exists(scad_file):
+                                part.shapes += self.parse_scad(scad_file, mesh.color)
+                            converted_meshes.append(mesh)
+
+                    for converted_mesh in converted_meshes:
+                        converted_mesh.collision = False
+                        part.prune_unused_geometry()
+            os.chdir(getcwd)
+
+    def multmatrix_parse(self, parameters: str):
+        matrix = np.array(json.loads(parameters), dtype=float)
+        matrix[0, 3] /= 1000.0
+        matrix[1, 3] /= 1000.0
+        matrix[2, 3] /= 1000.0
+
+        return matrix
+
+    def cube_parse(self, parameters: str):
+        results = re.findall(r"^size = (.+), center = (.+)$", parameters)
+        if len(results) != 1:
+            raise Exception(f"! Can't parse CSG cube parameters: {parameters}")
+        extra = np.array([self.pure_shape_dilatation] * 3)
+
+        return (
+            extra + np.array(json.loads(results[0][0]), dtype=float) / 1000.0
+        ), results[0][1] == "true"
+
+    def cylinder_parse(self, parameters: str):
+        results = re.findall(
+            r"h = (.+), r1 = (.+), r2 = (.+), center = (.+)", parameters
+        )
+        if len(results) != 1:
+            raise Exception(f"! Can't parse CSG cylinder parameters: {parameters}")
+        result = results[0]
+        extra = np.array([self.pure_shape_dilatation / 2, self.pure_shape_dilatation])
+
+        return (extra + np.array([result[0], result[1]], dtype=float) / 1000.0), result[
+            3
+        ] == "true"
+
+    def sphere_parse(self, parameters: str):
+        results = re.findall(r"r = (.+)$", parameters)
+        if len(results) != 1:
+            raise Exception(f"! Can't parse CSG sphere parameters: {parameters}")
+
+        return self.pure_shape_dilatation + float(results[0]) / 1000.0
+
+    def extract_node_parameters(self, line: str):
+        line = line.strip()
+        parts = line.split("(", 1)
+        node = parts[0]
+        parameters = parts[1]
+        if parameters[-1] == ";":
+            parameters = parameters[:-2]
+        if parameters[-1] == "{":
+            parameters = parameters[:-3]
+        return node, parameters
+
+    def translation(self, x: float, y: float, z: float):
+        m = np.eye(4)
+        m[:3, 3] = [x, y, z]
+
+        return m
+
+    def parse_csg(self, csg_data: str, color):
+        shapes: list[Shape] = []
+        lines = csg_data.split("\n")
+        matrices = []
+
+        for line in lines:
+            line = line.strip()
+            if line != "":
+                if line[-1] == "{":
+                    node, parameters = self.extract_node_parameters(line)
+                    if node == "multmatrix":
+                        matrix = self.multmatrix_parse(parameters)
+                    else:
+                        matrix = np.eye(4)
+                    matrices.append(matrix)
+                elif line[-1] == "}":
+                    matrices.pop()
+                else:
+                    node, parameters = self.extract_node_parameters(line)
+
+                    transform = np.eye(4)
+                    for matrix in matrices:
+                        transform = transform @ matrix
+
+                    if node == "cube":
+                        size, center = self.cube_parse(parameters)
+                        if not center:
+                            transform = transform @ self.translation(
+                                size[0] / 2.0, size[1] / 2.0, size[2] / 2.0
+                            )
+                        shapes.append(
+                            Box(transform, size, color, visual=False, collision=True)
+                        )
+                    if node == "cylinder":
+                        size, center = self.cylinder_parse(parameters)
+                        if not center:
+                            transform = transform @ self.translation(
+                                0, 0, size[0] / 2.0
+                            )
+                        shapes.append(
+                            Cylinder(
+                                transform,
+                                size[0],
+                                size[1],
+                                color,
+                                visual=False,
+                                collision=True,
+                            )
+                        )
+                    if node == "sphere":
+                        shapes.append(
+                            Sphere(
+                                transform,
+                                self.sphere_parse(parameters),
+                                color,
+                                visual=False,
+                                collision=True,
+                            )
+                        )
+
+        return shapes
+
+    def parse_scad(self, scad_file: str, color: np.ndarray):
+        tmp_data = os.getcwd() + "/_tmp_data.csg"
+        os.system("openscad " + scad_file + " -o " + tmp_data)
+        with open(tmp_data, "r", encoding="utf-8") as stream:
+            data = stream.read()
+        os.system("rm " + tmp_data)
+
+        return self.parse_csg(data, color)

onshape_to_robot/processor_simplify_stls.py

@@ -0,0 +1,89 @@
+import os
+from .config import Config
+from .robot import Robot
+from .processor import Processor
+from .message import bright, info, error
+
+
+class ProcessorSimplifySTLs(Processor):
+    """
+    Allow for mesh simplifications using MeshLab
+    """
+
+    def __init__(self, config: Config):
+        super().__init__(config)
+
+        # STL merge / simplification
+        self.simplify_stls = config.get("simplify_stls", False)
+        self.max_stl_size = config.get("max_stl_size", 3)
+
+        if self.simplify_stls:
+            self.pymeshlab = self.check_meshlab()
+
+    def check_meshlab(self):
+        print(bright("* Checking pymeshlab presence..."))
+        try:
+            import pymeshlab
+
+            return pymeshlab
+        except ImportError:
+            self.simplify_stls = False
+            print(error("No pymeshlab, disabling STL simplification support"))
+            print(info("TIP: consider installing pymeshlab:"))
+            print(info("pip install pymeshlab"))
+
+    def process(self, robot: Robot):
+        if self.simplify_stls:
+            simplify_all = (
+                self.simplify_stls != "visual" and self.simplify_stls != "collision"
+            )
+            simplified = set()
+            getcwd = os.getcwd()
+            # Changing directory to output to have relative paths working
+            os.chdir(self.config.output_directory)
+            for link in robot.links:
+                for part in link.parts:
+                    for mesh in part.meshes:
+                        if (
+                            simplify_all or mesh.is_type(self.simplify_stls)
+                        ) and mesh.filename not in simplified:
+                            simplified.add(mesh.filename)
+                            self.simplify_stl(mesh.filename)
+            os.chdir(getcwd)
+
+    def reduce_faces(self, filename: str, reduction: float = 0.9):
+        mesh_set = self.pymeshlab.MeshSet()
+
+        # Add input mesh
+        mesh_set.load_new_mesh(filename)
+
+        # Apply filter
+        mesh_set.apply_filter(
+            "meshing_decimation_quadric_edge_collapse",
+            targetperc=reduction,
+            qualitythr=0.5,
+            preserveboundary=False,
+            boundaryweight=1,
+            preservenormal=True,
+            preservetopology=False,
+            optimalplacement=True,
+            planarquadric=True,
+            qualityweight=False,
+            planarweight=0.001,
+            autoclean=True,
+            selected=False,
+        )
+
+        # Save mesh
+        mesh_set.save_current_mesh(filename)
+
+    def simplify_stl(self, filename: str):
+        size_M = os.path.getsize(filename) / (1024 * 1024)
+
+        if size_M > self.max_stl_size:
+            print(
+                info(
+                    f"+ {os.path.basename(filename)} is {size_M:.2f} M, running mesh simplification"
+                )
+            )
+            self.reduce_faces(filename, self.max_stl_size / size_M)

onshape_to_robot/processors.py

@@ -0,0 +1,21 @@
+from .processor_merge_parts import ProcessorMergeParts
+from .processor_scad import ProcessorScad
+from .processor_simplify_stls import ProcessorSimplifySTLs
+from .processor_fixed_links import ProcessorFixedLinks
+from .processor_dummy_base_link import ProcessorDummyBaseLink
+from .processor_convex_decomposition import ProcessorConvexDecomposition
+from .processor_collision_as_visual import ProcessorCollisionAsVisual
+from .processor_no_collision_meshes import ProcessorNoCollisionMeshes
+from .processor_ball_to_euler import ProcessorBallToEuler
+
+default_processors = [
+    ProcessorBallToEuler,
+    ProcessorScad,
+    ProcessorMergeParts,
+    ProcessorSimplifySTLs,
+    ProcessorFixedLinks,
+    ProcessorDummyBaseLink,
+    ProcessorConvexDecomposition,
+    ProcessorNoCollisionMeshes,
+    ProcessorCollisionAsVisual,
+]

onshape_to_robot/pure_sketch.py

@@ -0,0 +1,157 @@
+def main():
+    import numpy as np
+    import math
+    import commentjson as json
+    import os
+    import sys, os
+    from dotenv import load_dotenv, find_dotenv
+    from colorama import Fore, Back, Style
+
+    load_dotenv(find_dotenv(usecwd=True))
+
+    if len(sys.argv) < 2:
+        print("Usage: onshape-to-robot-pure-shape {STL file} [prefix=PureShapes]")
+    else:
+        fileName = sys.argv[1]
+        robotDir = os.path.dirname(fileName)
+        configFile = os.path.join(robotDir, "config.json")
+        prefix = "PureShapes"
+        if len(sys.argv) > 2:
+            prefix = sys.argv[2]
+
+        from .onshape_api.client import Client
+
+        client = Client(logging=False, creds=configFile)
+
+        parts = fileName.split(".")
+        parts[-1] = "part"
+        partFileName = ".".join(parts)
+        parts[-1] = "scad"
+        scadFileName = ".".join(parts)
+
+        with open(partFileName, "r", encoding="utf-8") as stream:
+            part = json.load(stream)
+        partid = part["partId"]
+        result = client.get_sketches(
+            part["documentId"],
+            part["documentMicroversion"],
+            part["elementId"],
+            part["configuration"],
+        )
+
+        scad = '% scale(1000) import("' + os.path.basename(fileName) + '");\n'
+
+        sketchDatas = []
+        for sketch in result["sketches"]:
+            if sketch["sketch"].startswith(prefix):
+                parts = sketch["sketch"].split(" ")
+                if len(parts) >= 2:
+                    sketch["thickness"] = float(parts[1])
+                else:
+                    print(
+                        Fore.RED
+                        + 'ERROR: The sketch name should contain extrusion size (e.g "PureShapes 5.3")'
+                        + Style.RESET_ALL
+                    )
+                    exit(0)
+                sketchDatas.append(sketch)
+
+        if len(sketchDatas):
+            print(
+                Fore.GREEN
+                + "* Found "
+                + str(len(sketchDatas))
+                + " PureShapes sketches"
+                + Style.RESET_ALL
+            )
+            for sketchData in sketchDatas:
+                # Retrieving sketch transform matrix
+                m = sketchData["transformMatrix"]
+                mm = [m[0:4], m[4:8], m[8:12], m[12:16]]
+                mm[0][3] *= 1000
+                mm[1][3] *= 1000
+                mm[2][3] *= 1000
+                scad += "\n"
+                scad += "// Sketch " + sketchData["sketch"] + "\n"
+                scad += "multmatrix(" + str(mm) + ") {" + "\n"
+                scad += "thickness = %f;\n" % sketchData["thickness"]
+                scad += "translate([0, 0, -thickness]) {\n"
+
+                boxes = {}
+
+                def boxSet(id, pointName, point):
+                    if id not in boxes:
+                        boxes[id] = {}
+                    boxes[id][pointName] = point
+
+                for entry in sketchData["geomEntities"]:
+                    if entry["entityType"] == "circle":
+                        center = entry["center"]
+                        scad += "  translate([%f, %f, 0]) {\n" % (
+                            center[0] * 1000,
+                            center[1] * 1000,
+                        )
+                        scad += "    cylinder(r=%f,h=thickness);\n" % (
+                            entry["radius"] * 1000
+                        )
+                        scad += "  }\n"
+                    if entry["entityType"] == "point":
+                        parts = entry["id"].split(".")
+                        if len(parts) == 3:
+                            if parts[1] == "top" and parts[2] == "start":
+                                boxSet(parts[0], "A", entry["point"])
+                            if parts[1] == "top" and parts[2] == "end":
+                                boxSet(parts[0], "B", entry["point"])
+                            if parts[1] == "bottom" and parts[2] == "start":
+                                boxSet(parts[0], "C", entry["point"])
+                            if parts[1] == "bottom" and parts[2] == "end":
+                                boxSet(parts[0], "D", entry["point"])
+
+                for id in boxes:
+                    if len(boxes[id]) == 4:
+                        A, B = np.array(boxes[id]["A"]), np.array(boxes[id]["B"])
+                        C, D = np.array(boxes[id]["C"]), np.array(boxes[id]["D"])
+                        AB = B - A
+
+                        # Making sure that the orientation of the square is correct
+                        AB90 = np.array([-AB[1], AB[0]])
+                        side = AB90.dot(C - A)
+                        width = np.linalg.norm(B - A)
+                        height = np.linalg.norm(B - D)
+                        if side < 0:
+                            A, B, C, D = C, D, A, B
+
+                        AB = B - A
+                        alpha = np.rad2deg(math.atan2(AB[1], AB[0]))
+                        scad += "  translate([%f, %f, 0]) {\n" % (
+                            A[0] * 1000,
+                            A[1] * 1000,
+                        )
+                        scad += "    rotate([0, 0, " + str(alpha) + "]) {" + "\n"
+                        scad += "      cube([%f, %f, thickness]);\n" % (
+                            width * 1000,
+                            height * 1000,
+                        )
+                        scad += "    }\n"
+                        scad += "  }\n"
+
+                scad += "}\n"
+                scad += "}\n"
+
+            with open(scadFileName, "w", encoding="utf-8") as stream:
+                stream.write(scad)
+
+            directory = os.path.dirname(fileName)
+            os.system(
+                "cd " + directory + "; openscad " + os.path.basename(scadFileName)
+            )
+        else:
+            print(
+                Fore.RED
+                + "ERROR: Can't find pure shape sketch in this part"
+                + Style.RESET_ALL
+            )
+
+
+if __name__ == "__main__":
+    main()

onshape_to_robot/robot.py

@@ -0,0 +1,170 @@
+from __future__ import annotations
+from copy import deepcopy
+import numpy as np
+from .geometry import Shape, Mesh
+
+
+class Part:
+    """
+    A part is a single component of a link.
+    """
+
+    def __init__(
+        self,
+        name: str,
+        T_world_part: np.ndarray,
+        mass: float,
+        com: np.ndarray,
+        inertia: np.ndarray,
+        meshes: list[Mesh] = [],
+        shapes: list[Shape] = [],
+    ):
+        self.name: str = name
+        self.T_world_part: np.ndarray = T_world_part
+        self.mass: float = mass
+        self.com: np.ndarray = com
+        self.inertia: np.ndarray = inertia
+        self.meshes: list[Mesh] = deepcopy(meshes)
+        self.shapes: list[Shape] = deepcopy(shapes)
+
+    def prune_unused_geometry(self):
+        """
+        Remove meshes or shapes that are neither visual nor collision.
+        """
+        self.meshes = [mesh for mesh in self.meshes if (mesh.visual or mesh.collision)]
+        self.shapes = [
+            shape for shape in self.shapes if (shape.visual or shape.collision)
+        ]
+
+
+class Link:
+    """
+    A link of a robot.
+    """
+
+    def __init__(self, name: str):
+        self.name = name
+        self.parts: list[Part] = []
+        self.frames: dict[str, np.ndarray] = {}
+        self.fixed: bool = False
+
+    def get_dynamics(self, T_world_frame: np.ndarray = np.eye(4)):
+        """
+        Returns the dynamics (mass, com, inertia) in a given frame.
+        The CoM is expressed in the required frame.
+        Inertia is expressed around the CoM, aligned with the required frame.
+        """
+        mass = 0
+        com = np.zeros(3)
+        inertia = np.zeros((3, 3))
+        T_frame_world = np.linalg.inv(T_world_frame)
+
+        for part in self.parts:
+            T_frame_part = T_frame_world @ part.T_world_part
+            com_frame = (T_frame_part @ [*part.com, 1])[:3]
+            com += com_frame * part.mass
+            mass += part.mass
+
+        if mass > 1e-9:
+            com /= mass
+
+        for part in self.parts:
+            T_frame_part = T_frame_world @ part.T_world_part
+            com_frame = (T_frame_part @ [*part.com, 1])[:3]
+            R = T_frame_part[:3, :3]
+            q = (com_frame - com).reshape((3, 1))
+            # See Modern Robotics, (8.26) & (8.27)
+            inertia += (
+                R @ part.inertia @ R.T + ((q.T @ q) * np.eye(3) - q @ q.T) * part.mass
+            )
+
+        return mass, com, inertia
+
+
+class Relation:
+    """
+    Represents a relation (for example a gear) with a source joint
+    """
+
+    def __init__(self, source_joint: str, ratio: float):
+        self.source_joint: str = source_joint
+        self.ratio: float = ratio
+
+
+class Joint:
+    """
+    A joint connects two links.
+    """
+
+    # Joint types
+    FIXED = "fixed"
+    REVOLUTE = "revolute"
+    PRISMATIC = "prismatic"
+    CONTINUOUS = "continuous"
+    BALL = "ball"
+
+    def __init__(
+        self,
+        name: str,
+        joint_type: str,
+        parent: Link,
+        child: Link,
+        T_world_joint: np.ndarray,
+        properties: dict = {},
+        limits: tuple[float, float] | None = None,
+        axis: np.ndarray = np.array([0.0, 0.0, 1.0]),
+    ):
+        self.name: str = name
+        self.joint_type: str = joint_type
+        self.properties: dict = properties
+        self.parent: Link = parent
+        self.child: Link = child
+        self.limits: tuple[float, float] | None = limits
+        self.axis: np.ndarray = axis
+        self.T_world_joint: np.ndarray = T_world_joint
+        self.relation: Relation | None = None
+
+
+class Closure:
+    """
+    A kinematics closure
+    """
+
+    FIXED = "fixed"
+    REVOLUTE = "revolute"
+    BALL = "ball"
+    SLIDER = "slider"
+
+    def __init__(self, closure_type: str, frame1: str, frame2: str):
+        self.closure_type: str = closure_type
+        self.frame1: str = frame1
+        self.frame2: str = frame2
+
+
+class Robot:
+    """
+    Robot representation produced after requesting Onshape API, and before
+    exporting (e.g URDF, MuJoCo).
+    """
+
+    def __init__(self, name: str):
+        self.name: str = name
+        self.links: list[Link] = []
+        self.base_links: list[Link] = []
+        self.joints: list[Joint] = []
+        self.closures: list[Closure] = []
+
+    def get_link(self, name: str):
+        for link in self.links:
+            if link.name == name:
+                return link
+        raise ValueError(f"Link {name} not found")
+
+    def get_joint(self, name: str):
+        for joint in self.joints:
+            if joint.name == name:
+                return joint
+        raise ValueError(f"Joint {name} not found")
+
+    def get_link_joints(self, link: Link):
+        return [joint for joint in self.joints if joint.parent == link]

onshape_to_robot/robot_builder.py

@@ -0,0 +1,456 @@
+import numpy as np
+import os
+import hashlib
+import json
+import fnmatch
+from .geometry import Mesh
+from .message import warning, info, success, error, dim, bright
+from .assembly import Assembly
+from .config import Config
+from .robot import Part, Joint, Link, Robot, Relation, Closure
+from .csg import process as csg_process
+
+
+class RobotBuilder:
+    def __init__(self, config: Config):
+        self.config: Config = config
+        self.assembly: Assembly = Assembly(config)
+        self.robot: Robot = Robot(config.robot_name)
+
+        for closure_type, frame1, frame2 in self.assembly.closures:
+            self.robot.closures.append(Closure(closure_type, frame1, frame2))
+
+        self.unique_names = {}
+        self.stl_filenames: dict = {}
+
+        for node in self.assembly.root_nodes:
+            link = self.build_robot(node)
+            self.robot.base_links.append(link)
+
+    def part_is_ignored(self, name: str, what: str) -> bool:
+        """
+        Checks if a given part should be ignored by config
+        """
+        ignored = False
+
+        # Removing <1>, <2> etc. suffix
+        name = "<".join(name.split("<")[:-1]).strip()
+
+        for entry in self.config.ignore:
+            to_ignore = True
+            match_entry = entry
+            if entry[0] == "!":
+                to_ignore = False
+                match_entry = entry[1:]
+
+            if fnmatch.fnmatch(name.lower(), match_entry.lower()):
+                if (
+                    self.config.ignore[entry] == "all"
+                    or self.config.ignore[entry] == what
+                ):
+                    ignored = to_ignore
+
+        return ignored
+
+    def slugify(self, value: str) -> str:
+        """
+        Turns a value into a slug
+        """
+        return "".join(c if c.isalnum() else "_" for c in value).strip("_")
+
+    def printable_configuration(self, instance: dict) -> str:
+        """
+        Retrieve configuration enums to replace "List_..." with proper enum names
+        """
+        configuration = instance["configuration"]
+
+        if instance["configuration"] != "default":
+            if "documentVersion" in instance:
+                version = instance["documentVersion"]
+                wmv = "v"
+            else:
+                version = instance["documentMicroversion"]
+                wmv = "m"
+            elements = self.assembly.client.elements_configuration(
+                instance["documentId"],
+                version,
+                instance["elementId"],
+                wmv=wmv,
+                linked_document_id=self.config.document_id,
+            )
+            for entry in elements["configurationParameters"]:
+                type_name = entry["typeName"]
+                message = entry["message"]
+
+                if type_name.startswith("BTMConfigurationParameterEnum"):
+                    parameter_name = message["parameterName"]
+                    parameter_id = message["parameterId"]
+                    configuration = configuration.replace(parameter_id, parameter_name)
+
+        return configuration
+
+    def part_name(self, part: dict, include_configuration: bool = False) -> str:
+        """
+        Retrieve the name from a part.
+        i.e "Base link <1>" -> "base_link"
+        """
+        name = part["name"]
+        parts = name.split(" ")
+        del parts[-1]
+        base_part_name = self.slugify("_".join(parts).lower())
+
+        if not include_configuration:
+            return base_part_name
+
+        # Only add configuration to name if its not default and not a very long configuration (which happens for library parts like screws)
+        configuration = self.printable_configuration(part)
+        if configuration != "default" and self.config.include_configuration_suffix:
+            if len(configuration) < 40:
+                parts += ["_" + configuration.replace("=", "_").replace(" ", "_")]
+            else:
+                parts += ["_" + hashlib.md5(configuration.encode("utf-8")).hexdigest()]
+
+        return self.slugify("_".join(parts).lower())
+
+    def unique_name(self, part: dict, type: str):
+        """
+        Get unique part name (plate, plate_2, plate_3, ...)
+        In the case where multiple parts have the same name in Onshape, they will result in different names in the URDF
+        """
+        while True:
+            name = self.part_name(part, include_configuration=True)
+
+            if type not in self.unique_names:
+                self.unique_names[type] = {}
+
+            if name in self.unique_names[type]:
+                self.unique_names[type][name] += 1
+                name = f"{name}_{self.unique_names[type][name]}"
+            else:
+                self.unique_names[type][name] = 1
+                name = name
+
+            if name not in [frame.name for frame in self.assembly.frames]:
+                return name
+
+    def instance_request_params(self, instance: dict) -> dict:
+        """
+        Build parameters to make an API call for a given instance
+        """
+        params = {}
+
+        if "documentVersion" in instance:
+            params["wmvid"] = instance["documentVersion"]
+            params["wmv"] = "v"
+        else:
+            params["wmvid"] = instance["documentMicroversion"]
+            params["wmv"] = "m"
+
+        params["did"] = instance["documentId"]
+        params["eid"] = instance["elementId"]
+        params["linked_document_id"] = self.config.document_id
+        params["configuration"] = instance["configuration"]
+
+        return params
+
+    def get_stl_filename(self, instance: dict) -> str:
+        """
+        Get a STL filename unique to the instance
+        """
+        exact_instance = (
+            instance["documentId"],
+            instance["documentMicroversion"],
+            instance["elementId"],
+            instance["configuration"],
+            instance["partId"],
+        )
+
+        if exact_instance not in self.stl_filenames:
+            part_name_config = self.part_name(instance, True)
+            stl_filename = part_name_config
+            k = 1
+            while stl_filename in self.stl_filenames.values():
+                k += 1
+                stl_filename = part_name_config + f"__{k}"
+            if k != 1:
+                print(
+                    warning(
+                        f'WARNING: Parts with same name "{part_name_config}", incrementing STL name to "{stl_filename}"'
+                    )
+                )
+            self.stl_filenames[exact_instance] = stl_filename
+
+        return self.stl_filenames[exact_instance]
+
+    def get_stl(self, instance: dict) -> str:
+        """
+        Download and store STL file
+        """
+        os.makedirs(self.config.asset_path(""), exist_ok=True)
+
+        stl_filename = self.get_stl_filename(instance)
+        filename = stl_filename + ".stl"
+
+        params = self.instance_request_params(instance)
+        stl = self.assembly.client.part_studio_stl_m(
+            **params,
+            partid=instance["partId"],
+        )
+        with open(self.config.asset_path(filename), "wb") as stream:
+            stream.write(stl)
+
+        # Storing metadata for imported instances in the .part file
+        stl_metadata = stl_filename + ".part"
+        with open(
+            self.config.asset_path(stl_metadata), "w", encoding="utf-8"
+        ) as stream:
+            json.dump(instance, stream, indent=4, sort_keys=True)
+
+        return self.config.asset_path(filename)
+
+    def get_color(self, instance: dict) -> np.ndarray:
+        """
+        Retrieve the color of a part
+        """
+        if self.config.color is not None:
+            color = np.array(self.config.color)
+        else:
+            params = self.instance_request_params(instance)
+            metadata = self.assembly.client.part_get_metadata(
+                **params,
+                partid=instance["partId"],
+            )
+
+            color = np.array([0.5, 0.5, 0.5, 1.0])
+
+            # XXX: There must be a better way to retrieve the part color
+            for entry in metadata["properties"]:
+                if (
+                    "value" in entry
+                    and type(entry["value"]) is dict
+                    and "color" in entry["value"]
+                ):
+                    rgb = entry["value"]["color"]
+                    a = entry["value"]["opacity"]
+                    color = np.array([rgb["red"], rgb["green"], rgb["blue"], a]) / 255.0
+
+        return color
+
+    def get_dynamics(self, instance: dict) -> tuple:
+        """
+        Retrieve the dynamics (mass, com, inertia) of a given instance
+        """
+        if self.config.no_dynamics:
+            mass = 0
+            com = [0] * 3
+            inertia = [0] * 12
+        else:
+            if instance["isStandardContent"]:
+                mass_properties = self.assembly.client.standard_cont_mass_properties(
+                    instance["documentId"],
+                    instance["documentVersion"],
+                    instance["elementId"],
+                    instance["partId"],
+                    configuration=instance["configuration"],
+                    linked_document_id=self.config.document_id,
+                )
+            else:
+                params = self.instance_request_params(instance)
+                mass_properties = self.assembly.client.part_mass_properties(
+                    **params,
+                    partid=instance["partId"],
+                )
+
+            body = mass_properties["bodies"].get(instance["partId"])
+
+            # Surfaces (sheet bodies) have no volume and thus no dynamics. Onshape
+            # reports them with "hasMass": false (and may omit them from "bodies"
+            # entirely), so relying on presence in "bodies" alone misses them.
+            if body is None or not body.get("hasMass", True):
+                print(
+                    warning(
+                        f"WARNING: part {instance['name']} has no dynamics (maybe it is a surface)"
+                    )
+                )
+                return 0.0, np.zeros(3), np.zeros((3, 3))
+
+            mass = body["mass"][0]
+            com = body["centroid"]
+            inertia = body["inertia"]
+
+            if abs(mass) < 1e-9:
+                print(
+                    warning(
+                        f"WARNING: part {instance['name']} has no mass, maybe you should assign a material to it ?"
+                    )
+                )
+
+        return mass, com[:3], np.reshape(inertia[:9], (3, 3))
+
+    def add_part(self, occurrence: dict):
+        """
+        Add a part to the current link
+        """
+        instance = occurrence["instance"]
+
+        if instance["suppressed"]:
+            return
+
+        if instance["partId"] == "":
+            print(warning(f"WARNING: Part '{instance['name']}' has no partId"))
+            return
+
+        part_name = instance["name"]
+        extra = ""
+        if instance["configuration"] != "default":
+            extra = dim(
+                " (configuration: " + self.printable_configuration(instance) + ")"
+            )
+        symbol = "+"
+        if self.part_is_ignored(part_name, "visual") or self.part_is_ignored(
+            part_name, "collision"
+        ):
+            symbol = "-"
+            extra += dim(" / ")
+            if self.part_is_ignored(part_name, "visual"):
+                extra += dim("(ignoring visual)")
+            if self.part_is_ignored(part_name, "collision"):
+                extra += dim(" (ignoring collision)")
+
+        print(success(f"{symbol} Adding part {part_name}{extra}"))
+
+        if self.part_is_ignored(part_name, "visual") and self.part_is_ignored(
+            part_name, "collision"
+        ):
+            stl_file = None
+        else:
+            stl_file = self.get_stl(instance)
+
+        # Obtain metadatas about part to retrieve color
+        color = self.get_color(instance)
+
+        # Obtain the instance dynamics
+        mass, com, inertia = self.get_dynamics(instance)
+
+        # Obtain part pose
+        T_world_part = np.array(occurrence["transform"]).reshape(4, 4)
+
+        # Adding non-ignored meshes
+        meshes = []
+        stl_relpath = (
+            os.path.relpath(stl_file, self.config.output_directory)
+            if stl_file is not None
+            else None
+        )
+        mesh = Mesh(stl_relpath, color)
+        if self.part_is_ignored(part_name, "visual"):
+            mesh.visual = False
+        if self.part_is_ignored(part_name, "collision"):
+            mesh.collision = False
+        if mesh.visual or mesh.collision:
+            meshes.append(mesh)
+
+        # Get unique part name (with _2, _3 suffixes for duplicates)
+        unique_part_name = self.unique_name(instance, "part")
+
+        # Apply geom_properties based on unique part name pattern matching
+        for mesh in meshes:
+            visual_properties = {}
+            collision_properties = {}
+
+            for pattern_name in self.config.geom_properties:
+                if fnmatch.fnmatch(unique_part_name, pattern_name):
+                    pattern_props = self.config.geom_properties[pattern_name]
+
+                    # Check for nested visual/collision structure
+                    has_nested = (
+                        "visual" in pattern_props or "collision" in pattern_props
+                    )
+
+                    if has_nested:
+                        visual_properties = {
+                            **visual_properties,
+                            **pattern_props.get("visual", {}),
+                        }
+                        collision_properties = {
+                            **collision_properties,
+                            **pattern_props.get("collision", {}),
+                        }
+                    else:
+                        # Apply to both if not nested
+                        visual_properties = {**visual_properties, **pattern_props}
+                        collision_properties = {**collision_properties, **pattern_props}
+
+            mesh.visual_properties = visual_properties
+            mesh.collision_properties = collision_properties
+
+        part = Part(
+            unique_part_name,
+            T_world_part,
+            mass,
+            com,
+            inertia,
+            meshes,
+        )
+
+        self.robot.links[-1].parts.append(part)
+
+    def build_robot(self, body_id: int):
+        """
+        Add recursively body nodes to the robot description.
+        """
+        instance = self.assembly.body_instance(body_id)
+
+        if body_id in self.assembly.link_names:
+            link_name = self.assembly.link_names[body_id]
+        else:
+            link_name = self.unique_name(instance, "link")
+
+        # Adding all the parts in the current link
+        link = Link(link_name)
+        self.robot.links.append(link)
+        for occurrence in self.assembly.body_occurrences(body_id):
+            if occurrence["instance"]["type"] == "Part":
+                self.add_part(occurrence)
+            if occurrence["fixed"]:
+                link.fixed = True
+
+        # Adding frames to the link
+        for frame in self.assembly.frames:
+            if frame.body_id == body_id:
+                self.robot.links[-1].frames[frame.name] = frame.T_world_frame
+
+        for children_body in self.assembly.tree_children[body_id]:
+            dof = self.assembly.get_dof(body_id, children_body)
+            child_body = dof.other_body(body_id)
+            T_world_axis = dof.T_world_mate.copy()
+
+            properties = self.config.joint_properties.get("default", {})
+            for joint_name in self.config.joint_properties:
+                if fnmatch.fnmatch(dof.name, joint_name):
+                    properties = {
+                        **properties,
+                        **self.config.joint_properties[joint_name],
+                    }
+
+            joint = Joint(
+                dof.name,
+                dof.joint_type,
+                link,
+                None,
+                T_world_axis,
+                properties,
+                dof.limits,
+                dof.axis,
+            )
+            if dof.name in self.assembly.relations:
+                source, ratio = self.assembly.relations[dof.name]
+                joint.relation = Relation(source, ratio)
+
+            # The joint is added before the recursive call, ensuring items in robot.joints has the
+            # same order as recursive calls on the tree
+            self.robot.joints.append(joint)
+
+            joint.child = self.build_robot(child_body)
+
+        return link

onshape_to_robot/simulation.py

@@ -0,0 +1,541 @@
+from transforms3d.quaternions import mat2quat, quat2mat
+import math
+import sys
+import time
+import numpy as np
+import pybullet as p
+from time import sleep
+import os
+import re
+
+
+class Simulation:
+    """
+    A Bullet simulation involving Onshape to robot model
+    """
+
+    def __init__(self, robotPath, floor=True, fixed=False, transparent=False, gui=True, ignore_self_collisions=False,
+                 realTime=True, panels=False, useUrdfInertia=True, dt=0.002, physicsClient = None):
+        """Creates an instance of humanoid simulation
+
+        Keyword Arguments:
+            field {bool} -- enable the display of the field (default: {False})
+            fixed {bool} -- makes the base of the robot floating/fixed (default: {False})
+            transparent {bool} -- makes the robot transparent (default: {False})
+            gui {bool} -- enables the gui visualizer, if False it will runs headless (default {True})
+            realTime {bool} -- try to have simulation in real time (default {True})
+            panels {bool} -- show/hide the user interaction pyBullet panels (default {False})
+            useUrdfInertia {bool} -- use URDF from URDF file (default {True})
+            dt {float} -- time step (default {0.002})
+        """
+
+        self.dir = os.path.dirname(os.path.abspath(__file__))
+        self.gui = gui
+        self.realTime = realTime
+        self.t = 0
+        self.start = time.time()
+        self.dt = dt
+        self.mass = None
+
+        # Debug lines drawing
+        self.lines = []
+        self.currentLine = 0
+        self.lastLinesDraw = 0
+        self.lineColors = [[1, 0, 0], [0, 1, 0], [
+            0, 0, 1], [1, 1, 0], [1, 0, 1], [0, 1, 1]]
+
+        # Instanciating bullet
+        if physicsClient is None:
+            if gui:
+                physicsClient = p.connect(p.GUI)
+            else:
+                physicsClient = p.connect(p.DIRECT)
+        p.setGravity(0, 0, -9.81)
+
+        # Light GUI
+        if not panels:
+            p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+            p.configureDebugVisualizer(
+                p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
+            p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
+            p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
+
+        p.configureDebugVisualizer(p.COV_ENABLE_MOUSE_PICKING, 1)
+
+        # Loading floor and/or plane ground
+        if floor:
+            self.floor = p.loadURDF(self.dir+'/bullet/plane.urdf')
+        else:
+            self.floor = None
+
+        # Loading robot
+        startPos = [0, 0, 0]
+        if not fixed:
+            startPos[2] = 1
+        startOrientation = p.getQuaternionFromEuler([0, 0, 0])
+        flags = 0 if ignore_self_collisions else p.URDF_USE_SELF_COLLISION
+        if useUrdfInertia:
+            flags += p.URDF_USE_INERTIA_FROM_FILE
+        self.robot = p.loadURDF(robotPath,
+                                startPos, startOrientation,
+                                flags=flags, useFixedBase=fixed)
+
+        # Setting frictions parameters to default ones
+        self.setFloorFrictions()
+
+        # Engine parameters
+        p.setPhysicsEngineParameter(fixedTimeStep=self.dt, maxNumCmdPer1ms=0)
+        # p.setRealTimeSimulation(0)
+        # p.setPhysicsEngineParameter(numSubSteps=1)
+
+        # Retrieving joints and frames
+        self.joints = {}
+        self.passive_joints = {}
+        self.jointsInfos = {}
+        self.jointsIndexes = {}
+        self.frames = {}
+        self.maxTorques = {}
+
+        # Collecting the available joints
+        n = 0
+        for k in range(p.getNumJoints(self.robot)):
+            jointInfo = p.getJointInfo(self.robot, k)
+            name = jointInfo[1].decode('utf-8')
+            
+            if 'passive' in name:
+                self.passive_joints[name] = k
+            elif not name.endswith('_fixed'):
+                if '_frame' in name:
+                    self.frames[name] = k
+                else:
+                    self.jointsIndexes[name] = n
+                    n += 1
+                    self.joints[name] = k
+                    self.jointsInfos[name] = {
+                        'type': jointInfo[2]
+                    }
+                    if jointInfo[8] < jointInfo[9]:
+                        self.jointsInfos[name]['lowerLimit'] = jointInfo[8]
+                        self.jointsInfos[name]['upperLimit'] = jointInfo[9]
+
+        # Changing robot opacity if transparent set to true
+        if transparent:
+            for k in range(p.getNumJoints(self.robot)):
+                p.changeVisualShape(self.robot, k, rgbaColor=[
+                                    0.3, 0.3, 0.3, 0.3])
+
+        print('* Found '+str(len(self.joints))+' DOFs')
+        print('* Found '+str(len(self.frames))+' frames')
+
+    def setFloorFrictions(self, lateral=1, spinning=-1, rolling=-1):
+        """Sets the frictions with the plane object
+
+        Keyword Arguments:
+            lateral {float} -- lateral friction (default: {1.0})
+            spinning {float} -- spinning friction (default: {-1.0})
+            rolling {float} -- rolling friction (default: {-1.0})
+        """
+        if self.floor is not None:
+            p.changeDynamics(self.floor, -1, lateralFriction=lateral,
+                             spinningFriction=spinning, rollingFriction=rolling)
+
+    def lookAt(self, target):
+        """Control the look of the visualizer camera
+
+        Arguments:
+            target {tuple} -- target as (x,y,z) tuple
+        """
+        if self.gui:
+            params = p.getDebugVisualizerCamera()
+            p.resetDebugVisualizerCamera(
+                params[10], params[8], params[9], target)
+
+    def getRobotPose(self):
+        """Gets the robot (origin) position
+
+        Returns:
+            (tuple(3), tuple(3)) -- (x,y,z), (roll, pitch, yaw)
+        """
+        pose = p.getBasePositionAndOrientation(self.robot)
+        return (pose[0], p.getEulerFromQuaternion(pose[1]))
+
+    def frameToWorldMatrix(self, frame):
+        """Gets the given frame to world matrix transformation. can be a frame name
+        from URDF/SDF or "origin" for the part origin
+
+        Arguments:
+            frame {str} -- frame name
+
+        Returns:
+            np.matrix -- a 4x4 matrix
+        """
+
+        if frame == 'origin':
+            frameToWorldPose = p.getBasePositionAndOrientation(self.robot)
+        else:
+            frameToWorldPose = p.getLinkState(self.robot, self.frames[frame])
+
+        return self.poseToMatrix(frameToWorldPose)
+
+    def transformation(self, frameA, frameB):
+        """Transformation matrix AtoB
+
+        Arguments:
+            frameA {str} -- frame A name
+            frameB {str} -- frame B name
+
+        Returns:
+            np.matrix -- A 4x4 matrix
+        """
+        AtoWorld = self.frameToWorldMatrix(frameA)
+        BtoWorld = self.frameToWorldMatrix(frameB)
+
+        return np.linalg.inv(BtoWorld) * AtoWorld
+
+    def poseToMatrix(self, pose):
+        """Converts a pyBullet pose to a transformation matrix"""
+        translation = pose[0]
+        quaternion = pose[1]
+
+        # NOTE: PyBullet quaternions are x, y, z, w
+        rotation = quat2mat([quaternion[3], quaternion[0],
+                             quaternion[1], quaternion[2]])
+
+        m = np.identity(4)
+        m[0:3, 0:3] = rotation
+        m.T[3, 0:3] = translation
+
+        return np.matrix(m)
+
+    def matrixToPose(self, matrix):
+        """Converts a transformation matrix to a pyBullet pose"""
+        arr = np.array(matrix)
+        translation = list(arr.T[3, 0:3])
+        quaternion = mat2quat(arr[0:3, 0:3])
+
+        # NOTE: PyBullet quaternions are x, y, z, w
+        quaternion = [quaternion[1], quaternion[2],
+                      quaternion[3], quaternion[0]]
+
+        return translation, quaternion
+
+    def setRobotPose(self, pos, orn):
+        """Sets the robot (origin) pose
+
+        Arguments:
+            pos {tuple} -- (x,y,z) position
+            orn {tuple} -- (x,y,z,w) quaternions
+        """
+        p.resetBasePositionAndOrientation(self.robot, pos, orn)
+
+    def reset(self, height=0.5, orientation='straight'):
+        """Resets the robot for experiment (joints, robot position, simulator time)
+
+        Keyword Arguments:
+            height {float} -- height of the reset (m) (default: {0.55})
+            orientation {str} -- orientation (straight, front or back) of the robot (default: {'straight'})
+        """
+        self.lines = []
+        self.t = 0
+        self.start = time.time()
+
+        # Resets the robot position
+        orn = [0, 0, 0]
+        if orientation == 'front':
+            orn = [0, math.pi/2, 0]
+        elif orientation == 'back':
+            orn = [0, -math.pi/2, 0]
+        self.resetPose([0, 0, height], p.getQuaternionFromEuler(orn))
+
+        # Reset the joints to 0
+        for entry in self.joints.values():
+            p.resetJointState(self.robot, entry, 0)
+
+    def resetPose(self, pos, orn):
+        """Called by reset() with the robot pose
+
+        Arguments:
+            pos {tuple} -- (x,y,z) position
+            orn {tuple} -- (x,y,z,w) quaternions
+        """
+        self.setRobotPose(pos, orn)
+
+    def getFrame(self, frame):
+        """Gets the given frame
+
+        Arguments:
+            frame {str} -- frame name
+
+        Returns:
+            tuple -- (pos, orn), where pos is (x, y, z) and orn is quaternions (x, y, z, w)
+        """
+        jointState = p.getLinkState(self.robot, self.frames[frame])
+        return (jointState[0], jointState[1])
+
+    def getFrames(self):
+        """Gets the available frames in the current robot model
+
+        Returns:
+            dict -- dict of str -> (pos, orientation)
+        """
+        frames = {}
+
+        for name in self.frames.keys():
+            jointState = p.getLinkState(self.robot, self.frames[name])
+            pos = jointState[0]
+            orientation = p.getEulerFromQuaternion(jointState[1])
+            frames[name] = [pos, orientation]
+
+        return frames
+    
+    def getVelocity(self, frame):
+        """Gets the velocity of the given frame
+
+        Arguments:
+            frame {str} -- frame name
+
+        Returns:
+            tuple -- (linear, angular)
+        """
+        jointState = p.getLinkState(self.robot, self.frames[frame], computeLinkVelocity=True)
+        return (jointState[6], jointState[7])
+
+    def resetJoints(self, joints):
+        """Reset all the joints to a given position
+
+        Arguments:
+            joints {dict} -- dict of joint name -> angle (float, radian)
+        """
+        for name in joints:
+            p.resetJointState(self.robot, self.joints[name], joints[name])
+
+    def setJoints(self, joints):
+        """Set joint targets for motor control in simulation
+
+        Arguments:
+            joints {dict} -- dict of joint name -> angle (float, radian)
+
+        Raises:
+            Exception: if a joint is not found, exception is raised
+
+        Returns:
+            applied {dict} -- dict of joint states (position, velocity, reaction forces, applied torque)
+        """
+        applied = {}
+
+        for name in self.passive_joints:
+            p.setJointMotorControl2(self.robot, self.passive_joints[name], controlMode=p.VELOCITY_CONTROL, force=0)
+
+        for name in joints.keys():
+            if name in self.joints:
+                if name.endswith('_speed'):
+                    p.setJointMotorControl2(
+                        self.robot, self.joints[name], p.VELOCITY_CONTROL, targetVelocity=joints[name])
+                else:
+                    if name in self.maxTorques:
+                        maxTorque = self.maxTorques[name]
+                        p.setJointMotorControl2(
+                            self.robot, self.joints[name], p.POSITION_CONTROL, joints[name], force=maxTorque)
+                    else:
+                        p.setJointMotorControl2(
+                            self.robot, self.joints[name], p.POSITION_CONTROL, joints[name])
+
+                applied[name] = p.getJointState(self.robot, self.joints[name])
+            else:
+                raise Exception("Can't find joint %s" % name)
+
+        return applied
+
+    def getJoints(self):
+        """Get all the joints names
+
+        Returns:
+            list -- list of str, with joint names
+        """
+        return self.joints.keys()
+
+    def getJointsInfos(self, name):
+        """Get informations about a joint
+
+        Return:
+            list -- a list with key type, lowerLimit & upperLimit (if defined)
+        """
+
+        return self.jointsInfos[name]
+
+    def getRobotMass(self):
+        """Returns the robot mass
+
+        Returns:
+            float -- the robot mass (kg)
+        """
+        if self.mass is None:
+            k = -1
+            self.mass = 0
+            while True:
+                if k == -1 or p.getLinkState(self.robot, k) is not None:
+                    d = p.getDynamicsInfo(self.robot, k)
+                    self.mass += d[0]
+                else:
+                    break
+                k += 1
+
+        return self.mass
+
+    def getCenterOfMassPosition(self):
+        """Returns center of mass of the robot
+
+        Returns:
+            pos -- (x, y, z) robot center of mass
+        """
+
+        k = -1
+        mass = 0
+        com = np.array([0., 0., 0.])
+        while True:
+            if k == -1:
+                pos, _ = p.getBasePositionAndOrientation(self.robot)
+            else:
+                res = p.getLinkState(self.robot, k)
+                if res is None:
+                    break
+                pos = res[0]
+
+            d = p.getDynamicsInfo(self.robot, k)
+            m = d[0]
+            com += np.array(pos) * m
+            mass += m
+
+            k += 1
+
+        return com / mass
+
+    def addDebugPosition(self, position, color=None, duration=30):
+        """Adds a debug position to be drawn as a line
+
+        Arguments:
+            position {tuple} -- (x,y,z) (m)
+
+        Keyword Arguments:
+            color {tuple} -- (r,g,b) (0->1) (default: {None})
+            duration {float} -- line duration on screen before disapearing (default: {30})
+        """
+        if color is None:
+            color = self.lineColors[self.currentLine % len(self.lineColors)]
+
+        if self.currentLine >= len(self.lines):
+            self.lines.append({})
+
+        self.lines[self.currentLine]['update'] = True
+        self.lines[self.currentLine]['to'] = position
+        self.lines[self.currentLine]['color'] = color
+        self.lines[self.currentLine]['duration'] = duration
+
+        self.currentLine += 1
+
+    def drawDebugLines(self):
+        """Updates the drawing of debug lines"""
+        self.currentLine = 0
+        if time.time() - self.lastLinesDraw > 0.05:
+            for line in self.lines:
+                if 'from' in line:
+                    if line['update'] == True:
+                        p.addUserDebugLine(
+                            line['from'], line['to'], line['color'], 2, line['duration'])
+                        line['update'] = False
+                    else:
+                        del line['from']
+                line['from'] = line['to']
+
+            self.lastLinesDraw = time.time()
+
+    def contactPoints(self):
+        """Gets all contact points and forces
+
+        Returns:
+            list -- list of entries (link_name, position in m, normal force vector, force in N)
+        """
+        result = []
+        contacts = p.getContactPoints(bodyA=self.floor, bodyB=self.robot)
+        for contact in contacts:
+            link_index = contact[4]
+            if link_index >= 0:
+                link_name = (p.getJointInfo(
+                    self.robot, link_index)[12]).decode()
+            else:
+                link_name = 'base'
+            result.append((link_name, contact[6], contact[7], contact[9]))
+
+        return result
+
+    def autoCollisions(self):
+        """Returns the total amount of N in autocollisions (not with ground)
+
+        Returns:
+            float -- Newtons of collisions not with ground
+        """
+        total = 0
+        for k in range(1, p.getNumJoints(self.robot)):
+            contacts = p.getContactPoints(bodyA=k)
+            for contact in contacts:
+                if contact[2] != self.floor:
+                    total += contact[9]
+        return total
+
+    def addConstraint(self, frameA, frameB, constraint = p.JOINT_POINT2POINT):
+        """Adds a constraint between two given frames
+
+        Args:
+            frameA (str): frame A name
+            frameB (str): frame A name
+            constraint (int, optional): pyBullet joint type. Defaults to p.JOINT_POINT2POINT.
+
+        Returns:
+            int: returns from pybullet createConstraint
+        """        
+        infosA = p.getJointInfo(self.robot, self.frames[frameA])
+        infosB = p.getJointInfo(self.robot, self.frames[frameB])
+
+        st = p.getLinkState(self.robot, infosA[16])
+        T_world_parentA = self.poseToMatrix(st[:2])
+        T_world_childA = self.poseToMatrix(self.getFrame(frameA))
+        T_parentA_childA = np.linalg.inv(T_world_parentA) * T_world_childA
+        childApose = self.matrixToPose(T_parentA_childA)
+
+        st = p.getLinkState(self.robot, infosB[16])
+        T_world_parentB = self.poseToMatrix(st[:2])
+        T_world_childB = self.poseToMatrix(self.getFrame(frameB))
+        T_parentB_childB = np.linalg.inv(T_world_parentB) * T_world_childB
+        childBpose = self.matrixToPose(T_parentB_childB)
+        
+        c =  p.createConstraint(
+            self.robot,
+            infosA[16],
+            self.robot,
+            infosB[16],
+            constraint,
+            [0.0, 0.0, 0.0],
+            childApose[0],
+            childBpose[0],
+            childApose[1],
+            childBpose[1],
+        )
+
+        p.changeConstraint(c, maxForce=1e3)
+
+        return c
+
+    def execute(self):
+        """Executes the simulaiton infinitely (blocks)"""
+        while True:
+            self.tick()
+
+    def tick(self):
+        """Ticks one step of simulation. If realTime is True, sleeps to compensate real time"""
+        self.t += self.dt
+        self.drawDebugLines()
+
+        p.stepSimulation()
+        delay = self.t - (time.time() - self.start)
+        if delay > 0 and self.realTime:
+            time.sleep(delay)