├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── documentation ├── figures │ ├── cad_cs_gilotina.png │ ├── gilotina2.png │ ├── robot_arm_cad_with_cs.PNG │ ├── rviz moved.png │ ├── rviz tf center.png │ └── rviz_tf_gilotina.png ├── jsi-logo-1-150x150.png ├── reconcycle-transparent.png └── rosin_eu_flag.jpg ├── install_python_occ.sh ├── launch ├── build_urdf_from_step.launch └── template.launch ├── package.xml ├── scripts └── create_urdf.py ├── setup.py └── src └── import_asembly ├── Asembly_import.py └── __init__.py /.gitignore: -------------------------------------------------------------------------------- 1 | src/import_asembly/__pycache__/Asembly_import.cpython-38.pyc 2 | .vscode/* 3 | build/* 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(urdf_from_step) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | 22 | 23 | catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs # Or other packages containing msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES urdf_from_step 108 | # CATKIN_DEPENDS rospy 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/urdf_from_step.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/urdf_from_step_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | catkin_install_python(PROGRAMS 163 | scripts/create_urdf.py 164 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | ) 166 | 167 | ## Mark executables for installation 168 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 169 | # install(TARGETS ${PROJECT_NAME}_node 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark libraries for installation 174 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 175 | # install(TARGETS ${PROJECT_NAME} 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | 190 | 191 | 192 | install( 193 | DIRECTORY 194 | launch/ 195 | DESTINATION 196 | ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 197 | FILES_MATCHING PATTERN "*.launch" 198 | ) 199 | 200 | # install(FILES 201 | # # myfile1 202 | # # myfile2 203 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 204 | # ) 205 | 206 | ############# 207 | ## Testing ## 208 | ############# 209 | 210 | ## Add gtest based cpp test target and link libraries 211 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_urdf_from_step.cpp) 212 | # if(TARGET ${PROJECT_NAME}-test) 213 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 214 | # endif() 215 | 216 | ## Add folders to be run by python nosetests 217 | # catkin_add_nosetests(test) 218 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2023, ReconCycle 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | jsi reconcycle 6 | 7 | # urdf_from_step 8 | 9 | 10 | 11 | ## About 12 | 13 | This is ROS package for automated conversion of STEP models to URDF format. The program takes as input the STEP file (left images) of the desired robot or robot-like maschine and creates a new ROS package. The package created contains the URDF description, the STL mesh files required by URDF description, and the ROS launch file to load the data into the ROS for visualization (center images) and control (right images). 14 | 15 | Simple robot arm: 16 | 17 | 21 | 22 | 23 | Cutter module from Reconcycle project: 24 | 25 | 29 | 30 | 31 | ### Working principle 32 | 33 | The URDF file is generated in the following steps. First, the STEP file is loaded and its contents are analyzed using tools from the Open Cascade Technology (OCCT) library [1]. The analysis looks for keywords such as "joint" and "link" in the part names or in the assembly names in the model design tree. The instances with these keywords in their names represent the corresponding "joint" and "link" building blocks of URDF. The remaining part names containing the keyword encode the connections between individual URDF elements and their names in the URDF file. Once these instances and their connections have been identified, the correct local transformation between them must be computed from the values of their base coordinate systems in the STEP file. The calculated local transformations are transformed accordingly into the coordinate system values of the "joint" and "link" URDF definitions. The instances that do not have keywords in their names represent geometric shapes. They are transformed into the STL mesh specified in the appropriate local coordinate system according to the given URDF tree structure. From the collected and computed URDF data, the XML in URDF format is created using the urdfdom parser library. Finally, everything is stored in a newly created ROS package. 34 | 35 | 36 | ## Instalation 37 | 38 | Because of the challenging installation of the required python package [pythonOCC-core](https://github.com/tpaviot/pythonocc-core) is highly recommended to use a docker image. 39 | 40 | ### Docker 41 | 42 | Docker repository source is [hier](https://github.com/ReconCycle/urdf-from-step-docker). 43 | 44 | Builded docker image is [hier](https://github.com/ReconCycle/urdf-from-step-docker/pkgs/container/urdf-from-step). 45 | 46 | Pulling builded docker image: 47 | 48 | ```bash 49 | docker pull ghcr.io/reconcycle/urdf-from-step:latest 50 | ``` 51 | 52 | 53 | ## Usage 54 | 55 | 56 | The examples and manuals are provided [hier](https://github.com/ReconCycle/urdf-from-step-examples). 57 | 58 | Preparation of STEP file is on the example of a simple robot arm described [hier](https://github.com/ReconCycle/urdf-from-step-examples/tree/main/documentation/step_file_creation). 59 | 60 | The prepared step file is turned to the corresponding ROS package containing URDF like this: 61 | 62 | ```bash 63 | roslaunch urdf_from_step build_urdf_from_step.launch step_file_path:="/input_step_files/robot_arm.step" urdf_package_name:="robot_arm" 64 | 65 | ``` 66 | The created package needs to be added to the catkin workspace for building, sourcing, and launching: 67 | 68 | ```bash 69 | catkin build robot_arm 70 | cd catkin_ws 71 | source devel/setup.bash 72 | roslaunch robot_arm load_urdf.launch 73 | ``` 74 | 75 | 76 | More detailed instructions regarding conversion from STEP to URDF are provided in [hier](https://github.com/ReconCycle/urdf-from-step-examples/tree/main/documentation/step_to_urdf_conversion). 77 | 78 | Where also the instructions for URDF visualization are provided [hier](https://github.com/ReconCycle/urdf-from-step-examples/tree/main/documentation/visualization). 79 | 80 | ## References 81 | 82 | * [1] pythonocc: Thomas Paviot. (2022). pythonocc (7.7.0). Zenodo. https://doi.org/10.5281/zenodo.3605364 83 | 84 | 85 | ## Funding 86 | 87 | eu_flag 89 | 90 | This project has received funding from the European Union’s Horizon 2020 research and innovation programme under grant agreement No. 871352. 91 | -------------------------------------------------------------------------------- /documentation/figures/cad_cs_gilotina.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ReconCycle/urdf_from_step/ebe40d9e9d95c9b170d4ac1cee5ee47db625f10f/documentation/figures/cad_cs_gilotina.png -------------------------------------------------------------------------------- /documentation/figures/gilotina2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ReconCycle/urdf_from_step/ebe40d9e9d95c9b170d4ac1cee5ee47db625f10f/documentation/figures/gilotina2.png -------------------------------------------------------------------------------- /documentation/figures/robot_arm_cad_with_cs.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ReconCycle/urdf_from_step/ebe40d9e9d95c9b170d4ac1cee5ee47db625f10f/documentation/figures/robot_arm_cad_with_cs.PNG -------------------------------------------------------------------------------- /documentation/figures/rviz moved.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ReconCycle/urdf_from_step/ebe40d9e9d95c9b170d4ac1cee5ee47db625f10f/documentation/figures/rviz moved.png -------------------------------------------------------------------------------- /documentation/figures/rviz tf center.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ReconCycle/urdf_from_step/ebe40d9e9d95c9b170d4ac1cee5ee47db625f10f/documentation/figures/rviz tf center.png -------------------------------------------------------------------------------- /documentation/figures/rviz_tf_gilotina.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ReconCycle/urdf_from_step/ebe40d9e9d95c9b170d4ac1cee5ee47db625f10f/documentation/figures/rviz_tf_gilotina.png -------------------------------------------------------------------------------- /documentation/jsi-logo-1-150x150.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ReconCycle/urdf_from_step/ebe40d9e9d95c9b170d4ac1cee5ee47db625f10f/documentation/jsi-logo-1-150x150.png -------------------------------------------------------------------------------- /documentation/reconcycle-transparent.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ReconCycle/urdf_from_step/ebe40d9e9d95c9b170d4ac1cee5ee47db625f10f/documentation/reconcycle-transparent.png -------------------------------------------------------------------------------- /documentation/rosin_eu_flag.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ReconCycle/urdf_from_step/ebe40d9e9d95c9b170d4ac1cee5ee47db625f10f/documentation/rosin_eu_flag.jpg -------------------------------------------------------------------------------- /install_python_occ.sh: -------------------------------------------------------------------------------- 1 | 2 | git clone git://github.com/tpaviot/pythonocc-core.git pythonocc 3 | cd pythonocc 4 | mkdir build 5 | cd build 6 | cmake -DOCE_INCLUDE_PATH=/usr/include/opencascade -DOCE_LIB_PATH=/usr/lib/x86_64-linux-gnu .. 7 | make -------------------------------------------------------------------------------- /launch/build_urdf_from_step.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /launch/template.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | urdf_from_step 4 | 0.0.1 5 | The urdf_from_step package 6 | 7 | 8 | 9 | 10 | Rok Pahic 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | rospy 54 | rospy 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /scripts/create_urdf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | 4 | #OpenCascade 5 | from OCC.Core.gp import gp_Vec, gp_Quaternion, gp_Pnt , gp_Trsf, gp_Ax1 6 | from OCC.Core.TopoDS import TopoDS_Compound, TopoDS_Solid, TopoDS_Shell 7 | 8 | from OCC.Extend.DataExchange import write_stl_file 9 | from OCC.Core.BRepBuilderAPI import BRepBuilderAPI_Transform 10 | 11 | #ROS 12 | import rospy 13 | from tf.transformations import euler_from_quaternion, quaternion_from_euler 14 | from urdf_parser_py import urdf 15 | 16 | from catkin_pkg.package_templates import create_package_files, PackageTemplate 17 | import rospkg 18 | 19 | 20 | #Python 21 | import numpy as np 22 | import os 23 | import xml.etree.ElementTree as ET 24 | from shutil import rmtree 25 | 26 | import sys 27 | 28 | #Custom 29 | from import_asembly.Asembly_import import read_step_file_asembly 30 | 31 | 32 | 33 | def createXacroPropertyWithParameterizedNamespace(property_name, value, prefix_param_name): 34 | xacro_string = "" 35 | # 36 | return xacro_string 37 | 38 | 39 | 40 | def createXacroRunMacro(): 41 | 42 | 43 | xacro_string = """ 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | """ 56 | 57 | return xacro_string 58 | 59 | def changeXacroJointNames(xacro_string, joint_names , property_line = ""): 60 | 61 | link_i = 0 62 | base_link_string = "" 68 | xacro_string = insertToXacroLine(xacro_string, property_line, new_property) 69 | current_link_string = base_link_string + link_name + "\"" 70 | new_link_string = base_link_string + "${" + new_link_name + "}\"" 71 | xacro_string = xacro_string.replace(current_link_string , new_link_string) 72 | link_i = link_i + 1 73 | 74 | 75 | return xacro_string 76 | 77 | 78 | def changeXacroLinkNames(xacro_string, link_names , property_line = ""): 79 | 80 | # 81 | 82 | link_i = 0 83 | base_link_string = "" 93 | xacro_string = insertToXacroLine(xacro_string, property_line, new_property) 94 | 95 | 96 | current_link_string = base_link_string + link_name + "\"" 97 | new_link_string = base_link_string + "${" + new_link_name + "}\"" 98 | xacro_string = xacro_string.replace(current_link_string , new_link_string) 99 | 100 | #replace in joints 101 | 102 | current_link_string = link_string_as_parent + link_name + "\"" 103 | new_link_string = link_string_as_parent + "${" + new_link_name + "}\"" 104 | xacro_string = xacro_string.replace(current_link_string , new_link_string) 105 | 106 | current_link_string = link_string_as_child + link_name + "\"" 107 | new_link_string = link_string_as_child + "${" + new_link_name + "}\"" 108 | xacro_string = xacro_string.replace(current_link_string , new_link_string) 109 | 110 | link_i = link_i + 1 111 | 112 | 113 | return xacro_string 114 | 115 | def substituteInXacro(xacro_string, robot_joints,robot_links_names): 116 | 117 | xacro_head = "" 118 | xacro_macro_head = "" 119 | 120 | xacro_string = xacro_string.replace("", xacro_head ) 121 | 122 | xacro_string = insertToXacroLine(xacro_string, xacro_head,xacro_macro_head) 123 | 124 | xacro_string = insertToXacroLine(xacro_string,"","", before = True) 125 | 126 | prefix_property = "" 127 | xacro_string = insertToXacroLine(xacro_string, xacro_macro_head,prefix_property ) 128 | 129 | run_string = createXacroRunMacro() 130 | 131 | run_reference = "" 132 | 133 | xacro_string = insertToXacroLine(xacro_string,run_reference,run_string) 134 | 135 | xacro_string = changeXacroLinkNames(xacro_string, robot_links_names, prefix_property ) 136 | robot_joints_names = [] 137 | for joint in robot_joints: 138 | if joint["parent"]!="": 139 | robot_joints_names.append(joint["name"]) 140 | 141 | xacro_string = changeXacroJointNames(xacro_string, robot_joints_names, prefix_property ) 142 | 143 | 144 | return xacro_string 145 | 146 | def insertToXacroLine(xacro_string, reference_text, insert_text , before = False): 147 | 148 | base_lines = xacro_string.splitlines() 149 | insert_lines = insert_text.splitlines() 150 | 151 | insert_i = 0 152 | for i, line in enumerate(base_lines): 153 | 154 | if reference_text in line: 155 | insert_i = i 156 | break 157 | 158 | if before == False: 159 | insert_i = insert_i + 1 160 | 161 | 162 | new_list = base_lines[:insert_i] + insert_lines + base_lines[insert_i:] 163 | 164 | xacro_string = "\n".join(new_list) 165 | 166 | return xacro_string 167 | 168 | 169 | 170 | def changePos2M(segment_location): 171 | 172 | return [segment_location.TranslationPart().X() / 1000, segment_location.TranslationPart().Y() / 1000, segment_location.TranslationPart().Z() / 1000] 173 | 174 | def toEuler(segment_location): 175 | 176 | 177 | q = np.array([segment_location.GetRotation().X(), segment_location.GetRotation().Y(), segment_location.GetRotation().Z(), segment_location.GetRotation().W()]) 178 | return list(euler_from_quaternion(q)) 179 | 180 | 181 | def calculateTfToRoot(joint, joint_list): 182 | 183 | parent_name = joint['parent'] 184 | 185 | global_this_tf = joint['location'] 186 | local_tf = gp_Trsf()#copy.deepcopy(global_this_tf) 187 | found = False 188 | for other_joint in joint_list: 189 | 190 | if other_joint['child'] == parent_name: 191 | global_other_tf = other_joint['location'] 192 | local_tf = global_other_tf.Inverted().Multiplied(global_this_tf) 193 | found = True 194 | break 195 | 196 | if found == False: #is to root joint 197 | 198 | local_tf.SetTranslation(gp_Vec(0,0,0)) 199 | local_tf.SetRotation(gp_Quaternion(0,0,0,1)) 200 | 201 | #local_tf = global_this_tf 202 | 203 | return local_tf 204 | 205 | def findOneVersionOfString(string_word, versions): 206 | anser = -1 207 | 208 | for version in versions: 209 | anser = string_word.find(version) 210 | 211 | if anser != -1: 212 | return anser 213 | 214 | return anser 215 | 216 | 217 | def separateRobotPartsFromStep(parts_data): 218 | print("Searching trough step...") 219 | print("Prepared " + str(len(parts_data)) + " parts data") 220 | 221 | joints_id_names = ["joint_","JOINT_","Joint_"] 222 | connection_word_id_names = ["_to_","_TO_" ,"_To_"] 223 | urdf_id_names = ["urdf","URDF","Urdf"] 224 | 225 | avalibel_joint_types = ["fixed","revolute", "prismatic"] 226 | joint_types_id_names = {} 227 | joint_types_id_names["fixed"] = ["fixed","FIXED","Fixed"] 228 | joint_types_id_names["revolute"] = ["revolute","REVOLUTE","Revolute"] 229 | joint_types_id_names["prismatic"] = ["prismatic","PRISMATIC","Prismatic"] 230 | 231 | 232 | robot_joints =[] 233 | robot_parts = [] 234 | robot_links = [] 235 | 236 | 237 | 238 | 239 | root_link_name = None 240 | 241 | for part in parts_data: 242 | 243 | part_data = parts_data[part] 244 | #print(part_data) 245 | 246 | if len(part_data)==4: #type(part) == TopoDS_Solid or type(part) == TopoDS_Compound or type(part) == TopoDS_Shell: #check id solid or compound 247 | segment_name, segment_color, segment_hierarchy, segment_trans = part_data 248 | 249 | segment_name = segment_name.replace(" ", "-") 250 | # print("hierarchy:") 251 | # print(segment_hierarchy) 252 | # print(segment_name) 253 | 254 | segment_location = part.Location().Transformation() 255 | 256 | 257 | segment_position = changePos2M(segment_location) 258 | segment_q_orientation = [segment_location.GetRotation().X(), segment_location.GetRotation().Y(), segment_location.GetRotation().Z(), segment_location.GetRotation().W()] 259 | #print(segment_location) 260 | #Parse joints data 261 | if len(segment_hierarchy)>-1: #filter out joints, (unlocked hiarchi ? 1 -> -1 262 | urdf_detected = False 263 | h_i = 0 264 | for hiarchie_names in segment_hierarchy: 265 | if findOneVersionOfString(hiarchie_names,urdf_id_names) == 0: 266 | print("got to urdf" + str(h_i)) 267 | urdf_detected = True 268 | break 269 | 270 | h_i = h_i + 1 271 | 272 | if urdf_detected: # findOneVersionOfString(segment_hierarchy[1],urdf_id_names) == 0: #check for urdf 273 | 274 | joint_name = segment_hierarchy[h_i+1] 275 | 276 | if findOneVersionOfString(joint_name,joints_id_names) ==0: 277 | 278 | connection_name = joint_name[6:] 279 | connection_id_string = "_to_" 280 | 281 | ind = findOneVersionOfString(connection_name, connection_word_id_names) 282 | 283 | if ind == -1: #this is for base joint 284 | ind = connection_name.find("_") 285 | parent_name = connection_name[0:ind] 286 | root_link_name = parent_name 287 | child_name = parent_name #to enable moving everithing to this frame 288 | parent_name = "" #"root joint doesnt have parent" 289 | else: 290 | parent_name = connection_name[0:ind] 291 | connection_name = connection_name[len(parent_name)+len(connection_id_string):] #TODO this presumes all len(connection_id_string) is same 292 | ind = connection_name.find("_") 293 | child_name = connection_name[0:ind] 294 | 295 | 296 | 297 | joint_data = {} 298 | joint_data["name"] = parent_name + "_" + child_name 299 | 300 | for test_type in avalibel_joint_types: #iterate trough avalibel type and set correct one 301 | 302 | 303 | if findOneVersionOfString(segment_name,joint_types_id_names[test_type])==0: 304 | joint_data["type"] = test_type 305 | break 306 | 307 | joint_data["parent"] = parent_name 308 | joint_data["child"] = child_name 309 | joint_data["position"] = segment_position 310 | joint_data["rotation"] = segment_q_orientation 311 | joint_data["location"] = segment_location 312 | robot_joints.append(joint_data) 313 | 314 | #DEFINE LINKS 315 | #Test child links and add 316 | if not child_name in robot_links: 317 | robot_links.append(child_name) 318 | 319 | #Create links 320 | if parent_name !="": #not epty root mark 321 | if not parent_name in robot_links: 322 | robot_links.append(parent_name) 323 | 324 | 325 | 326 | continue 327 | else: 328 | 329 | print("PROBLEM: Not correct naming of joints in URDF asembly") 330 | print(joint_name) 331 | continue 332 | 333 | part_for_saving = False 334 | 335 | if type(part) == TopoDS_Solid:#== TopoDS_Compound : # 336 | 337 | if False: 338 | if not(segment_hierarchy[1] in robot_links) and not(segment_hierarchy[1] in robot_links_vis_data): #make list of links at top hiarchie 339 | #segments_data={segment_hierarchy[-1]:{segment_name:segment_location}} 340 | robot_links_vis_data.append(segment_hierarchy[1]) 341 | 342 | #zloži elemente v dictionary po hiarhiji 343 | if segment_hierarchy[-1] in segments_data: 344 | segments_data[segment_hierarchy[-1]].update({segment_name: part}) 345 | else: 346 | segments_data.update({segment_hierarchy[-1]:{segment_name:part}}) 347 | 348 | part_for_saving = True 349 | 350 | if type(part) == TopoDS_Shell: 351 | 352 | part_for_saving = True 353 | 354 | if part_for_saving: 355 | 356 | robot_part = {} 357 | 358 | segment_name = segment_name.replace("/", "_") #safty for names includinh / as path 359 | 360 | robot_part["name"] = segment_name 361 | robot_part["location"] = segment_location 362 | robot_part["hierarchy"] = segment_hierarchy 363 | robot_part["part"] = part 364 | robot_part["color"] = [segment_color.Red(),segment_color.Green(),segment_color.Blue()] 365 | robot_parts.append(robot_part) 366 | 367 | 368 | 369 | 370 | else: 371 | segment_name, segment_color = parts_data[part] 372 | 373 | 374 | 375 | return robot_parts, robot_joints, robot_links , root_link_name 376 | 377 | 378 | 379 | def createSTLs(robot_parts,meshes_path, mode="binary"): 380 | print("Preparing meshes...") 381 | 382 | 383 | #CREATE STLs 384 | 385 | #convert to stls 386 | 387 | stl_output_dir = meshes_path #package_path + "/meshes" 388 | output_files = [] 389 | file_names = [] 390 | 391 | test_count = 0 392 | max_parts = 2000 393 | #root_link_name ="Base" 394 | #print("len:"+str(len(robot_parts))) 395 | #print("robotParts:") 396 | 397 | for part in robot_parts: 398 | 399 | 400 | #print(part) 401 | #file_name = part['hierarchy'][-1]+"-"+part["name"]+".stl" 402 | if test_count>max_parts: 403 | print("to many parts, set higher limit!") 404 | break 405 | test_count = test_count + 1 406 | 407 | 408 | made_name = part["name"] 409 | #for h_name in part['hierarchy']: 410 | #made_name = h_name + made_name 411 | name_counter = 0 412 | file_name = made_name + str(name_counter) 413 | 414 | while file_name in file_names: 415 | 416 | name_counter = name_counter + 1 417 | file_name = made_name + str(name_counter) 418 | 419 | 420 | file_names.append(file_name) 421 | 422 | file_name = file_name + ".stl" 423 | 424 | 425 | output_file = os.path.join(stl_output_dir,file_name) 426 | #stl_writer = StlAPI_Writer() 427 | #stl_writer.SetASCIIMode(True) 428 | #mesh = BRepMesh_IncrementalMesh( part["part"], 0.1, False, 0.1, True 429 | #) 430 | 431 | 432 | #mesh = BRepMesh_IncrementalMesh(my_box, 0.1) 433 | #mesh.Perform() 434 | trfs = gp_Trsf() 435 | trfs.SetScale(gp_Pnt(),0.001) 436 | scaled_part = BRepBuilderAPI_Transform(part['part'], trfs).Shape() 437 | 438 | print("output file: " + output_file) 439 | write_stl_file(scaled_part, output_file, mode=mode) 440 | output_files.append(file_name) 441 | 442 | return output_files 443 | 444 | def createMaterialsAndColors(robot_parts, robot_links, meshes_paths, root_link_name): 445 | print("Creating materials...") 446 | colors_values = [] 447 | colors_names = [] 448 | color_counter = 0 449 | materials = [] 450 | 451 | link_meshes = {} 452 | 453 | for name in robot_links: #preapre empty matrixes for link meshes 454 | 455 | link_meshes[name] = [] 456 | 457 | mesh_i = 0 458 | 459 | for part in robot_parts: 460 | #print(mesh_i) 461 | 462 | #PREPARE new color 463 | 464 | if part["color"] in colors_values: 465 | color_name = colors_names[colors_values.index(part["color"])] 466 | 467 | 468 | else: 469 | colors_values.append(part["color"]) 470 | 471 | color_name = "color"+str(color_counter) 472 | colors_names.append(color_name) 473 | materials.append(urdf.Material(name = color_name, color = urdf.Color(part["color"]+[1]) )) 474 | color_counter = color_counter + 1 475 | 476 | part["material_name"] = color_name 477 | 478 | #FIND_LINK_name: 479 | current_name = part["name"] 480 | print("link name: " + current_name) 481 | 482 | if "link_" in current_name: #search for link definition in part name 483 | current_name = current_name[5:] 484 | current_name = current_name[0:current_name.find("_")] 485 | else: #search for link defition in hiearchy 486 | for parent_name in part["hierarchy"]: 487 | if "link_" in parent_name: 488 | current_name = parent_name[5:] 489 | current_name = current_name[0:current_name.find("_")] 490 | break 491 | else: 492 | current_name = root_link_name 493 | #print(current_name) 494 | if current_name in robot_links: 495 | file_name = meshes_paths[mesh_i] 496 | link_meshes[current_name].append({"mesh_name":file_name,"mesh_material":color_name,"color_value":part["color"]}) 497 | 498 | else: 499 | print("error: no link name: " + current_name) 500 | 501 | mesh_i = mesh_i + 1 502 | 503 | return robot_parts, link_meshes 504 | 505 | 506 | 507 | def generateURDF(robot_joints,robot_links, link_meshes, root_link_name, package_name): 508 | 509 | #PREPARE TRANSFORM MATRIX 510 | 511 | for joint in robot_joints: 512 | 513 | tf = calculateTfToRoot(joint, robot_joints) 514 | 515 | joint["local_tf"] = tf 516 | 517 | pass 518 | 519 | 520 | 521 | # ROBOT META DATA 522 | 523 | robot=urdf.URDF() 524 | #robot.materials = materials 525 | 526 | robot.name='fifi' 527 | 528 | robot.version='1.0' 529 | 530 | robot.gazebos = ['control'] 531 | 532 | #CREATE ROBOT JOINTS 533 | 534 | for joint in robot_joints: 535 | 536 | if joint["parent"]!="":#if root link joint dont add it to the joints 537 | joint_limit=urdf.JointLimit(effort=1000, lower=-1.548, upper=1.548, velocity=0.5) 538 | or_j = toEuler(joint["local_tf"]) 539 | pos_j = changePos2M(joint["local_tf"]) 540 | 541 | #pos_j = joint["position"] 542 | Pos1 = urdf.Pose(xyz=pos_j, rpy=or_j)#'zyx' 543 | #Pos1 = Pose(xyz=[0,0,0], rpy=[0,0,0,])#'zyx' 544 | new_joint = urdf.Joint( name= joint["name"], parent=joint["parent"], child=joint["child"], joint_type=joint["type"], 545 | axis=[1, 0, 0], origin=Pos1, 546 | limit=joint_limit, dynamics=None, safety_controller=None, 547 | calibration=None, mimic=None) 548 | 549 | 550 | 551 | robot.add_joint(new_joint) 552 | else: 553 | root_location_in_step = joint["location"] 554 | 555 | 556 | 557 | #robot.add_material(Mat1) 558 | 559 | #CREATE ROBOT LINKS 560 | relative_mesh_path = "meshes/" 561 | stl_urdf_root ="package://"+package_name+"/" 562 | 563 | 564 | for link_name in robot_links: 565 | 566 | #if link_name != root_link_name: #this is overjuped because we add it leater 567 | #search for coresponding joint 568 | urdf_link = urdf.Link(name = link_name, visual = None, inertial = None, collision = None)#, origin = link_pose 569 | 570 | #add visuals 571 | 572 | for mesh in link_meshes[link_name]: 573 | meshpath = stl_urdf_root + relative_mesh_path + mesh["mesh_name"] 574 | 575 | #ADD MATERIALS 576 | Mat1 = urdf.Material(name = mesh["mesh_material"], color = urdf.Color(mesh["color_value"]+[1])) 577 | #if mesh["mesh_material"] in already_defined_colors: #check if you have already somwhere defined this color if not define it 578 | #Mat1 = Material(name = mesh["mesh_material"] ) 579 | #else: 580 | #already_defined_colors.append(mesh["mesh_material"]) 581 | #Mat1 = Material(name = mesh["mesh_material"], color = urdf.Color(mesh["color_value"]+[1])) 582 | 583 | #translation = changePos2M(tf) 584 | if link_name == root_link_name: 585 | mesh_location = root_location_in_step 586 | else: 587 | for joint in robot_joints: 588 | if link_name == joint["child"]: 589 | mesh_location = joint["location"] 590 | break 591 | 592 | translation = changePos2M(mesh_location.Inverted())# [0,0,0] 593 | #or_part = toEuler(tf) 594 | or_part = toEuler(mesh_location.Inverted())#0,0,0] 595 | 596 | Mesh_to_joint_pose = urdf.Pose(xyz=translation,rpy=or_part)#'zyx' 597 | 598 | 599 | Vis1 = urdf.Visual(geometry=urdf.Mesh(filename= meshpath), material=Mat1, origin=Mesh_to_joint_pose, name=None) 600 | 601 | #Vis1 = Visual(geometry=Cylinder(radius=0.005, length=0.5), material=None, origin=Pos1, name=None) 602 | urdf_link.add_aggregate('visual', Vis1) 603 | 604 | 605 | robot.add_link(urdf_link) 606 | 607 | 608 | return robot 609 | 610 | 611 | def createPackageROS(package_name,output_folder_path): 612 | 613 | package_description = "This is automaticly created ROS package with URDF" 614 | 615 | package_template = PackageTemplate._create_package_template( 616 | package_name=package_name, 617 | description=package_description, 618 | licenses=["BSD"] or [], 619 | maintainer_names=["urfd_from_step_package"], 620 | author_names=["urfd_from_step_package"], 621 | version="1.0.0", 622 | catkin_deps=["rviz"]) 623 | 624 | package_path = output_folder_path + "/"+ package_name 625 | 626 | create_package_files(target_path=package_path, 627 | package_template=package_template, 628 | rosdistro="noetic", 629 | #newfiles={"launch/start.launch":"test"} 630 | )#,"urdf/test.urdf":"test2" 631 | 632 | 633 | def setupLaunchXml(new_package_name): 634 | 635 | 636 | 637 | # importing element tree 638 | # under the alias of ET 639 | 640 | rospack = rospkg.RosPack() 641 | template_path = rospack.get_path('urdf_from_step') + "/launch/template.launch" 642 | 643 | print(template_path) 644 | 645 | # Passing the path of the 646 | # xml document to enable the 647 | # parsing process 648 | tree = ET.parse(template_path) 649 | 650 | # getting the parent tag of 651 | # the xml document 652 | root = tree.getroot() 653 | 654 | # go to atributes 655 | 656 | 657 | for child in root: 658 | 659 | try: 660 | if child.attrib['name'] == "tf_prefix": 661 | child.attrib['default'] = new_package_name 662 | if child.attrib['name'] == "urdf_name": 663 | child.attrib['default'] = new_package_name 664 | if child.attrib['name'] == "urdf_path": 665 | child.attrib['default'] = "$(find "+ new_package_name +")/urdf/$(arg urdf_name).urdf" 666 | except: 667 | pass 668 | 669 | for child in root: 670 | 671 | print(child.attrib) 672 | 673 | xml_string = ET.tostring(root) 674 | 675 | return xml_string 676 | 677 | def changeCmake(input_string): 678 | 679 | 680 | old_text = "# install(FILES\n# # myfile1\n# # myfile2\n# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n# )" 681 | new_text = "install(DIRECTORY\n launch/\n DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n FILES_MATCHING PATTERN \"*.launch\" \n )" 682 | 683 | 684 | 685 | 686 | output_string = input_string.replace(old_text, new_text) 687 | 688 | return output_string 689 | 690 | 691 | 692 | if __name__ == '__main__': 693 | 694 | 695 | 696 | rospy.init_node('URDF_creator') 697 | step_file_path = rospy.get_param('~step_file_path', '/input_step_files/robot_arm.step') 698 | output_folder_path = rospy.get_param('~output_folder_path', '/output_ros_urdf_packages') 699 | package_name = rospy.get_param('~urdf_package_name', 'test_package') 700 | package_path = "/output_ros_urdf_packages/" + package_name 701 | 702 | rospy.loginfo("Creating ROS package:") 703 | rospy.loginfo(package_name) 704 | 705 | #delete existing package 706 | if os.path.exists(package_path) == True: 707 | rospy.loginfo("Package alread exist! Deliting:" + package_name) 708 | rmtree(package_path) 709 | 710 | 711 | createPackageROS(package_name,output_folder_path) 712 | 713 | 714 | rospy.loginfo("Creating URDF from STEP file:") 715 | rospy.loginfo(step_file_path) 716 | 717 | 718 | 719 | #CHANGE CMAKE 720 | 721 | cmake_path = package_path +"/CMakeLists.txt" 722 | 723 | cmake_file_handle = open(cmake_path,"r+") 724 | 725 | text = cmake_file_handle.read() 726 | 727 | changed_text = changeCmake(text) 728 | 729 | cmake_file_handle.write(changed_text) 730 | cmake_file_handle.close() 731 | rospy.loginfo("READ STEP FILE ASEMBLY...") 732 | parts_data = read_step_file_asembly(step_file_path) 733 | 734 | rospy.loginfo("SEPARATE ROBOT PARTS...") 735 | robot_parts, robot_joints, robot_links, root_link_name = separateRobotPartsFromStep(parts_data) 736 | print("JOINTS:") 737 | for joint in robot_joints: 738 | print(joint) 739 | 740 | #sys.exit() 741 | 742 | meshes_path = package_path +"/meshes" 743 | if os.path.exists(meshes_path) == False: 744 | os.mkdir(meshes_path) 745 | 746 | 747 | mesh_paths = createSTLs(robot_parts, meshes_path) 748 | 749 | 750 | robot_parts, link_meshes = createMaterialsAndColors(robot_parts, robot_links, mesh_paths, root_link_name) 751 | 752 | 753 | robot = generateURDF(robot_joints,robot_links, link_meshes, root_link_name, package_name) 754 | #print(robot) 755 | 756 | if os.path.exists(package_path +"/urdf") == False: 757 | os.mkdir(package_path +"/urdf") 758 | 759 | urdf_path = package_path +"/urdf/"+package_name+".urdf" 760 | 761 | file_handle = open(urdf_path,"w") 762 | 763 | file_handle.write(robot.to_xml_string()) 764 | file_handle.close() 765 | 766 | export_xacro = True 767 | if export_xacro: 768 | xacro_string = robot.to_xml_string() 769 | 770 | xacro_string = substituteInXacro(xacro_string, robot_joints,robot_links) 771 | #print(xacro_string) 772 | 773 | xacro_path = package_path +"/urdf/"+package_name+".urdf.xacro" 774 | file_handle = open(xacro_path,"w") 775 | file_handle.write(xacro_string) 776 | file_handle.close() 777 | 778 | #COPY LAUNCH TEMPLATE 779 | print("Prepare launch file..") 780 | new_package_name = package_name 781 | xml_string = setupLaunchXml(new_package_name) 782 | 783 | 784 | if os.path.exists(package_path +"/launch") == False: 785 | os.mkdir(package_path +"/launch") 786 | 787 | launch_path = package_path +"/launch/load_urdf.launch" 788 | 789 | launch_file_handle = open(launch_path,"wb") 790 | launch_file_handle.write(xml_string) 791 | launch_file_handle.close() 792 | 793 | 794 | 795 | 796 | print("Conversion finished.") 797 | 798 | rospy.signal_shutdown("Finish") 799 | 800 | 801 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 4 | 5 | from setuptools import setup 6 | from catkin_pkg.python_setup import generate_distutils_setup 7 | 8 | # fetch values from package.xml 9 | setup_args = generate_distutils_setup( 10 | packages=['import_asembly'], 11 | package_dir={'': 'src'}) 12 | 13 | setup(**setup_args) -------------------------------------------------------------------------------- /src/import_asembly/Asembly_import.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | 5 | from OCC.Core.TopoDS import TopoDS_Shape 6 | from OCC.Core.BRepMesh import BRepMesh_IncrementalMesh 7 | from OCC.Core.StlAPI import stlapi_Read, StlAPI_Writer 8 | from OCC.Core.BRep import BRep_Builder 9 | from OCC.Core.gp import gp_Pnt, gp_Dir, gp_Pnt2d 10 | from OCC.Core.Bnd import Bnd_Box2d 11 | from OCC.Core.TopoDS import TopoDS_Compound 12 | from OCC.Core.IGESControl import IGESControl_Reader, IGESControl_Writer 13 | from OCC.Core.STEPControl import STEPControl_Reader, STEPControl_Writer, STEPControl_AsIs 14 | from OCC.Core.Interface import Interface_Static_SetCVal 15 | from OCC.Core.IFSelect import IFSelect_RetDone, IFSelect_ItemsByEntity 16 | from OCC.Core.TDocStd import TDocStd_Document 17 | 18 | #OLD version 19 | #from OCC.Core.XCAFDoc import (XCAFDoc_DocumentTool_ShapeTool, XCAFDoc_DocumentTool_ColorTool) 20 | 21 | from OCC.Core.XCAFDoc import ( 22 | XCAFDoc_DocumentTool, 23 | XCAFDoc_ColorTool, 24 | ) 25 | 26 | from OCC.Core.STEPCAFControl import STEPCAFControl_Reader 27 | from OCC.Core.TDF import TDF_LabelSequence, TDF_Label 28 | from OCC.Core.TCollection import TCollection_ExtendedString 29 | from OCC.Core.Quantity import Quantity_Color, Quantity_TOC_RGB 30 | from OCC.Core.TopLoc import TopLoc_Location 31 | from OCC.Core.BRepBuilderAPI import BRepBuilderAPI_Transform 32 | 33 | from OCC.Extend.TopologyUtils import (discretize_edge, get_sorted_hlr_edges, 34 | list_of_shapes_to_compound) 35 | 36 | 37 | 38 | #from OCC.Extend.DataExchange import read_step_file_with_names_colors 39 | 40 | try: 41 | import svgwrite 42 | HAVE_SVGWRITE = True 43 | except ImportError: 44 | HAVE_SVGWRITE = False 45 | 46 | import rospy 47 | 48 | def read_step_file_asembly(filename): #(from OCC.Extend.DataExchange import read_step_file_with_names_colors) 49 | 50 | """ Returns list of tuples (topods_shape, label, color) 51 | Use OCAF. 52 | """ 53 | enable_print = False 54 | if not os.path.isfile(filename): 55 | raise FileNotFoundError("%s not found." % filename) 56 | 57 | # the list: 58 | output_shapes = {} 59 | # create an handle to a document 60 | 61 | #doc = TDocStd_Document(TCollection_ExtendedString("pythonocc-doc")) 62 | doc = TDocStd_Document("pythonocc-doc-step-import") 63 | 64 | rospy.loginfo("tools") 65 | # Get root assembly 66 | shape_tool = XCAFDoc_DocumentTool.ShapeTool(doc.Main()) 67 | color_tool = XCAFDoc_DocumentTool.ColorTool(doc.Main()) 68 | #layer_tool = XCAFDoc_DocumentTool_LayerTool(doc.Main()) 69 | #mat_tool = XCAFDoc_DocumentTool_MaterialTool(doc.Main()) 70 | # rospy.loginfo("test43") 71 | step_reader = STEPCAFControl_Reader() 72 | step_reader.SetColorMode(True) 73 | step_reader.SetLayerMode(True) 74 | step_reader.SetNameMode(True) 75 | step_reader.SetMatMode(True) 76 | step_reader.SetGDTMode(True) 77 | 78 | status = step_reader.ReadFile(filename) 79 | #rospy.loginfo("test432") 80 | if status == IFSelect_RetDone: 81 | step_reader.Transfer(doc) 82 | 83 | locs = [] 84 | hiarchy = [] 85 | 86 | def _get_sub_shapes(lab, loc): 87 | # global cnt, lvl 88 | # cnt += 1 89 | # print("\n[%d] level %d, handling LABEL %s\n" % (cnt, lvl, _get_label_name(lab))) 90 | # print() 91 | # print(lab.DumpToString()) 92 | # print() 93 | # print("Is Assembly :", shape_tool.IsAssembly(lab)) 94 | # print("Is Free :", shape_tool.IsFree(lab)) 95 | # print("Is Shape :", shape_tool.IsShape(lab)) 96 | # print("Is Compound :", shape_tool.IsCompound(lab)) 97 | # print("Is Component :", shape_tool.IsComponent(lab)) 98 | # print("Is SimpleShape :", shape_tool.IsSimpleShape(lab)) 99 | # print("Is Reference :", shape_tool.IsReference(lab)) 100 | 101 | # users = TDF_LabelSequence() 102 | # users_cnt = shape_tool.GetUsers(lab, users) 103 | # print("Nr Users :", users_cnt) 104 | 105 | l_subss = TDF_LabelSequence() 106 | shape_tool.GetSubShapes(lab, l_subss) 107 | # print("Nb subshapes :", l_subss.Length()) 108 | l_comps = TDF_LabelSequence() 109 | shape_tool.GetComponents(lab, l_comps) 110 | # print("Nb components :", l_comps.Length()) 111 | # print() 112 | name = lab.GetLabelName() 113 | print("Name :", name) 114 | 115 | if shape_tool.IsAssembly(lab): 116 | l_c = TDF_LabelSequence() 117 | shape_tool.GetComponents(lab, l_c) 118 | for i in range(l_c.Length()): 119 | label = l_c.Value(i + 1) 120 | if shape_tool.IsReference(label): 121 | # print("\n######## reference label :", label) 122 | label_reference = TDF_Label() 123 | shape_tool.GetReferredShape(label, label_reference) 124 | loc = shape_tool.GetLocation(label) 125 | # print(" loc :", loc) 126 | # trans = loc.Transformation() 127 | # print(" tran form :", trans.Form()) 128 | # rot = trans.GetRotation() 129 | # print(" rotation :", rot) 130 | # print(" X :", rot.X()) 131 | # print(" Y :", rot.Y()) 132 | # print(" Z :", rot.Z()) 133 | # print(" W :", rot.W()) 134 | # tran = trans.TranslationPart() 135 | # print(" translation :", tran) 136 | # print(" X :", tran.X()) 137 | # print(" Y :", tran.Y()) 138 | # print(" Z :", tran.Z()) 139 | 140 | locs.append(loc) 141 | hiarchy.append(name) 142 | # print(">>>>") 143 | # lvl += 1 144 | _get_sub_shapes(label_reference, loc) 145 | # lvl -= 1 146 | # print("<<<<") 147 | hiarchy.pop() 148 | locs.pop() 149 | 150 | elif shape_tool.IsSimpleShape(lab): 151 | # print("\n######## simpleshape label :", lab) 152 | shape = shape_tool.GetShape(lab) 153 | # print(" all ass locs :", locs) 154 | 155 | loc = TopLoc_Location() 156 | for l in locs: 157 | # print(" take loc :", l) 158 | loc = loc.Multiplied(l) 159 | 160 | # trans = loc.Transformation() 161 | # print(" FINAL loc :") 162 | # print(" tran form :", trans.Form()) 163 | # rot = trans.GetRotation() 164 | # print(" rotation :", rot) 165 | # print(" X :", rot.X()) 166 | # print(" Y :", rot.Y()) 167 | # print(" Z :", rot.Z()) 168 | # print(" W :", rot.W()) 169 | # tran = trans.TranslationPart() 170 | # print(" translation :", tran) 171 | # print(" X :", tran.X()) 172 | # print(" Y :", tran.Y()) 173 | # print(" Z :", tran.Z()) 174 | c = Quantity_Color(0.5, 0.5, 0.5, Quantity_TOC_RGB) # default color 175 | color_set = False 176 | if ( 177 | color_tool.GetInstanceColor(shape, 0, c) 178 | or color_tool.GetInstanceColor(shape, 1, c) 179 | or color_tool.GetInstanceColor(shape, 2, c) 180 | ): 181 | color_tool.SetInstanceColor(shape, 0, c) 182 | color_tool.SetInstanceColor(shape, 1, c) 183 | color_tool.SetInstanceColor(shape, 2, c) 184 | color_set = True 185 | n = c.Name(c.Red(), c.Green(), c.Blue()) 186 | # print( 187 | # " instance color Name & RGB: ", 188 | # c, 189 | # n, 190 | # c.Red(), 191 | # c.Green(), 192 | # c.Blue(), 193 | # ) 194 | 195 | if not color_set: 196 | if ( 197 | XCAFDoc_ColorTool.GetColor(lab, 0, c) 198 | or XCAFDoc_ColorTool.GetColor(lab, 1, c) 199 | or XCAFDoc_ColorTool.GetColor(lab, 2, c) 200 | ): 201 | color_tool.SetInstanceColor(shape, 0, c) 202 | color_tool.SetInstanceColor(shape, 1, c) 203 | color_tool.SetInstanceColor(shape, 2, c) 204 | 205 | n = c.Name(c.Red(), c.Green(), c.Blue()) 206 | # print( 207 | # " shape color Name & RGB: ", 208 | # c, 209 | # n, 210 | # c.Red(), 211 | # c.Green(), 212 | # c.Blue(), 213 | # ) 214 | 215 | shape_disp = BRepBuilderAPI_Transform(shape, loc.Transformation()).Shape() 216 | if shape_disp not in output_shapes: 217 | output_shapes[shape_disp] = [lab.GetLabelName(), c, hiarchy.copy(),locs.copy()] 218 | for i in range(l_subss.Length()): 219 | lab_subs = l_subss.Value(i + 1) 220 | # print("\n######## simpleshape subshape label :", lab) 221 | shape_sub = shape_tool.GetShape(lab_subs) 222 | 223 | c = Quantity_Color(0.5, 0.5, 0.5, Quantity_TOC_RGB) # default color 224 | color_set = False 225 | if ( 226 | color_tool.GetInstanceColor(shape_sub, 0, c) 227 | or color_tool.GetInstanceColor(shape_sub, 1, c) 228 | or color_tool.GetInstanceColor(shape_sub, 2, c) 229 | ): 230 | color_tool.SetInstanceColor(shape_sub, 0, c) 231 | color_tool.SetInstanceColor(shape_sub, 1, c) 232 | color_tool.SetInstanceColor(shape_sub, 2, c) 233 | color_set = True 234 | n = c.Name(c.Red(), c.Green(), c.Blue()) 235 | # print( 236 | # " instance color Name & RGB: ", 237 | # c, 238 | # n, 239 | # c.Red(), 240 | # c.Green(), 241 | # c.Blue(), 242 | # ) 243 | 244 | if not color_set: 245 | if ( 246 | XCAFDoc_ColorTool.GetColor(lab_subs, 0, c) 247 | or XCAFDoc_ColorTool.GetColor(lab_subs, 1, c) 248 | or XCAFDoc_ColorTool.GetColor(lab_subs, 2, c) 249 | ): 250 | color_tool.SetInstanceColor(shape, 0, c) 251 | color_tool.SetInstanceColor(shape, 1, c) 252 | color_tool.SetInstanceColor(shape, 2, c) 253 | 254 | n = c.Name(c.Red(), c.Green(), c.Blue()) 255 | # print( 256 | # " shape color Name & RGB: ", 257 | # c, 258 | # n, 259 | # c.Red(), 260 | # c.Green(), 261 | # c.Blue(), 262 | # ) 263 | shape_to_disp = BRepBuilderAPI_Transform( 264 | shape_sub, loc.Transformation() 265 | ).Shape() 266 | # position the subshape to display 267 | if shape_to_disp not in output_shapes: 268 | output_shapes[shape_to_disp] = [lab_subs.GetLabelName(), c] 269 | def _get_shapes(): 270 | labels = TDF_LabelSequence() 271 | shape_tool.GetFreeShapes(labels) 272 | #global cnt 273 | #cnt += 1 274 | 275 | print() 276 | print("Number of shapes at root :", labels.Length()) 277 | print() 278 | for i in range(labels.Length()): 279 | root_item = labels.Value(i+1) 280 | _get_sub_shapes(root_item, None) 281 | _get_shapes() 282 | return output_shapes 283 | 284 | -------------------------------------------------------------------------------- /src/import_asembly/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ReconCycle/urdf_from_step/ebe40d9e9d95c9b170d4ac1cee5ee47db625f10f/src/import_asembly/__init__.py --------------------------------------------------------------------------------