"""
Description
-----------
This file contains the URDF converter for transforming
arbitrary URDFs into URDFs that the Drake toolbox can
support.
"""
import coacd
from copy import copy, deepcopy
from doctest import UnexpectedException
from enum import IntEnum
import logging
import os
import trimesh
from typing import List
import numpy as np
from pathlib import Path
from pydrake.all import RigidTransform, RotationMatrix, RollPitchYaw
import shutil
import xml.etree.ElementTree as ET
# Internal Imports
from brom_drake.directories import DEFAULT_BROM_MODELS_DIR
from .config import DrakeReadyURDFConverterConfig, MeshReplacementStrategy
from .file_manager import DrakeReadyURDFConverterFileManager
from .mesh_file_converter import MeshFileConverter
from .util import (
does_drake_parser_support,
find_mesh_file_path_in,
get_mesh_element_in,
URDF_CONVERSION_LOG_LEVEL_NAME, URDF_CONVERSION_LEVEL,
tree_contains_transmission_for_joint,
create_transmission_element_for_joint,
)
from brom_drake.file_manipulation.urdf.shapes.cylinder import CylinderDefinition
from brom_drake.file_manipulation.urdf.simple_writer.urdf_element_creator import URDFElementCreator
[docs]
class DrakeReadyURDFConverter:
"""
*Description*
This class is used to convert a URDF file into a Drake-compatible URDF file.
It makes the following considerations when parsing a URDF file:
- If a mesh file is not supported by the Drake parser,
then it will convert it into a .obj file (which is supported).
- If a joint in the URDF file is not "fixed" (i.e., it is actuated)
and the parser finds no transmission element for it, then it will add
a transmission element to the URDF file.
*Parameters*
original_urdf_filename: str
The path to the original URDF file.
config: DrakeReadyURDFConverterConfig (optional)
The configuration for the URDF converter.
This controls:
- whether or not to convert unsupported mesh files
- whether or not to add missing transmissions
- etc.
"""
def __init__(
self,
original_urdf_filename: str,
config: DrakeReadyURDFConverterConfig = DrakeReadyURDFConverterConfig(),
):
"""
*Description*
This method initializes the DrakeReadyURDFConverter.
*Parameters*
original_urdf_filename: str
The path to the original URDF file.
config: DrakeReadyURDFConverterConfig (optional)
The configuration for the URDF converter.
This controls:
- whether or not to convert unsupported mesh files
- whether or not to add missing transmissions
- etc.
"""
# Setup
self.config = config
self.original_urdf_filename = original_urdf_filename
# Define the file manager which handles the tricky file paths that
# we encounter during the conversion process.
self.file_manager : DrakeReadyURDFConverterFileManager = DrakeReadyURDFConverterFileManager(
original_urdf_path=Path(self.original_urdf_filename),
models_directory=Path(DEFAULT_BROM_MODELS_DIR),
log_file_name=config.log_file_name,
_output_urdf_path=config.output_urdf_file_path,
)
# Define conversion data directory in brom
# (If it already exists, then we will overwrite it with the proper flag set)
self.file_manager.models_directory = Path(DEFAULT_BROM_MODELS_DIR)
overwrite_old_models = self.config.overwrite_old_models
if overwrite_old_models:
self.file_manager.clean_up_models_dir()
os.makedirs(self.file_manager.models_directory, exist_ok=True)
# Input Processing
original_urdf_path = Path(self.original_urdf_filename)
assert original_urdf_path.exists(), f"URDF file {original_urdf_path} does not exist!"
if not (".urdf" in original_urdf_path.name):
raise ValueError("Original URDF file must have a .urdf extension!")
# Set Up logger
self.logger = self.create_logger()
self.log(
f"Created a new DrakeReadyURDFConverter for file \"{self.original_urdf_filename}\".",
)
# Configure coacd, just in case
coacd.set_log_level(config.coacd_log_level)
# Keep Track of the Joints In The Diagram
self.actuated_joint_names = []
[docs]
def create_logger(self) -> logging.Logger:
"""
*Description*
This method configures the `logging` logger for the URDF conversion.
*Returns*
logging.Logger
The configured logger.
"""
# Setup
log_file_name = self.config.log_file_name
target_file = self.original_urdf_filename
# Check if the log level exists in the logging module
overwrite_old_logs = self.config.overwrite_old_logs
if overwrite_old_logs and os.path.exists(self.file_manager.output_file_directory() / log_file_name):
os.remove(self.file_manager.output_file_directory() / log_file_name)
# Create (or collect) a logger for the given file
logger = logging.getLogger("DrakeReadyURDFConverter ({target_file})" )
# Create the file handler
if not self.file_manager.output_file_directory().exists():
# Create the parent directory if it does not exist
self.file_manager.output_file_directory().mkdir(parents=True, exist_ok=True)
file_handler = logging.FileHandler(
self.file_manager.output_file_directory() / log_file_name,
mode='w'
)
file_handler.setLevel(logging.INFO)
# Create a formatter and set it for the file handler
formatter = logging.Formatter(
'%(asctime)s - %(name)s - %(levelname)s - %(message)s',
datefmt='%Y-%m-%d %H:%M:%S'
)
file_handler.setFormatter(formatter)
# Add the file handler to the logger
logger.addHandler(file_handler)
# Avoid duplicate logs
logger.propagate = False
# Make sure the logger responds to all messages of level DEBUG and above
logger.setLevel(logging.DEBUG) # Set to DEBUG to capture all messages
return logger
[docs]
def convert_collision_element(self, collision_elt: ET.Element) -> List[ET.Element]:
"""
*Description*
This method will convert the collision element in the URDF file.
*Parameters*
collision_elt: xml.etree.ElementTree.Element
The collision element that we want to convert.
*Returns*
List[xml.etree.ElementTree.Element]
The collision elements that will replace the original collision element.
Usually, this is a list of length 1, but in some cases (e.g., convex decomposition)
it may be longer.
"""
# Setup
replacement_strategy = self.config.mesh_replacement_strategies.collision_meshes
supported_strategies = [
MeshReplacementStrategy.kWithObj,
MeshReplacementStrategy.kWithMinimalEnclosingCylinder,
MeshReplacementStrategy.kWithConvexDecomposition,
]
# Input Processing
if replacement_strategy not in supported_strategies:
error_message = (
f"Replacement strategy {replacement_strategy} is not supported for collision elements. "
+ "Supported strategies are: "
+ ", ".join([str(strategy) for strategy in supported_strategies])
)
self.log(error_message)
raise ValueError(error_message)
# Announce algorithm start
self.log(
f"Converting element with tag \"{collision_elt.tag}\" using strategy {replacement_strategy}."
)
n_replacements_made = 0
new_elts = []
# If the strategy is kWithMinimalEnclosingCylinder,
# then we can replace the entire collision element and don't need to iterate through the children.
if replacement_strategy == MeshReplacementStrategy.kWithMinimalEnclosingCylinder:
n_replacements_made += 1
new_elts = [self.replace_element_with_enclosing_cylinder(
deepcopy(collision_elt)
)]
elif replacement_strategy == MeshReplacementStrategy.kWithObj:
# Otherwise, let's slowly build the new element
# by iterating through the children of the collision element.
new_elt = ET.Element(collision_elt.tag, collision_elt.attrib)
for ii, child_ii in enumerate(collision_elt):
# Check to see if the child is a geometry element
if child_ii.tag == "geometry":
new_geometry_elt = self.convert_geometry_element(child_ii, replacement_strategy)
new_elt.append(new_geometry_elt)
else:
# If the child is not a geometry element, then we will just copy it over
new_elt.append(deepcopy(child_ii))
# Add the new element to the list of new elements
new_elts.append(new_elt)
elif replacement_strategy == MeshReplacementStrategy.kWithConvexDecomposition:
# Determine if the collision element has a mesh element within it
original_mesh_path = find_mesh_file_path_in(collision_elt)
if original_mesh_path is None:
self.log(
"The collision element does not contain a mesh file. Cannot replace with convex decomposition."
)
new_elts.append(deepcopy(collision_elt))
return new_elts
# If the collision element has a mesh element within it, then we will perform convex decomposition
original_mesh_elt = get_mesh_element_in(collision_elt)
scale_as_str = original_mesh_elt.attrib.get("scale", "1 1 1")
original_mesh_scale = np.array(
[float(x) for x in scale_as_str.split()]
)
# Load the mesh file and perform convex decomposition
self.log(
f"Found a mesh file at {original_mesh_path}. Performing convex decomposition."
)
# Load the mesh file
mesh: trimesh.Trimesh = trimesh.load(
self.file_manager.file_path_in_context_of_urdf_dir(original_mesh_path),
force="mesh",
)
mesh = coacd.Mesh(mesh.vertices, mesh.faces)
parts = coacd.run_coacd(mesh) # a list of convex hulls
self.log(
f"coacd created {len(parts)} convex parts from the mesh file \"{original_mesh_path}\""
)
for ii, part_ii in enumerate(parts):
# print(part_ii)
# print(type(part_ii))
# print(len(part_ii))
# Construct new mesh and choose the path to export it to
mesh_ii = trimesh.Trimesh(
vertices=part_ii[0], faces=part_ii[1]
)
mesh_file_name_ii = original_mesh_path.name[:original_mesh_path.name.index(".")]
mesh_ii_file_relative_path = f"meshes/{mesh_file_name_ii}/{mesh_file_name_ii}_part_{ii}.obj"
mesh_ii_file_path = self.file_manager.output_file_directory() / mesh_ii_file_relative_path
# Export after checking that the parent directory exists
os.makedirs(mesh_ii_file_path.parent, exist_ok=True)
mesh_ii.export(mesh_ii_file_path)
# Create a new collision element for each part
collision_element_ii = URDFElementCreator.create_collision_element(
name=f"{collision_elt.attrib.get('name', 'collision')}_part_{ii}",
mesh_file_path="./" + str(mesh_ii_file_relative_path),
mesh_scale=original_mesh_scale,
)
new_elts.append(collision_element_ii)
n_replacements_made += 1
# Check to see if the collision element has a mesh element within it
# # Iterate through every element of the collision element
# for ii, child_ii in enumerate(collision_elt):
# if child_ii.tag == "geometry":
# # If the child is a geometry element, then we will check if replacement is needed.
# geometry_elt = child_ii
# # Iterate through every element of the geometry element
# for jj, child_jj in enumerate(geometry_elt):
# if child_jj.tag == "mesh":
# # If the child is a mesh element, then we will check if replacement is needed.
# mesh_elt = child_jj
# # Replace!
# if replacement_strategy == MeshReplacementStrategy.kWithObj:
# new_mesh_elt = self.convert_mesh_element(mesh_elt)
# new_elt[ii][jj] = new_mesh_elt
# elif replacement_strategy == MeshReplacementStrategy.kWithMinimalEnclosingCylinder:
# new_elt = self.replace_element_with_enclosing_cylinder(new_elt)
# # elif replacement_strategy == MeshReplacementStrategy.kWithConvexDecomposition:
# else:
# raise ValueError(
# f"Invalid mesh replacement strategy: {replacement_strategy}"
# )
# n_replacements_made += 1
# Announce the number of replacements made in the logger.
self.log(
f"Made {n_replacements_made} replacements in the collision element."
)
return new_elts
[docs]
def convert_geometry_element(
self,
geometry_elt: ET.Element,
replacement_strategy: MeshReplacementStrategy
) -> ET.Element:
"""
*Description*
This method will convert the geometry element in the URDF (i.e., an XML) file
according to the conversion strategy provided as input.
*Parameters*
geometry_elt: xml.etree.ElementTree.Element
The geometry element that we want to convert.
replacement_strategy: MeshReplacementStrategy
The strategy to use for replacing mesh files.
*Returns*
xml.etree.ElementTree.Element
The new geometry element.
"""
# Setup
new_elt = ET.Element(geometry_elt.tag, geometry_elt.attrib)
# Algorithm
self.log(
f"Converting geometry element with strategy \"{replacement_strategy}\"."
)
# Iterate through every element of the geometry element
for ii, child_ii in enumerate(geometry_elt):
if child_ii.tag == "mesh":
# If the child is a mesh element, then we will check if replacement is needed.
mesh_elt = child_ii
# Replace!
new_mesh_elt = self.convert_mesh_element(mesh_elt)
new_elt.append(new_mesh_elt)
else:
# If the child is not a mesh element, then we will just copy it over
new_elt.append(deepcopy(child_ii))
return new_elt
[docs]
def convert_visual_element(self, visual_elt: ET.Element) -> ET.Element:
"""
*Description*
This method will convert the collision element in the URDF file.
For now, this is very similar to the collision element handling, but it may
change in the future.
*Parameters*
visual_elt: xml.etree.ElementTree.Element
The visual element that we want to convert.
*Returns*
xml.etree.ElementTree.Element
The new visual element.
"""
# Setup
new_elt = deepcopy(visual_elt)
replacement_strategy = self.config.mesh_replacement_strategies.visual_meshes
# Algorithm
self.log(
f"Converting element with tag \"{visual_elt.tag}\" using the \"{replacement_strategy}\" strategy."
)
# Search through all children of the visual element
n_replacements_made = 0
added_material = False
for ii, child_ii in enumerate(visual_elt):
if child_ii.tag == "geometry":
# If the child is a geometry element, then we will check if replacement is needed.
geometry_elt = child_ii
# Iterate through every element of the geometry element
for jj, child_jj in enumerate(geometry_elt):
if child_jj.tag == "mesh":
# If the child is a mesh element, then we will check if replacement is needed.
mesh_elt = child_jj
# Replace!
if replacement_strategy == MeshReplacementStrategy.kWithObj:
new_mesh_elt = self.convert_mesh_element(mesh_elt)
new_elt[ii][jj] = new_mesh_elt
# TODO: Implement this
# elif replacement_strategy == MeshReplacementStrategy.kDoNotReplace:
# # Copy the associated file into the models directory
# old_mesh_file = mesh_elt.attrib["filename"]
# temp_mfc = MeshFileConverter(
# mesh_file_path=old_mesh_file,
# urdf_dir=original_urdf_dir,
# new_urdf_dir=self.file_manager.output_file_directory(),
# )
# true_old_mesh_file = temp_mfc.true_mesh_file_path()
# new_mesh_full_path = temp_mfc.define_output_path()
# print(f"Copying {true_old_mesh_file} to {new_mesh_full_path}")
# # Copy!
# shutil.copy(true_old_mesh_file, new_mesh_full_path)
else:
raise ValueError(
f"Invalid mesh replacement strategy: {replacement_strategy}"
)
n_replacements_made += 1
elif child_ii.tag == "material":
new_material_elt = self.create_new_material_element(child_ii)
# replace the material element in new_elt with this one
new_elt[ii] = new_material_elt
added_material = True
if self.config.replace_colors_with is not None:
self.log(
f"Replaced the color of the material element with RGBA values {self.config.replace_colors_with}."
)
# If material did not exist in the visual element, then we will add it.
if not added_material:
# Create a new material element
new_material_elt = ET.Element("material")
new_elt.append(new_material_elt)
transformed_material_elt = self.create_new_material_element(new_material_elt)
new_elt[-1] = transformed_material_elt
self.log(
f"Added a new material element to the visual element with RGBA values {self.config.replace_colors_with}."
)
n_replacements_made += 1
added_material = True
return new_elt
[docs]
def convert_urdf(self) -> Path:
"""
*Description*
Converts the original URDF file into a Drake-compatible URDF file.
*Returns*
Path
The path to the new URDF file.
"""
# Setup
original_urdf_path = Path(self.original_urdf_filename)
original_xml = ET.ElementTree(file=original_urdf_path)
add_missing_actuators = self.config.add_missing_actuators
# Use recursive function to convert the tree
new_trees = self.convert_tree(original_xml)
assert len(new_trees) == 1, \
f"Expected to get exactly one new tree after conversion, but got {len(new_trees)}."
new_tree = new_trees[0]
# Add transmissions, if needed
if add_missing_actuators:
for joint_name in self.actuated_joint_names:
if tree_contains_transmission_for_joint(new_tree, joint_name):
continue
else:
# If transmission doesn't exist in URDF, then add it!
transmission_element = create_transmission_element_for_joint(joint_name)
ET.indent(transmission_element, space="\t", level=0)
new_tree.getroot().append(transmission_element)
# Output the new tree to a file
output_urdf_path = self.file_manager.output_urdf_path
os.makedirs(output_urdf_path.parent, exist_ok=True)
new_tree.write(output_urdf_path)
self.log(
f"Converted URDF file to Drake-compatible URDF file at {output_urdf_path}.\n\n"
)
return output_urdf_path
[docs]
def convert_mesh_element(self, mesh_elt_in: ET.Element) -> ET.Element:
"""
*Description*
This method will convert the mesh element in the URDF file
into a Drake-compatible mesh element.
*Parameters*
mesh_elt_in: xml.etree.ElementTree.Element
The mesh element that we want to convert.
*Returns*
xml.etree.ElementTree.Element
The new mesh element.
"""
# Setup
new_elt = deepcopy(mesh_elt_in)
# Algorithm
self.log(
f"Found a mesh element with filename \"{mesh_elt_in.attrib['filename']}\"."
)
# Check the value of the filename
if not ("filename" in new_elt.attrib):
raise ValueError(
f"Found a mesh element that does not contain the \"filename\" attribute.\n"
+ "Brom doesn't know how to handle this!"
)
# Filename exists; Let's check to see if it's obj or not
if does_drake_parser_support(new_elt.attrib["filename"]):
self.log(
f"\"{new_elt.attrib['filename']}\" is already supported by the Drake parser. Let's copy it into the right place."
)
# Let's try to copy the old file into the new directory
old_filename = new_elt.attrib["filename"]
new_filename = self.create_obj_to_replace_mesh_file(old_filename)
new_elt.set(
"filename", new_filename,
)
self.log(
f"Copied the Drake-compatible mesh file \"{old_filename}\" to \"{new_filename}\"."
)
else:
# If parser does not support the given filename,
# then let's try to create one
old_filename = new_elt.attrib["filename"]
new_filename = self.create_obj_to_replace_mesh_file(old_filename)
new_elt.set(
"filename", new_filename,
)
self.log(
f"Replaced the mesh file \"{old_filename}\" with a Drake-compatible .obj file at \"{new_filename}\"."
)
return new_elt
[docs]
def convert_tree(
self,
current_tree: ET.ElementTree
) -> List[ET.ElementTree]:
"""
*Description*
This recursive method is used to parse each element of the
xml element tree and replace the components that are not
supported by the Drake URDF parser.
*Parameters*
current_tree: xml.etree.ElementTree.ElementTree
The current tree that we are parsing.
*Returns*
List[xml.etree.ElementTree.ElementTree]
The new trees that has been converted.
"""
# Setup
root = deepcopy(current_tree.getroot())
# Modify root, if necessary
new_elements = self.convert_tree_element(root)
for element_ii in new_elements:
# Modify Children of the root
initial_children = list(element_ii)
for child in initial_children:
new_children = self.convert_tree(
ET.ElementTree(child)
)
element_ii.remove(child)
# Add new children to new root
for new_child in new_children:
# Convert the new child element
element_ii.append(new_child.getroot())
return [
ET.ElementTree(element_ii) for element_ii in new_elements
]
[docs]
def convert_tree_element(
self,
elt: ET.Element,
) -> List[ET.Element]:
"""
*Description*
This method is used to transform one element in the XML tree into an eleemnt
that that Drake URDF parser can handle. If necessary, it will create a new
3d model file and place it into the right directory.
*Parameters*
elt: xml.etree.ElementTree.Element
An element in the ElementTree that we would like to convert
into a Drake-ready element.
*Returns*
List[xml.etree.ElementTree.Element]
The new elements that has been converted.
"""
# Setup
new_elts = []
# Convert file paths for mesh elements if necessary
if elt.tag == "collision":
new_elts = self.convert_collision_element(elt)
elif elt.tag == "visual":
new_elts = [self.convert_visual_element(elt)]
elif elt.tag == "transmission":
# Ignore transmission elements during the conversion
new_elts = [deepcopy(elt)]
elif elt.tag == "joint":
self.handle_joint_element(elt)
new_elts = [deepcopy(elt)]
else:
# If the element is not a mesh element, then we will just return a copy of it.
self.log(
f"Found an element with tag \"{elt.tag}\" that does not require handling. No modification needed..."
)
new_elts = [deepcopy(elt)]
# Otherwise, just return a copy of the previous element.
return new_elts
[docs]
def create_obj_to_replace_mesh_file(
self,
mesh_file_name: str,
) -> str:
"""
*Description*
This function will create an .obj file to replace the
.stl or .dae file that is given in mesh file "mesh_file_name".
*Parameters*
mesh_file_name: str
The name of the mesh file that we want to convert.
*Returns*
str
The name of the new .obj file that has been created.
"""
# Setup
original_urdf_dir = Path(self.original_urdf_filename).parent
# Convert the file
converter = MeshFileConverter(
mesh_file_path=mesh_file_name,
urdf_dir=Path(original_urdf_dir),
new_urdf_dir=self.file_manager.output_file_directory(),
logger=self.logger,
)
new_mesh_path = converter.convert()
# Clip off all parts of path that include the exported output
new_mesh_path = str(new_mesh_path).replace(str(self.file_manager.output_file_directory()), "")
return "." + str(new_mesh_path)
[docs]
def handle_joint_element(self, joint_elt: ET.Element):
"""
*Description*
This method will handle the joint element in the URDF file.
Specifically, it will check to see if the joint is actuated
and (if it is) add it to the list of actuated joints.
This is useful when attempting to add transmissions to the URDF file
later on.
*Parameters*
joint_elt: xml.etree.ElementTree.Element
The joint element that we want to handle.
*Returns*
None
"""
# Setup
# Keep track of all FREE joint names
# If the joint does not have a "type" attribute, then we will assume it is fixed
# and won't add it.
if "type" not in joint_elt.attrib:
return
# If joint is "fixed", then don't add it to the list of actuated joints
joint_is_fixed = joint_elt.attrib["type"] == "fixed"
if joint_is_fixed:
return
joint_name = joint_elt.attrib["name"]
self.actuated_joint_names.append(joint_name)
[docs]
def create_new_material_element(self, material_elt: ET.Element):
"""
*Description*
This method will handle the material element in the URDF file.
Specifically, it will check to see if the user wants us to assign a color to the
material element. If so, then we will assign the color to the material element.
This is useful when attempting to add transmissions to the URDF file
later on.
*Parameters*
material_elt: xml.etree.ElementTree.Element
The material element that we want to handle.
*Returns*
None, but modifies the material element.
"""
# Setup
replace_colors_with = self.config.replace_colors_with
material_elt = deepcopy(material_elt)
# Input Processing (Abort, if no color replacement is needed)
if replace_colors_with is None:
return material_elt
if len(replace_colors_with) != 4:
raise ValueError(
"The color replacement list must have exactly 4 elements (RGBA)."
)
# Algorithm
# Check to see if the material element has a color element
color_elt = material_elt.find("color")
if color_elt is None:
# If no color element exists, then we will create one
color_elt = ET.Element("color")
material_elt.append(color_elt)
else:
# If a color element exists, then we will modify it
material_elt.remove(color_elt)
color_elt = ET.Element("color")
material_elt.append(color_elt)
# Set the color of the material element
color_elt.set(
"rgba",
f"{replace_colors_with[0]} {replace_colors_with[1]} {replace_colors_with[2]} {replace_colors_with[3]}"
)
return material_elt
[docs]
def log(self, message: str):
"""
*Description*
Logs a message to the logging.logger.
*Parameters*
message: str
The message that we want to log.
*Returns*
None
"""
self.logger.log(logging.INFO, message)
[docs]
def replace_element_with_enclosing_cylinder(self, collision_elt: ET.Element) -> ET.Element:
"""
*Description*
This method will replace the geometry element with an enclosing cylinder.
*Parameters*
collision_elt: xml.etree.ElementTree.Element
The collision element that we want to replace.
*Returns*
xml.etree.ElementTree.Element
The new collision element with the enclosing cylinder.
"""
# Setup
new_elt = deepcopy(collision_elt)
original_urdf_dir = Path(self.original_urdf_filename).parent
# Save the initial pose of the element
origin_elt = new_elt.find("origin")
pose0 = RigidTransform(
R=RollPitchYaw(np.array(
[float(elt) for elt in origin_elt.attrib["rpy"].split(" ")]
)).ToRotationMatrix(),
p=np.array(
[float(elt) for elt in origin_elt.attrib["xyz"].split(" ")],
),
)
# Extract the mesh element and the filename that it references
mesh_elt = new_elt.find("geometry/mesh")
mesh_file_name = mesh_elt.attrib["filename"]
# Load the mesh from the mesh element
converter = MeshFileConverter(
mesh_file_path=mesh_file_name,
urdf_dir=Path(original_urdf_dir),
new_urdf_dir=self.file_manager.output_file_directory(),
logger=self.logger,
)
mesh = trimesh.load_mesh(
str(converter.urdf_dir / converter.true_mesh_file_path())
)
# Create the enclosing cylinder
cylinder_result = trimesh.bounds.minimum_cylinder(mesh, sample_count=12)
cylinder_shape = CylinderDefinition(
radius=cylinder_result['radius'],
length=cylinder_result['height'],
)
# Remove the mesh element and pose element from the collision element, if they exist
geometry_elt = new_elt.find("geometry")
if mesh_elt is not None:
new_elt.remove(geometry_elt)
origin_elt = new_elt.find("origin")
if origin_elt is not None:
new_elt.remove(origin_elt)
# Add the origin element to the collision element
transform = cylinder_result['transform']
transform_as_matrix = np.array(transform)
transform_as_rt = RigidTransform(
RotationMatrix(transform_as_matrix[:3, :3]),
transform_as_matrix[:3, 3],
)
pose_as_rt = pose0.multiply(transform_as_rt)
ET.SubElement(
new_elt,
"origin",
{
"xyz": f"{pose_as_rt.translation()[0]} {pose_as_rt.translation()[1]} {pose_as_rt.translation()[2]}",
"rpy": f"{pose_as_rt.rotation().ToRollPitchYaw().roll_angle()} {pose_as_rt.rotation().ToRollPitchYaw().pitch_angle()} {pose_as_rt.rotation().ToRollPitchYaw().yaw_angle()}",
},
)
# Create a new geometry element and add the cylinder element to it
new_elt.append(ET.Element("geometry"))
cylinder_shape.add_geometry_to_element(new_elt.find("geometry"))
return new_elt