From 3f5cc6bbc6e517218497bb2c91c92a5de8ecb33d Mon Sep 17 00:00:00 2001 From: Jietao Chen Date: Thu, 26 Mar 2026 14:13:17 +0800 Subject: [PATCH 1/3] modify urdf assembly --- .../source/features/toolkits/urdf_assembly.md | 89 ++++++++ embodichain/lab/sim/cfg.py | 81 ++++++++ embodichain/lab/sim/utility/cfg_utils.py | 12 ++ .../toolkits/urdf_assembly/component.py | 113 ++++++++-- .../toolkits/urdf_assembly/connection.py | 109 ++++++++-- .../toolkits/urdf_assembly/signature.py | 20 +- .../urdf_assembly/urdf_assembly_manager.py | 195 +++++++++++++++++- examples/sim/robot/dexforce_w1.py | 2 +- 8 files changed, 569 insertions(+), 52 deletions(-) diff --git a/docs/source/features/toolkits/urdf_assembly.md b/docs/source/features/toolkits/urdf_assembly.md index 76f48ddb..dcc4d9d9 100644 --- a/docs/source/features/toolkits/urdf_assembly.md +++ b/docs/source/features/toolkits/urdf_assembly.md @@ -201,6 +201,74 @@ Get all attached sensors. manager.get_attached_sensors() -> dict ``` +##### Component name prefixes (`component_prefix`) + +`URDFAssemblyManager` uses `component_prefix` to configure name prefixes for +each supported component type. This attribute is a list of 2-tuples: + +- Form: `[(component_name, prefix), ...]` +- The default value is: + + ```python + [ + ("chassis", None), + ("legs", None), + ("torso", None), + ("head", None), + ("left_arm", "left_"), + ("right_arm", "right_"), + ("left_hand", "left_"), + ("right_hand", "right_"), + ("arm", None), + ("hand", None), + ] + ``` + +You can configure it in a *patch-style* manner via the property: + +```python +# Only override prefixes for existing components; do not introduce +# new component names. +manager.component_prefix = [ + ("left_arm", "L_"), + ("right_arm", "R_"), + ("left_hand", "L_"), + ("right_hand", "R_"), +] +``` + +Semantics: + +- Only components that already exist in the default configuration (e.g. `chassis/torso/left_arm/...`) may be overridden; new component names are not allowed. +- Components not listed in `new_prefixes` keep their original prefix. +- If `new_prefixes` contains an unknown component name, a `ValueError` is raised indicating that new component types cannot be introduced. + +##### Name casing policy (`name_case`) + +`URDFAssemblyManager` supports a global name casing policy that controls how +link and joint names are normalized during assembly. This is configured via +the optional `name_case` argument in the constructor: + +```python +manager = URDFAssemblyManager( + name_case={ + "joint": "upper", # or "lower" / "none" + "link": "lower", # or "upper" / "none" + } +) +``` + +Semantics: + +- Valid keys: `"joint"`, `"link"`. +- Valid values: `"upper"`, `"lower"`, `"none"`. +- Default behavior matches the legacy implementation: + - joints are normalized to **UPPERCASE**, + - links are normalized to **lowercase**. +- This policy is propagated to the internal component and connection managers, + and is also included in the assembly signature. Changing `name_case` will + therefore force a rebuild of the assembled URDF. + ## Using with URDFCfg for Robot Creation The URDF Assembly Tool can be used directly with `URDFCfg` to create robots with multiple components in the simulation. This is the recommended approach when building robots from assembled URDF files. @@ -232,6 +300,27 @@ cfg = RobotCfg( ) ``` +When using `URDFCfg` to build multi-component robots, you can pass custom +component prefixes to the internal `URDFAssemblyManager` via +`URDFCfg.component_prefix`. Its semantics are identical to +`URDFAssemblyManager.component_prefix`: + +- Each element is a `(component_name, prefix)` tuple. +- Only prefixes for components that exist in the default configuration may be overridden; no new component names can be added. +- Components not explicitly listed keep their original prefix. + +Example: + +```python +urdf_cfg = URDFCfg( + components=[...], +) +urdf_cfg.component_prefix = [ + ("left_arm", "L_"), + ("right_arm", "R_"), +] +``` + ### Complete Example Here's a complete example from `scripts/tutorials/sim/create_robot.py`: diff --git a/embodichain/lab/sim/cfg.py b/embodichain/lab/sim/cfg.py index 1f4d9ee1..3f9057f3 100644 --- a/embodichain/lab/sim/cfg.py +++ b/embodichain/lab/sim/cfg.py @@ -720,6 +720,27 @@ class URDFCfg: fpath_prefix: str = EMBODICHAIN_DEFAULT_DATA_ROOT + "/assembled" """Output directory prefix for the assembled URDF file.""" + component_prefix: List[tuple[str, Union[str, None]]] = field( + default_factory=lambda: [ + ("chassis", None), + ("legs", None), + ("torso", None), + ("head", None), + ("left_arm", "left_"), + ("right_arm", "right_"), + ("left_hand", "left_"), + ("right_hand", "right_"), + ("arm", None), + ("hand", None), + ] + ) + """Component name prefixes used during URDF assembly. + + Preferred form is a list of ``(component_name, prefix)`` tuples. For + convenience, a mapping ``{component_name: prefix}`` is also accepted when + constructing :class:`URDFCfg` and will be normalized internally. + """ + def __init__( self, components: list[dict[str, str | np.ndarray]] | None = None, @@ -729,6 +750,8 @@ def __init__( fpath_prefix: str = EMBODICHAIN_DEFAULT_DATA_ROOT + "/assembled", use_signature_check: bool = True, base_link_name: str = "base_link", + component_prefix: list[tuple[str, str | None]] | None = None, + name_case: dict[str, str] | None = None, ): """ Initialize URDFCfg with optional list of components and output path settings. @@ -745,6 +768,9 @@ def __init__( fpath_prefix (str): Output directory prefix for the assembled URDF file. use_signature_check (bool): Whether to use signature check when merging URDFs. base_link_name (str): Name of the base link in the assembled robot. + component_prefix (list[tuple[str, str | None]] | None): Optional + list of (component_type, prefix) pairs to override default + component name prefixes. """ self.components = {} self.sensors = sensors or {} @@ -754,6 +780,36 @@ def __init__( self.fname = fname self.fpath_prefix = fpath_prefix + # Initialize component prefixes (patch-style mapping per component type) + if component_prefix is None: + # Use the same default as the dataclass field + self.component_prefix = [ + ("chassis", None), + ("legs", None), + ("torso", None), + ("head", None), + ("left_arm", "left_"), + ("right_arm", "right_"), + ("left_hand", "left_"), + ("right_hand", "right_"), + ("arm", None), + ("hand", None), + ] + elif isinstance(component_prefix, dict): + # Allow dict-style config: {"left_hand": "l_", ...} + self.component_prefix = list(component_prefix.items()) + else: + # Assume caller provided a list of (component_name, prefix) tuples + self.component_prefix = component_prefix + + if name_case is None: + self.name_case = { + "joint": "upper", + "link": "lower", + } + else: + self.name_case = name_case + # Auto-add components if provided if components: for comp_config in components: @@ -915,6 +971,27 @@ def assemble_urdf(self) -> str: # If there are multiple components, merge them into a single URDF file. manager = URDFAssemblyManager() manager.base_link_name = self.base_link_name + + if self.component_prefix is None: + self.component_prefix = [ + ("left_arm", "left_"), + ("right_arm", "right_"), + ("left_hand", "left_"), + ("right_hand", "right_"), + ] + if isinstance(self.component_prefix, dict): + self.component_prefix = list(self.component_prefix.items()) + # Forward configured component prefixes to the assembly manager + manager.component_prefix = self.component_prefix + + if self.name_case is not None: + manager.name_case = self.name_case + else: + manager.name_case = { + "joint": "upper", + "link": "lower", + } + for comp_type, comp_config in components: params = comp_config.get("params", {}) success = manager.add_component( @@ -968,12 +1045,16 @@ def from_dict(cls, init_dict: Dict) -> "URDFCfg": fpath = init_dict.get("fpath", None) use_signature_check = init_dict.get("use_signature_check", True) base_link_name = init_dict.get("base_link_name", "base_link") + component_prefix = init_dict.get("component_prefix", None) + name_case = init_dict.get("name_case", None) return cls( components=components, sensors=sensors, fpath=fpath, use_signature_check=use_signature_check, base_link_name=base_link_name, + component_prefix=component_prefix, + name_case=name_case, ) diff --git a/embodichain/lab/sim/utility/cfg_utils.py b/embodichain/lab/sim/utility/cfg_utils.py index 94fdbf8d..841e7359 100644 --- a/embodichain/lab/sim/utility/cfg_utils.py +++ b/embodichain/lab/sim/utility/cfg_utils.py @@ -160,6 +160,18 @@ def merge_robot_cfg(base_cfg: RobotCfg, override_cfg_dict: dict[str, any]) -> Ro urdf_path=component.get("urdf_path"), transform=component.get("transform"), ) + for sensor in user_urdf_cfg.get("sensors", []): + base_cfg.urdf_cfg.add_sensor( + sensor_type=sensor.get("sensor_type"), + urdf_path=sensor.get("urdf_path"), + transform=sensor.get("transform"), + ) + component_prefix = user_urdf_cfg.get("component_prefix", None) + if component_prefix: + base_cfg.urdf_cfg.component_prefix = component_prefix + name_case = user_urdf_cfg.get("name_case", None) + if name_case: + base_cfg.urdf_cfg.name_case = name_case else: logger.log_warning( "urdf_cfg should be a dictionary. Skipping urdf_cfg merge." diff --git a/embodichain/toolkits/urdf_assembly/component.py b/embodichain/toolkits/urdf_assembly/component.py index 211ecf18..65c88a96 100644 --- a/embodichain/toolkits/urdf_assembly/component.py +++ b/embodichain/toolkits/urdf_assembly/component.py @@ -83,12 +83,75 @@ def __post_init__(self): class URDFComponentManager: - """Responsible for loading, renaming, and processing meshes for a single component.""" + """Responsible for loading, renaming, and processing meshes for a single component. + + This manager normalizes link and joint names according to a configurable + case policy so that the overall assembly naming scheme can be controlled + centrally (e.g. all links lowercase, all joints uppercase). + """ + + def __init__( + self, + mesh_manager: URDFMeshManager, + name_case: dict[str, str] | None = None, + ): + """Create a component manager. + + Args: + mesh_manager (URDFMeshManager): Mesh manager used for copying and + rewriting mesh references. + name_case (dict[str, str] | None): Optional mapping controlling + how joint and link names are normalized. Supported keys are + ``"joint"`` and ``"link"`` with values ``"upper``, + ``"lower"`` or ``"none"``. When omitted, joints are + uppercased and links are lowercased (the previous default + behavior). + """ - def __init__(self, mesh_manager: URDFMeshManager): self.mesh_manager = mesh_manager self.logger = URDFAssemblyLogger.get_logger("component_manager") + # By default, mimic the legacy behavior: links are lowercase, joints + # are uppercase. This should stay in sync conceptually with + # URDFConnectionManager. + self._name_case: dict[str, str] = { + "joint": "upper", + "link": "lower", + } + if name_case is not None: + for key, mode in name_case.items(): + if key in self._name_case and mode in {"upper", "lower", "none"}: + self._name_case[key] = mode + else: + self.logger.warning( + "Ignoring invalid name_case entry %r=%r (allowed keys: 'joint', 'link'; " + "allowed modes: 'upper', 'lower', 'none')", + key, + mode, + ) + + def _apply_case(self, kind: str, name: str | None) -> str | None: + """Normalize a name according to the configured case policy. + + Args: + kind (str): One of ``"joint"`` or ``"link"``. + name (str | None): The original name. + + Returns: + str | None: The normalized name, or the original value if + ``kind`` is unknown or mode is ``"none"``. + """ + + if name is None: + return None + + mode = self._name_case.get(kind, "none") + if mode == "lower": + return name.lower() + if mode == "upper": + return name.upper() + return name + def process_component( self, comp: str, @@ -119,12 +182,12 @@ def process_component( # Safe way to get link and joint names, handling None values global_link_names = { - link.get("name").lower() + self._apply_case("link", link.get("name")) for link in links if link.get("name") is not None } global_joint_names = { - joint.get("name").upper() + self._apply_case("joint", joint.get("name")) for joint in joints if joint.get("name") is not None } @@ -145,13 +208,14 @@ def process_component( if prefix: new_name = self._generate_unique_name( orig_name, prefix, global_link_names - ).lower() + ) else: # For components without prefix, ensure names are unique - if orig_name.lower() in global_link_names: - new_name = f"{comp}_{orig_name}".lower() + normalized_orig = self._apply_case("link", orig_name) + if normalized_orig in global_link_names: + new_name = self._apply_case("link", f"{comp}_{orig_name}") else: - new_name = orig_name.lower() + new_name = normalized_orig global_link_names.add(new_name) @@ -160,7 +224,7 @@ def process_component( base_points[comp] = new_name first_link_flag = False - # Update link name mapping and set link name to lowercase + # Update link name mapping and set link name according to policy name_mapping[(comp, orig_name)] = new_name link.set("name", new_name) links.append(link) @@ -176,9 +240,12 @@ def process_component( if orig_joint_name is None: continue - new_joint_name = self._generate_unique_name( - orig_joint_name, prefix, global_joint_names - ).upper() + new_joint_name = self._apply_case( + "joint", + self._generate_unique_name( + orig_joint_name, prefix, global_joint_names + ), + ) global_joint_names.add(new_joint_name) # Build the complete mapping table @@ -192,16 +259,16 @@ def process_component( # Set the new joint name joint.set("name", new_joint_name) - # Update parent and child links to lowercase - with None checks + # Update parent and child links with case normalization - with None checks parent_elem = joint.find("parent") child_elem = joint.find("child") if parent_elem is not None: parent = parent_elem.get("link") if parent is not None: - new_parent_name = name_mapping.get( - (comp, parent), parent - ).lower() + new_parent_name = self._apply_case( + "link", name_mapping.get((comp, parent), parent) + ) parent_elem.set("link", new_parent_name) else: self.logger.warning( @@ -211,7 +278,9 @@ def process_component( if child_elem is not None: child = child_elem.get("link") if child is not None: - new_child_name = name_mapping.get((comp, child), child).lower() + new_child_name = self._apply_case( + "link", name_mapping.get((comp, child), child) + ) child_elem.set("link", new_child_name) else: self.logger.warning( @@ -270,10 +339,14 @@ def _generate_unique_name( if orig_name is None: orig_name = "unnamed" + # For uniqueness checks we always operate on a normalized form that is + # consistent with the link case policy. This keeps collisions and + # generated names aligned with how names are written back to the URDF. + base_name = orig_name if prefix and not orig_name.lower().startswith(prefix.lower()): - new_name = f"{prefix}{orig_name}".lower() - else: - new_name = orig_name.lower() + base_name = f"{prefix}{orig_name}" + + new_name = base_name # Ensure the new name is unique if new_name in existing_names: diff --git a/embodichain/toolkits/urdf_assembly/connection.py b/embodichain/toolkits/urdf_assembly/connection.py index 4dad94a1..a96ed601 100644 --- a/embodichain/toolkits/urdf_assembly/connection.py +++ b/embodichain/toolkits/urdf_assembly/connection.py @@ -30,15 +30,63 @@ class URDFConnectionManager: Responsible for managing connection rules between components and sensor attachments. """ - def __init__(self, base_link_name: str): - r"""Initialize the URDFConnectionManager. + def __init__(self, base_link_name: str, name_case: dict[str, str] | None = None): + """Initialize the URDFConnectionManager. Args: - base_link_name (str): The name of the base link to which the chassis or other components may be attached. + base_link_name (str): The name of the base link to which the + chassis or other components may be attached. + name_case (dict[str, str] | None): Optional mapping controlling + how joint and link names are normalized. Supported keys are + ``"joint"`` and ``"link"`` with values ``"upper"``, + ``"lower"`` or ``"none"``. When omitted, joints are + uppercased and links are lowercased (the previous default + behavior). """ self.base_link_name = base_link_name self.logger = URDFAssemblyLogger.get_logger("connection_manager") + # Configure name normalization strategy for different entity types. + # By default, this preserves the legacy behavior of using uppercase + # for joint names and lowercase for link names. + self._name_case: dict[str, str] = { + "joint": "upper", + "link": "lower", + } + if name_case is not None: + for key, mode in name_case.items(): + if key in self._name_case and mode in {"upper", "lower", "none"}: + self._name_case[key] = mode + else: + self.logger.warning( + "Ignoring invalid name_case entry %r=%r (allowed keys: 'joint', 'link'; " + "allowed modes: 'upper', 'lower', 'none')", + key, + mode, + ) + + def _apply_case(self, kind: str, name: str | None) -> str | None: + """Normalize a name according to the configured case policy. + + Args: + kind (str): One of ``"joint"`` or ``"link"``. + name (str | None): The original name. + + Returns: + str | None: The normalized name, or the original value if + ``kind`` is unknown or mode is ``"none"``. + """ + + if name is None: + return None + + mode = self._name_case.get(kind, "none") + if mode == "lower": + return name.lower() + if mode == "upper": + return name.upper() + return name + def add_connections( self, joints: list, @@ -66,7 +114,9 @@ def add_connections( # chassis is always attached to base_link (no transform applied to this connection) if chassis_component in base_points: chassis_first_link = base_points[chassis_component] - joint_name = f"BASE_LINK_TO_{chassis_component.upper()}_CONNECTOR" + joint_name = self._apply_case( + "joint", f"BASE_LINK_TO_{chassis_component}_CONNECTOR" + ) if joint_name not in existing_joint_names: joint = ET.Element("joint", name=joint_name, type="fixed") ET.SubElement(joint, "origin", xyz="0 0 0", rpy="0 0 0") @@ -93,7 +143,7 @@ def add_connections( for comp in orphan_components: comp_first_link = base_points[comp] - joint_name = f"BASE_TO_{comp.upper()}_CONNECTOR" + joint_name = self._apply_case("joint", f"BASE_TO_{comp}_CONNECTOR") if joint_name not in existing_joint_names: joint = ET.Element("joint", name=joint_name, type="fixed") @@ -118,7 +168,11 @@ def add_connections( ET.SubElement(joint, "origin", xyz="0 0 0", rpy="0 0 0") ET.SubElement(joint, "parent", link=self.base_link_name) - ET.SubElement(joint, "child", link=comp_first_link) + ET.SubElement( + joint, + "child", + link=self._apply_case("link", comp_first_link), + ) joints.append(joint) existing_joint_names.add(joint_name) @@ -129,15 +183,19 @@ def add_connections( # Process other connection relationships for parent, child in connection_rules: if parent in parent_attach_points and child in base_points: - parent_connect_link = parent_attach_points[parent].lower() - child_connect_link = base_points[child].lower() + parent_connect_link = self._apply_case( + "link", parent_attach_points[parent] + ) + child_connect_link = self._apply_case("link", base_points[child]) self.logger.info( f"Connecting [{parent}]-({parent_connect_link}) to [{child}]-({child_connect_link})" ) # Create a unique joint name - base_joint_name = f"{parent.upper()}_TO_{child.upper()}_CONNECTOR" + base_joint_name = self._apply_case( + "joint", f"{parent}_TO_{child}_CONNECTOR" + ) if base_joint_name not in existing_joint_names: joint = ET.Element("joint", name=base_joint_name, type="fixed") @@ -181,32 +239,43 @@ def add_sensor_attachments( # Add sensor links and joints to the main lists for link in sensor_urdf.findall("link"): # Ensure sensor link names are lowercase - link.set("name", link.get("name").lower()) + link.set("name", self._apply_case("link", link.get("name"))) joints.append(link) # This should be added to links list instead for joint in sensor_urdf.findall("joint"): # Ensure sensor joint names are uppercase and link references are lowercase - joint.set("name", joint.get("name").upper()) + joint.set("name", self._apply_case("joint", joint.get("name"))) parent_elem = joint.find("parent") child_elem = joint.find("child") if parent_elem is not None: - parent_elem.set("link", parent_elem.get("link").lower()) + parent_elem.set( + "link", + self._apply_case("link", parent_elem.get("link")), + ) if child_elem is not None: - child_elem.set("link", child_elem.get("link").lower()) + child_elem.set( + "link", + self._apply_case("link", child_elem.get("link")), + ) joints.append(joint) - parent_link = base_points.get( - attach.parent_component, attach.parent_component - ).lower() # Ensure lowercase + parent_link = self._apply_case( + "link", + base_points.get(attach.parent_component, attach.parent_component), + ) # Create connection joint with uppercase name - joint_name = ( - f"{attach.parent_component.upper()}_TO_{sensor_name.upper()}_CONNECTOR" + joint_name = f"{attach.parent_component}_TO_{sensor_name}_CONNECTOR" + joint = ET.Element( + "joint", + name=self._apply_case("joint", joint_name), + type="fixed", ) - joint = ET.Element("joint", name=joint_name, type="fixed") ET.SubElement(joint, "origin", xyz="0 0 0", rpy="0 0 0") ET.SubElement(joint, "parent", link=parent_link) ET.SubElement( - joint, "child", link=sensor_urdf.find("link").get("name").lower() + joint, + "child", + link=self._apply_case("link", sensor_urdf.find("link").get("name")), ) joints.append(joint) diff --git a/embodichain/toolkits/urdf_assembly/signature.py b/embodichain/toolkits/urdf_assembly/signature.py index 3ebbd73a..27a56521 100644 --- a/embodichain/toolkits/urdf_assembly/signature.py +++ b/embodichain/toolkits/urdf_assembly/signature.py @@ -62,6 +62,12 @@ def calculate_assembly_signature(self, urdf_dict: dict, output_path: str) -> str signature_data = { "output_filename": os.path.basename(output_path), "components": {}, + # Optional metadata that can affect the assembly even if the + # component URDF files themselves do not change. For example, + # the processing order and name prefixes for each component, + # and the global casing policy for links/joints. + "component_order_and_prefix": [], + "name_case": {}, } def to_serializable(obj): @@ -85,8 +91,20 @@ def to_serializable(obj): else: return obj - # Process each component + # Process each entry passed in from the assembly manager. Most entries + # are components (with URDF files), but some may be metadata such as + # the component_order_and_prefix or name_case used during assembly. for comp_type, comp_obj in urdf_dict.items(): + # Special key reserved for component order/prefix metadata + if comp_type == "__component_order_and_prefix__": + signature_data["component_order_and_prefix"] = to_serializable(comp_obj) + continue + + # Special key reserved for global name_case policy (link/joint casing) + if comp_type == "__name_case__": + signature_data["name_case"] = to_serializable(comp_obj) + continue + if comp_obj is None: continue diff --git a/embodichain/toolkits/urdf_assembly/urdf_assembly_manager.py b/embodichain/toolkits/urdf_assembly/urdf_assembly_manager.py index 9739faa9..976fec11 100644 --- a/embodichain/toolkits/urdf_assembly/urdf_assembly_manager.py +++ b/embodichain/toolkits/urdf_assembly/urdf_assembly_manager.py @@ -128,6 +128,15 @@ def __init__( ): self.logger = setup_urdf_logging() + # Global name normalization strategy for this assembly. By default, + # this preserves the legacy behavior: link names are lowercase and + # joint names are uppercase. The same mapping is passed down to + # managers that deal with naming so that the policy stays consistent. + self._name_case: dict[str, str] = { + "joint": "upper", + "link": "lower", + } + # Use registries for components and sensors self.component_registry = component_registry or ComponentRegistry() self.sensor_registry = sensor_registry or SensorRegistry() @@ -137,13 +146,13 @@ def __init__( # Initialize managers for components and sensors self.component_manager = component_manager or URDFComponentManager( - self.mesh_manager + self.mesh_manager, name_case=self._name_case ) self.sensor_manager = sensor_manager or URDFSensorManager(self.mesh_manager) # Processing order for components with their name prefixes # Tuple format: (component_name, prefix) - self.component_order = [ + self._component_order_and_prefix = [ ("chassis", None), ("legs", None), ("torso", None), @@ -205,6 +214,147 @@ def __init__( # Initialize signature manager instead of cache manager self.signature_manager = URDFAssemblySignatureManager() + @property + def name_case(self): + """Get the current name case policy for joints and links. + + Returns: + dict[str, str]: A dictionary mapping 'joint' and 'link' to their respective case modes. + """ + return self._name_case + + @name_case.setter + def name_case(self, new_name_case: dict[str, str]): + """Set a new name case policy for joints and links. + + This method updates the name case policy and propagates it to the component and sensor managers. + + Args: + new_name_case (dict[str, str]): A dictionary mapping 'joint' and 'link' to their desired case modes (e.g., 'upper', 'lower', 'none'). + """ + if not isinstance(new_name_case, dict): + raise ValueError( + "name_case must be a dictionary mapping 'joint' and 'link' to case modes." + ) + if "joint" not in new_name_case or "link" not in new_name_case: + raise ValueError("name_case must contain keys 'joint' and 'link'.") + + self._name_case = new_name_case + self.component_manager.name_case = new_name_case + self.sensor_manager.name_case = new_name_case + + def _apply_case(self, kind: str, name: str | None) -> str | None: + """Normalize a name according to the assembly-wide case policy. + + This helper mirrors the behavior of the managers' own case helpers so + that any name sets computed here (e.g. for sensors) stay consistent + with how names are written into the URDF. + + Args: + kind (str): One of ``"joint"`` or ``"link"``. + name (str | None): The original name. + + Returns: + str | None: The normalized name, or the original value if the + kind is unknown or its mode is ``"none"``. + """ + + if name is None: + return None + + mode = self._name_case.get(kind, "none") + if mode == "lower": + return name.lower() + if mode == "upper": + return name.upper() + return name + + @property + def component_order_and_prefix(self): + """Get the internal component order with their name prefixes. + + Note: + This exposes the internal list of ``(component_name, prefix)`` pairs + used when assembling URDFs. In most user code it is recommended to + use :attr:`component_prefix` instead, which focuses on configuring + prefixes rather than ordering. + + Returns: + list[tuple[str, str | None]]: A list of tuples specifying component + names and their prefixes. + """ + return self._component_order_and_prefix + + @component_order_and_prefix.setter + def component_order_and_prefix(self, new_order): + """Patch the internal component prefix configuration. + + Args: + new_order (list[tuple[str, str | None]]): List of + ``(component_name, prefix)`` tuples. + + Raises: + ValueError: If the new order is not a list of tuples or if it + contains unknown component names. + """ + self._component_order_and_prefix = new_order + + @property + def component_prefix(self): + """Configure name prefixes per component type. + + This is a user-facing alias over :attr:`component_order_and_prefix`. + It accepts a list of ``(component_name, prefix)`` tuples and updates + the internal configuration in a patch-style manner, without requiring + users to reason about the full processing order. + """ + + return self.component_order_and_prefix + + @component_prefix.setter + def component_prefix(self, new_prefixes): + if not isinstance(new_prefixes, list) or not all( + isinstance(item, tuple) and len(item) == 2 for item in new_prefixes + ): + raise ValueError( + "component_order_and_prefix must be a list of (component_name, prefix) tuples." + ) + + # Treat new_order as a patch on top of the existing/default order: + # - For components already present in self._component_order_and_prefix, update their prefix. + # - Preserve components that are not mentioned in new_order, keeping their relative order. + # - Append any brand‑new components from new_order at the end. + + # Allowed components are exactly those already present in the default order. + existing_components = {comp for comp, _ in self._component_order_and_prefix} + + # Build override map from the incoming list, but only for existing components. + override_map = {} + for comp, prefix in new_prefixes: + if not isinstance(comp, str): + raise ValueError( + "component name in component_order_and_prefix must be a string." + ) + if comp not in existing_components: + raise ValueError( + f"component_order_and_prefix cannot introduce new component '{comp}'. " + f"Allowed components: {sorted(existing_components)}" + ) + override_map[comp] = prefix + + merged_order: list[tuple[str, str | None]] = [] + + # First, walk the existing order and apply overrides where available. + # The relative order of components is kept internal and usually does + # not need to be changed by users. + for comp, prefix in self._component_order_and_prefix: + if comp in override_map: + merged_order.append((comp, override_map.pop(comp))) + else: + merged_order.append((comp, prefix)) + + self._component_order_and_prefix = merged_order + def add_component( self, component_type: str, @@ -572,9 +722,21 @@ def merge_urdfs( self.logger.debug(f" Transform: applied") if use_signature_check: - # Calculate current assembly signature + # Calculate current assembly signature. In addition to the component + # registry contents, include the current component_order_and_prefix + # so that changes to name prefixes also invalidate the cache. + component_info = self.component_registry.all().copy() + component_info["__component_order_and_prefix__"] = list( + self.component_order_and_prefix + ) + # Also include the assembly-wide name_case policy so that + # renaming rules (e.g. link/joint casing) participate in the + # signature. This ensures that changing naming strategy forces + # a rebuild. + component_info["__name_case__"] = dict(self._name_case) + assembly_signature = self.signature_manager.calculate_assembly_signature( - self.component_registry.all(), output_path + component_info, output_path ) self.logger.info(f"Current assembly signature: [{assembly_signature}]") @@ -622,8 +784,12 @@ def merge_urdfs( ensure_directory_exists(output_dir, self.logger) mesh_manager = URDFMeshManager(output_dir) mesh_manager.ensure_dirs() - component_manager = URDFComponentManager(mesh_manager) - connection_manager = URDFConnectionManager(self.base_link_name) + component_manager = URDFComponentManager( + mesh_manager, name_case=self._name_case + ) + connection_manager = URDFConnectionManager( + self.base_link_name, name_case=self._name_case + ) # Initialize sensor manager with mesh_manager sensor_manager = URDFSensorManager(mesh_manager) @@ -647,7 +813,10 @@ def merge_urdfs( if comp_obj and comp_obj.transform is not None: component_transforms[comp] = comp_obj.transform - for comp, prefix in self.component_order: + print("\n📦 Processing components in order:") + print(self.component_order_and_prefix) + + for comp, prefix in self.component_order_and_prefix: comp_obj = self.component_registry.get(comp) if not comp_obj: continue @@ -747,12 +916,18 @@ def merge_urdfs( component_transforms, ) - # Track existing names for sensor processing + # Track existing names for sensor processing. Use the same case policy + # as the rest of the assembly so that collision checks are consistent + # with how names are written. existing_link_names = { - link.get("name").lower() for link in links if link.get("name") + self._apply_case("link", link.get("name")) + for link in links + if link.get("name") } existing_joint_names = { - joint.get("name").upper() for joint in joints if joint.get("name") + self._apply_case("joint", joint.get("name")) + for joint in joints + if joint.get("name") } # 5. Process sensor attachments using the new sensor manager diff --git a/examples/sim/robot/dexforce_w1.py b/examples/sim/robot/dexforce_w1.py index beee19d9..0efc72ce 100644 --- a/examples/sim/robot/dexforce_w1.py +++ b/examples/sim/robot/dexforce_w1.py @@ -51,7 +51,7 @@ def main(): "component_type": "right_hand", "urdf_path": "DH_PGC_140_50/DH_PGC_140_50.urdf", }, - ] + ], }, "drive_pros": { "max_effort": { From 9f58058a2a27c2dc5dd3d432cd68f7e5d7edb750 Mon Sep 17 00:00:00 2001 From: Copilot <198982749+Copilot@users.noreply.github.com> Date: Fri, 27 Mar 2026 14:52:07 +0800 Subject: [PATCH 2/3] Fix `add_sensor_attachments` mixing link elements into joints list (#204) Co-authored-by: copilot-swe-agent[bot] <198982749+Copilot@users.noreply.github.com> Co-authored-by: chase6305 <61959467+chase6305@users.noreply.github.com> --- embodichain/toolkits/urdf_assembly/connection.py | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/embodichain/toolkits/urdf_assembly/connection.py b/embodichain/toolkits/urdf_assembly/connection.py index a96ed601..fce16946 100644 --- a/embodichain/toolkits/urdf_assembly/connection.py +++ b/embodichain/toolkits/urdf_assembly/connection.py @@ -230,17 +230,25 @@ def add_connections( self.logger.error(f"Invalid connection rule: {parent} -> {child}") def add_sensor_attachments( - self, joints: list, attach_dict: dict, base_points: dict + self, links: list, joints: list, attach_dict: dict, base_points: dict ): - r"""Attach sensors to the robot by creating fixed joints.""" + r"""Attach sensors to the robot by creating fixed joints and links. + + Args: + links (list): A list to collect link elements from the sensor URDF. + joints (list): A list to collect joint elements from the sensor URDF + and the new attachment joint. + attach_dict (dict): A mapping from sensor names to their attachment configs. + base_points (dict): A mapping from component names to their base link names. + """ for sensor_name, attach in attach_dict.items(): sensor_urdf = ET.parse(attach.sensor_urdf).getroot() - # Add sensor links and joints to the main lists + # Add sensor links to the links list for link in sensor_urdf.findall("link"): # Ensure sensor link names are lowercase link.set("name", self._apply_case("link", link.get("name"))) - joints.append(link) # This should be added to links list instead + links.append(link) for joint in sensor_urdf.findall("joint"): # Ensure sensor joint names are uppercase and link references are lowercase From 8dfabffc961a35856e3a5c740305c2b52d15e1a8 Mon Sep 17 00:00:00 2001 From: Copilot <198982749+Copilot@users.noreply.github.com> Date: Fri, 27 Mar 2026 14:52:42 +0800 Subject: [PATCH 3/3] Add naming-semantics tests for URDF assembly; fix name_case propagation bug (#203) Co-authored-by: copilot-swe-agent[bot] <198982749+Copilot@users.noreply.github.com> Co-authored-by: chase6305 <61959467+chase6305@users.noreply.github.com> --- .../toolkits/urdf_assembly/component.py | 38 ++ tests/toolkits/test_urdf_assembly_naming.py | 370 ++++++++++++++++++ 2 files changed, 408 insertions(+) create mode 100644 tests/toolkits/test_urdf_assembly_naming.py diff --git a/embodichain/toolkits/urdf_assembly/component.py b/embodichain/toolkits/urdf_assembly/component.py index 65c88a96..213046b6 100644 --- a/embodichain/toolkits/urdf_assembly/component.py +++ b/embodichain/toolkits/urdf_assembly/component.py @@ -130,6 +130,44 @@ def __init__( mode, ) + @property + def name_case(self) -> dict[str, str]: + """Get the current name case policy. + + Returns: + dict[str, str]: A mapping of ``"joint"`` / ``"link"`` to their + case modes. + """ + return self._name_case + + @name_case.setter + def name_case(self, new_name_case: dict[str, str]) -> None: + """Replace the name case policy. + + Invalid entries are silently ignored (logged as warnings) to stay + consistent with the constructor behaviour. This intentionally differs + from :class:`URDFAssemblyManager`, which raises ``ValueError`` for + bad inputs, because this low-level manager is designed to be lenient + so it can be used in contexts where the caller cannot easily validate + inputs upfront. + + Args: + new_name_case (dict[str, str]): Mapping of ``"joint"`` / ``"link"`` + to ``"upper"``, ``"lower"``, or ``"none"``. + """ + updated: dict[str, str] = {} + for key, mode in new_name_case.items(): + if key in {"joint", "link"} and mode in {"upper", "lower", "none"}: + updated[key] = mode + else: + self.logger.warning( + "Ignoring invalid name_case entry %r=%r (allowed keys: 'joint', 'link'; " + "allowed modes: 'upper', 'lower', 'none')", + key, + mode, + ) + self._name_case.update(updated) + def _apply_case(self, kind: str, name: str | None) -> str | None: """Normalize a name according to the configured case policy. diff --git a/tests/toolkits/test_urdf_assembly_naming.py b/tests/toolkits/test_urdf_assembly_naming.py new file mode 100644 index 00000000..62d95209 --- /dev/null +++ b/tests/toolkits/test_urdf_assembly_naming.py @@ -0,0 +1,370 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import os +import textwrap +import xml.etree.ElementTree as ET +import pytest + +from embodichain.toolkits.urdf_assembly.urdf_assembly_manager import ( + URDFAssemblyManager, +) +from embodichain.toolkits.urdf_assembly.component import ( + URDFComponentManager, +) +from embodichain.toolkits.urdf_assembly.signature import ( + URDFAssemblySignatureManager, +) +from embodichain.toolkits.urdf_assembly.mesh import URDFMeshManager + + +# --------------------------------------------------------------------------- +# Minimal URDF fixture helpers +# --------------------------------------------------------------------------- + +_SIMPLE_URDF = textwrap.dedent( + """\ + + + + + + + + + + """ +) + + +def _write_urdf(tmp_path: str, filename: str = "component.urdf") -> str: + """Write a minimal URDF file and return its path.""" + path = os.path.join(tmp_path, filename) + with open(path, "w") as f: + f.write(_SIMPLE_URDF) + return path + + +# --------------------------------------------------------------------------- +# 1. Unknown component key raises ValueError +# --------------------------------------------------------------------------- + + +class TestUnknownComponentKeyRaises: + """Setting component_prefix with a key that is not in the default list + must raise a ``ValueError``.""" + + def test_unknown_key_raises(self): + manager = URDFAssemblyManager() + with pytest.raises(ValueError, match="cannot introduce new component"): + manager.component_prefix = [("nonexistent_part", "x_")] + + def test_multiple_items_with_one_unknown_raises(self): + manager = URDFAssemblyManager() + with pytest.raises(ValueError, match="cannot introduce new component"): + manager.component_prefix = [ + ("arm", "my_"), + ("totally_unknown", "bad_"), + ] + + def test_non_string_component_name_raises(self): + manager = URDFAssemblyManager() + with pytest.raises(ValueError): + # component name is an int, not a string + manager.component_prefix = [(42, "x_")] + + def test_non_list_raises(self): + manager = URDFAssemblyManager() + with pytest.raises(ValueError): + manager.component_prefix = {"arm": "my_"} # dict, not list + + +# --------------------------------------------------------------------------- +# 2. Per-component prefix patches preserve default ordering +# --------------------------------------------------------------------------- + + +class TestComponentPrefixPreservesOrder: + """Patching specific prefixes must not reorder the component list.""" + + def _default_order(self) -> list[str]: + m = URDFAssemblyManager() + return [comp for comp, _ in m.component_order_and_prefix] + + def test_patch_does_not_reorder(self): + manager = URDFAssemblyManager() + default_order = [comp for comp, _ in manager.component_order_and_prefix] + + # Patch a subset of components – order must remain the same + manager.component_prefix = [ + ("left_arm", "L_"), + ("right_arm", "R_"), + ] + + patched_order = [comp for comp, _ in manager.component_order_and_prefix] + assert patched_order == default_order + + def test_prefix_updated_correctly(self): + manager = URDFAssemblyManager() + manager.component_prefix = [("arm", "robot_arm_")] + + prefix_map = dict(manager.component_order_and_prefix) + assert prefix_map["arm"] == "robot_arm_" + + def test_unpatched_components_keep_original_prefix(self): + manager = URDFAssemblyManager() + original_prefix_map = dict(manager.component_order_and_prefix) + + # Only patch "head" – all others must keep their original prefix + manager.component_prefix = [("head", "hd_")] + + patched_prefix_map = dict(manager.component_order_and_prefix) + for comp, original_prefix in original_prefix_map.items(): + if comp == "head": + assert patched_prefix_map[comp] == "hd_" + else: + assert patched_prefix_map[comp] == original_prefix + + def test_multiple_patches_are_cumulative(self): + """Two successive patch calls must both take effect.""" + manager = URDFAssemblyManager() + manager.component_prefix = [("left_arm", "L_")] + manager.component_prefix = [("right_arm", "R_")] + + prefix_map = dict(manager.component_order_and_prefix) + assert prefix_map["left_arm"] == "L_" + assert prefix_map["right_arm"] == "R_" + + +# --------------------------------------------------------------------------- +# 3. name_case changes affect link / joint names via URDFComponentManager +# --------------------------------------------------------------------------- + + +class TestNameCaseAffectsNames: + """Changing ``name_case`` must propagate to the managers that rename + links and joints when processing URDF components.""" + + def _make_manager_and_process( + self, + tmp_path: str, + name_case: dict, + ) -> tuple[list, list]: + """Helper: create a component manager with given name_case, process a + minimal URDF, and return (links, joints) lists.""" + urdf_path = _write_urdf(tmp_path) + mesh_manager = URDFMeshManager(output_dir=tmp_path) + + comp_manager = URDFComponentManager( + mesh_manager=mesh_manager, + name_case=name_case, + ) + + # Use SimpleNamespace so the local urdf_path variable is captured correctly + import types + + fake_comp = types.SimpleNamespace( + urdf_path=urdf_path, + params=None, + transform=None, + ) + + links: list = [] + joints: list = [] + name_mapping: dict = {} + base_points: dict = {} + + comp_manager.process_component( + comp="chassis", + prefix=None, + comp_obj=fake_comp, + name_mapping=name_mapping, + base_points=base_points, + links=links, + joints=joints, + ) + return links, joints + + def test_link_names_lowercase(self, tmp_path): + links, _ = self._make_manager_and_process( + str(tmp_path), name_case={"link": "lower", "joint": "none"} + ) + for link in links: + name = link.get("name") + if name: + assert name == name.lower(), f"Link name not lowercase: {name!r}" + + def test_link_names_uppercase(self, tmp_path): + links, _ = self._make_manager_and_process( + str(tmp_path), name_case={"link": "upper", "joint": "none"} + ) + for link in links: + name = link.get("name") + if name: + assert name == name.upper(), f"Link name not uppercase: {name!r}" + + def test_joint_names_uppercase(self, tmp_path): + _, joints = self._make_manager_and_process( + str(tmp_path), name_case={"link": "none", "joint": "upper"} + ) + for joint in joints: + name = joint.get("name") + if name: + assert name == name.upper(), f"Joint name not uppercase: {name!r}" + + def test_joint_names_lowercase(self, tmp_path): + _, joints = self._make_manager_and_process( + str(tmp_path), name_case={"link": "none", "joint": "lower"} + ) + for joint in joints: + name = joint.get("name") + if name: + assert name == name.lower(), f"Joint name not lowercase: {name!r}" + + def test_name_case_none_preserves_original(self, tmp_path): + links, joints = self._make_manager_and_process( + str(tmp_path), name_case={"link": "none", "joint": "none"} + ) + link_names = {link.get("name") for link in links} + joint_names = {joint.get("name") for joint in joints} + # Original names from the fixture are mixed-case preserved exactly + assert "base_link" in link_names + assert "end_link" in link_names + assert "base_joint" in joint_names + + def test_assembly_manager_name_case_setter_validation(self): + manager = URDFAssemblyManager() + + with pytest.raises(ValueError, match="must be a dictionary"): + manager.name_case = "lower" # type: ignore[assignment] + + with pytest.raises(ValueError, match="must contain keys"): + manager.name_case = {"link": "lower"} # missing "joint" + + def test_assembly_manager_name_case_propagates_to_component_manager(self): + manager = URDFAssemblyManager() + manager.name_case = {"joint": "lower", "link": "upper"} + + assert manager.component_manager._name_case == { + "joint": "lower", + "link": "upper", + } + + +# --------------------------------------------------------------------------- +# 4. Signature changes when component_prefix or name_case changes +# --------------------------------------------------------------------------- + + +class TestSignatureChangesWithNamingSettings: + """Assembly signatures must differ when either ``component_prefix`` or + ``name_case`` differs, so stale URDF caches are correctly invalidated.""" + + _BASE_COMPONENT_INFO: dict = { + "__component_order_and_prefix__": [("arm", None), ("hand", None)], + "__name_case__": {"joint": "upper", "link": "lower"}, + } + + def _sig(self, component_info: dict) -> str: + mgr = URDFAssemblySignatureManager() + return mgr.calculate_assembly_signature(component_info, "/tmp/out.urdf") + + def test_baseline_is_stable(self): + """Same inputs must produce the same signature (determinism).""" + assert self._sig(self._BASE_COMPONENT_INFO) == self._sig( + self._BASE_COMPONENT_INFO + ) + + def test_prefix_change_invalidates_signature(self): + info_a = { + "__component_order_and_prefix__": [("arm", None), ("hand", None)], + "__name_case__": {"joint": "upper", "link": "lower"}, + } + info_b = { + "__component_order_and_prefix__": [("arm", "robot_"), ("hand", None)], + "__name_case__": {"joint": "upper", "link": "lower"}, + } + assert self._sig(info_a) != self._sig(info_b) + + def test_name_case_change_invalidates_signature(self): + info_a = { + "__component_order_and_prefix__": [("arm", None)], + "__name_case__": {"joint": "upper", "link": "lower"}, + } + info_b = { + "__component_order_and_prefix__": [("arm", None)], + "__name_case__": {"joint": "lower", "link": "lower"}, + } + assert self._sig(info_a) != self._sig(info_b) + + def test_order_change_invalidates_signature(self): + info_a = { + "__component_order_and_prefix__": [("arm", None), ("hand", None)], + "__name_case__": {"joint": "upper", "link": "lower"}, + } + info_b = { + "__component_order_and_prefix__": [("hand", None), ("arm", None)], + "__name_case__": {"joint": "upper", "link": "lower"}, + } + assert self._sig(info_a) != self._sig(info_b) + + def test_assembly_manager_signature_reflects_name_case(self, tmp_path): + """End-to-end: changing name_case on the manager changes the + signature that would be used to gate cache invalidation.""" + sig_mgr = URDFAssemblySignatureManager() + + manager_a = URDFAssemblyManager() + manager_b = URDFAssemblyManager() + manager_b.name_case = {"joint": "lower", "link": "lower"} + + output_path = str(tmp_path / "robot.urdf") + + def _compute_sig(m: URDFAssemblyManager) -> str: + component_info = m.component_registry.all().copy() + component_info["__component_order_and_prefix__"] = list( + m.component_order_and_prefix + ) + component_info["__name_case__"] = dict(m._name_case) + return sig_mgr.calculate_assembly_signature(component_info, output_path) + + assert _compute_sig(manager_a) != _compute_sig(manager_b) + + def test_assembly_manager_signature_reflects_component_prefix(self, tmp_path): + """End-to-end: changing component_prefix on the manager changes the + signature that would be used to gate cache invalidation.""" + sig_mgr = URDFAssemblySignatureManager() + + manager_a = URDFAssemblyManager() + manager_b = URDFAssemblyManager() + manager_b.component_prefix = [("arm", "robot_")] + + output_path = str(tmp_path / "robot.urdf") + + def _compute_sig(m: URDFAssemblyManager) -> str: + component_info = m.component_registry.all().copy() + component_info["__component_order_and_prefix__"] = list( + m.component_order_and_prefix + ) + component_info["__name_case__"] = dict(m._name_case) + return sig_mgr.calculate_assembly_signature(component_info, output_path) + + assert _compute_sig(manager_a) != _compute_sig(manager_b) + + +if __name__ == "__main__": + pytest.main([__file__, "-v"])