Source code for sapien.wrapper.urdf_exporter

from xml.etree import ElementTree as ET
from ..pysapien.physx import PhysxArticulationLinkComponent, PhysxArticulationJoint






[docs] def export_joint(joint: PhysxArticulationJoint): name = f"joint_{joint.child_link.index}" if joint.parent_link is None: type = "fixed" if joint.type == "fixed" else "floating" elem_joint = ET.Element("joint", {"name": name, "type": type}) ET.SubElement(elem_joint, "parent", {"link": "world"}) ET.SubElement(elem_joint, "child", {"link": f"link_{joint.child_link.index}"}) return [elem_joint] if joint.type == "fixed": type = "fixed" elif joint.type == "prismatic": type = "prismatic" elif joint.type.startswith("revolute"): if joint.limit[0][0] < -1e5 and joint.limit[0][1] > 1e5: type = "continuous" else: type = "revolute" else: raise Exception(f"invalid joint type {joint.type}") j2p = joint.pose_in_parent c2j = joint.pose_in_child.inv() angles = j2p.rpy # dummy link at joint frame elem_dummy_link = ET.Element( "link", {"name": f"link_dummy_{joint.child_link.index}"} ) # joint connecting parent and dummy elem_joint = ET.Element("joint", {"name": name, "type": type}) ET.SubElement( elem_joint, "origin", { "xyz": f"{j2p.p[0]} {j2p.p[1]} {j2p.p[2]}", "rpy": f"{angles[0]} {angles[1]} {angles[2]}", }, ) ET.SubElement(elem_joint, "axis", {"xyz": "1 0 0"}) ET.SubElement(elem_joint, "parent", {"link": f"link_{joint.parent_link.index}"}) ET.SubElement(elem_joint, "child", {"link": f"link_dummy_{joint.child_link.index}"}) if type == "prismatic" or type.startswith("revolute"): ET.SubElement( elem_joint, "limit", { "effort": "0", "velocity": "0", "lower": str(max(joint.limit[0][0], -1e5)), "upper": str(min(joint.limit[0][1], 1e5)), }, ) # fixed joint connecting dummy and child elem_dummy_joint = ET.Element( "joint", {"name": f"joint_dummy_{joint.child_link.index}", "type": "fixed"} ) angles = c2j.rpy ET.SubElement( elem_dummy_joint, "origin", { "xyz": f"{c2j.p[0]} {c2j.p[1]} {c2j.p[2]}", "rpy": f"{angles[0]} {angles[1]} {angles[2]}", }, ) ET.SubElement(elem_dummy_joint, "axis", {"xyz": "0 0 0"}) ET.SubElement( elem_dummy_joint, "parent", {"link": f"link_dummy_{joint.child_link.index}"} ) ET.SubElement(elem_dummy_joint, "child", {"link": f"link_{joint.child_link.index}"}) return [elem_joint, elem_dummy_link, elem_dummy_joint]
[docs] def export_kinematic_chain_xml(articulation): elem_robot = ET.Element("robot", {"name": ""}) ET.SubElement(elem_robot, "link", {"name": "world"}) for l in articulation.links: elem_robot.append(export_link(l)) for j in articulation.joints: for e in export_joint(j): elem_robot.append(e) return elem_robot
[docs] def export_kinematic_chain_urdf(articulation, force_fix_root=False): xml = export_kinematic_chain_xml(articulation) if force_fix_root: for j in xml.findall("joint"): if j.attrib["type"] == "floating": j.attrib["type"] = "fixed" return ET.tostring(xml, encoding="utf8").decode()