diff --git a/phobos/core/robot.py b/phobos/core/robot.py index 82ca2134..3b4f0096 100755 --- a/phobos/core/robot.py +++ b/phobos/core/robot.py @@ -456,7 +456,8 @@ def export_kccd(self, outputdir, rel_iv_meshes_path, output_mesh_format, join_be origin=link.collisions[0].origin, geometry=link.collisions[0].geometry, )) - kccd_robot.export_urdf(outputfile=kccd_urdf) + kccd_robot.export_meshes(kccd_meshes, format="stl", apply_scale=True) + kccd_robot.export_urdf(outputfile=kccd_urdf, mesh_format="stl") execute_shell_command("urdf2kccd -b " + self.name.replace('/','_') + ".urdf", cwd=kccd_path) kccd_kinematics_file = open(kccd_urdf[:-5] + "Kinematics.cfg", "r").read().split("\n\n") kccd_kinematics = {} @@ -658,12 +659,14 @@ def add_joint(joint): graph = pydot.graph_from_dot_data(out) graph[0].write_pdf(outputfile) - def export(self, outputdir, export_config, rel_mesh_pathes=None, ros_pkg_name=None, no_smurf=False, + def export(self, outputdir, export_config=None, rel_mesh_pathes=None, ros_pkg_name=None, no_smurf=False, ros_pkg_later=False, check_submechs=True, with_meshes=True, use_existing_meshes=False, apply_scale=False): assert self.check_linkage() outputdir = os.path.abspath(outputdir) if rel_mesh_pathes is None: rel_mesh_pathes = resources.get_default_rel_mesh_pathes() + if export_config is None: + export_config = resources.get_default_export_config() xml_file_in_smurf = None ros_pkg = False if ros_pkg_name is None: @@ -1176,7 +1179,7 @@ def instantiate_submodel(self, name=None, start=None, stop=None, robotname=None, for joint in submodel.joints: if submodel.get_link(joint.origin.relative_to) is None and submodel.get_joint(joint.origin.relative_to) is None: joint.origin = representation.Pose.from_matrix(self.get_transformation(end=vc.origin.relative_to, start=link), relative_to=joint.parent).dot(vc.origin) - submodel.export_pdf("test.pdf") + submodel.link_entities() if abstract_model and len(self.submechanisms) > 0: diff --git a/phobos/geometry/robot.py b/phobos/geometry/robot.py index 689ee056..47a62dd1 100644 --- a/phobos/geometry/robot.py +++ b/phobos/geometry/robot.py @@ -68,8 +68,6 @@ def get_meshes_of_link_and_fixed_children(parent, transform=np.identity(4)): mesh = geometry.reduce_mesh(mesh, 1-reduce_meshes) mesh_representation = representation.Mesh(mesh=mesh, meshname="collision_"+linkname) - # [TODO pre_v2.0.0] Review whether the mesh export happens on export of this xml anyways - mesh_representation.provide_mesh_file(targetpath=outputdir, format="stl") link.add_aggregate("collision", representation.Collision( origin=representation.Pose(rpy=[0, 0, 0], xyz=[0, 0, 0], relative_to=link), geometry=mesh_representation, diff --git a/phobos/io/representation.py b/phobos/io/representation.py index 72084a2c..27349ad2 100644 --- a/phobos/io/representation.py +++ b/phobos/io/representation.py @@ -653,10 +653,9 @@ def abs_filepath(self): else: assert self._related_robot_instance is not None assert self._related_robot_instance.mesh_format is not None - assert self._related_robot_instance.mesh_format != "input_type" or self.input_type.startswith("file"), \ - f"No input file to derive the format from! {self} {self.input_type}" format = self._related_robot_instance.mesh_format if format == "input_type": + assert self.input_type.startswith("file"), f"No input file to derive the format from!" format = self.input_type[5:] if format not in self._exported: raise IOError(f"The mesh {self.unique_name} with the required mesh format ({format}) has not yet been exported.")