Coverage for trimesh/exchange/urdf.py: 91%

1import os 

2 

3import numpy as np 

4 

5from ..constants import log, tol 

6from ..version import __version__ 

7 

8 

9def export_urdf(mesh, directory, scale=1.0, color=None, **kwargs): 

10 """ 

11 Convert a Trimesh object into a URDF package for physics 

12 simulation. This breaks the mesh into convex pieces and 

13 writes them to the same directory as the .urdf file. 

14 

15 Parameters 

16 --------- 

17 mesh : trimesh.Trimesh 

18 Input geometry 

19 directory : str 

20 The directory path for the URDF package 

21 

22 Returns 

23 --------- 

24 mesh : Trimesh 

25 Multi-body mesh containing convex decomposition 

26 """ 

27 

28 import lxml.etree as et 

29 

30 # TODO: fix circular import 

31 from .export import export_mesh 

32 

33 # Extract the save directory and the file name 

34 fullpath = os.path.abspath(directory) 

35 name = os.path.basename(fullpath) 

36 _, ext = os.path.splitext(name) 

37 

38 if ext != "": 

39 raise ValueError("URDF path must be a directory!") 

40 

41 # Create directory if needed 

42 if not os.path.exists(fullpath): 

43 os.mkdir(fullpath) 

44 elif not os.path.isdir(fullpath): 

45 raise ValueError("URDF path must be a directory!") 

46 

47 # Perform a convex decomposition 

48 try: 

49 convex_pieces = mesh.convex_decomposition() 

50 except BaseException: 

51 log.error("problem with convex decomposition, using hull", exc_info=True) 

52 convex_pieces = [mesh.convex_hull] 

53 

54 # Get the effective density of the mesh 

55 effective_density = mesh.volume / sum([m.volume for m in convex_pieces]) 

56 

57 # open an XML tree 

58 root = et.Element("robot", name="root") 

59 

60 # Loop through all pieces, adding each as a link 

61 prev_link_name = None 

62 for i, piece in enumerate(convex_pieces): 

63 # Save each nearly convex mesh out to a file 

64 piece_name = f"{name}_convex_piece_{i}" 

65 piece_filename = f"{piece_name}.obj" 

66 piece_filepath = os.path.join(fullpath, piece_filename) 

67 export_mesh(piece, piece_filepath) 

68 

69 # Set the mass properties of the piece 

70 piece.center_mass = mesh.center_mass 

71 piece.density = effective_density * mesh.density 

72 

73 link_name = f"link_{piece_name}" 

74 geom_name = f"{piece_filename}" 

75 I = [["{:.2E}".format(y) for y in x] for x in piece.moment_inertia] # NOQA 

76 

77 # Write the link out to the XML Tree 

78 link = et.SubElement(root, "link", name=link_name) 

79 

80 # Inertial information 

81 inertial = et.SubElement(link, "inertial") 

82 et.SubElement(inertial, "origin", xyz="0 0 0", rpy="0 0 0") 

83 et.SubElement(inertial, "mass", value=f"{piece.mass:.2E}") 

84 et.SubElement( 

85 inertial, 

86 "inertia", 

87 ixx=I[0][0], 

88 ixy=I[0][1], 

89 ixz=I[0][2], 

90 iyy=I[1][1], 

91 iyz=I[1][2], 

92 izz=I[2][2], 

93 ) 

94 # Visual Information 

95 visual = et.SubElement(link, "visual") 

96 et.SubElement(visual, "origin", xyz="0 0 0", rpy="0 0 0") 

97 geometry = et.SubElement(visual, "geometry") 

98 et.SubElement( 

99 geometry, 

100 "mesh", 

101 filename=geom_name, 

102 scale=f"{scale:.4E} {scale:.4E} {scale:.4E}", 

103 ) 

104 material = et.SubElement(visual, "material", name="") 

105 if color is not None: 

106 et.SubElement( 

107 material, "color", rgba=f"{color[0]:.2E} {color[1]:.2E} {color[2]:.2E} 1" 

108 ) 

109 

110 # Collision Information 

111 collision = et.SubElement(link, "collision") 

112 et.SubElement(collision, "origin", xyz="0 0 0", rpy="0 0 0") 

113 geometry = et.SubElement(collision, "geometry") 

114 et.SubElement( 

115 geometry, 

116 "mesh", 

117 filename=geom_name, 

118 scale=f"{scale:.4E} {scale:.4E} {scale:.4E}", 

119 ) 

120 

121 # Create rigid joint to previous link 

122 if prev_link_name is not None: 

123 joint_name = f"{link_name}_joint" 

124 joint = et.SubElement(root, "joint", name=joint_name, type="fixed") 

125 et.SubElement(joint, "origin", xyz="0 0 0", rpy="0 0 0") 

126 et.SubElement(joint, "parent", link=prev_link_name) 

127 et.SubElement(joint, "child", link=link_name) 

128 

129 prev_link_name = link_name 

130 

131 # Write URDF file 

132 tree = et.ElementTree(root) 

133 urdf_filename = f"{name}.urdf" 

134 tree.write(os.path.join(fullpath, urdf_filename), pretty_print=True) 

135 

136 # Write Gazebo config file 

137 root = et.Element("model") 

138 model = et.SubElement(root, "name") 

139 model.text = name 

140 version = et.SubElement(root, "version") 

141 version.text = "1.0" 

142 sdf = et.SubElement(root, "sdf", version="1.4") 

143 sdf.text = f"{name}.urdf" 

144 

145 author = et.SubElement(root, "author") 

146 et.SubElement(author, "name").text = f"trimesh {__version__}" 

147 et.SubElement(author, "email").text = "[email protected]" 

148 

149 description = et.SubElement(root, "description") 

150 description.text = name 

151 tree = et.ElementTree(root) 

152 

153 if tol.strict: 

154 from ..resources import get_stream 

155 

156 # todo : we don't pass the URDF schema validation 

157 schema = et.XMLSchema(file=get_stream("schema/urdf.xsd")) 

158 if not schema.validate(tree): 

159 # actual error isn't raised by validate 

160 log.debug(schema.error_log) 

161 

162 tree.write(os.path.join(fullpath, "model.config")) 

163 return np.sum(convex_pieces)