├── .gitignore
├── collada_parser
├── collada_parser_plugin_description.xml
├── mainpage.dox
├── package.xml
├── include
│ └── collada_parser
│ │ ├── collada_parser.h
│ │ └── collada_parser_plugin.h
├── src
│ └── collada_parser_plugin.cpp
├── CMakeLists.txt
└── CHANGELOG.rst
├── README.md
└── collada_urdf
├── package.xml
├── test
└── test_collada_urdf.cpp
├── include
└── collada_urdf
│ └── collada_urdf.h
├── src
├── urdf_to_collada.cpp
├── collada_to_urdf.cpp
└── collada_urdf.cpp
├── CMakeLists.txt
└── CHANGELOG.rst
/.gitignore:
--------------------------------------------------------------------------------
1 | .*.sw?
2 |
--------------------------------------------------------------------------------
/collada_parser/collada_parser_plugin_description.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | Parse models as URDF from Collada files.
5 |
6 |
7 |
8 |
--------------------------------------------------------------------------------
/collada_parser/mainpage.dox:
--------------------------------------------------------------------------------
1 | /**
2 | \mainpage
3 | \htmlinclude manifest.html
4 |
5 | \b collada_parser is ...
6 |
7 |
10 |
11 |
12 | \section codeapi Code API
13 |
14 |
24 |
25 |
26 | */
27 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Collada URDF
2 |
3 | This contains packages for converting from collada files to URDF.
4 | See the ROS wiki for API documentation and tutorials.
5 |
6 | * [`collada_urdf`](http://wiki.ros.org/collada_urdf)
7 | * [`collada_parser`](http://wiki.ros.org/collada_parser)
8 |
9 | This was originally part of the [`ros/robot_model`](https://github.com/ros/robot_model) repository.
10 | It has been moved to this repo as described by [`ros/robot_model#195`](https://github.com/ros/robot_model/issues/195)
11 |
12 | ## Linking issue with Raspbian
13 |
14 | If you are using a Raspberry Pi with Raspbian you might encounter a linking problem with Assimp that looks like:
15 | ```bash
16 | ~/ros_catkin_ws/devel_isolated/collada_urdf/lib/libcollada_urdf.so: undefined reference to `typeinfo for Assimp::IOSystem'
17 | collect2: error: ld returned 1 exit status
18 | ```
19 |
20 | A work-around consists of compiling and installing Assimp latest version:
21 | ```bash
22 | mkdir -p $HOME/libraries/assimp-3.3.1/build_release
23 | cd $HOME/libraries/assimp-3.3.1/
24 | wget https://github.com/assimp/assimp/archive/v3.3.1.zip
25 | unzip v3.3.1.zip
26 | rm v3.3.1.zip
27 | mv v3.3.1 src
28 | cd build_release
29 | cmake ../src -DCMAKE_BUILD_TYPE=Release -DASSIMP_BUILD_TESTS=False
30 | make -j3
31 |
32 | sudo make -j3 install
33 | ```
34 |
35 | Make sure you reconfigure the catkin workspace (or just delete the build files) so that the newest Assimp version is found before compiling again.
36 |
37 |
--------------------------------------------------------------------------------
/collada_parser/package.xml:
--------------------------------------------------------------------------------
1 |
2 | collada_parser
3 | 1.12.13
4 |
5 | This package contains a C++ parser for the Collada robot
6 | description format. The parser reads a Collada XML robot
7 | description, and creates a C++ URDF model. Although it is possible
8 | to directly use this parser when working with Collada robot
9 | descriptions, the preferred user API is found in the urdf package.
10 |
11 |
12 | Rosen Diankov
13 | Kei Okada
14 | Ioan Sucan
15 | Jackie Kay
16 | Chris Lalancette
17 | Shane Loretz
18 |
19 | BSD
20 |
21 | http://ros.org/wiki/collada_parser
22 | https://github.com/ros/collada_urdf
23 | https://github.com/ros/collada_urdf/issues
24 |
25 | catkin
26 |
27 | urdf
28 | urdf_parser_plugin
29 |
30 | class_loader
31 | collada-dom
32 | liburdfdom-headers-dev
33 | rosconsole
34 | urdf
35 | urdf_parser_plugin
36 |
37 | class_loader
38 | collada-dom
39 | rosconsole
40 | urdf_parser_plugin
41 |
42 |
43 |
44 |
45 |
46 |
47 |
48 |
--------------------------------------------------------------------------------
/collada_urdf/package.xml:
--------------------------------------------------------------------------------
1 |
2 | collada_urdf
3 | 1.12.13
4 |
5 | This package contains a tool to convert Unified Robot Description Format (URDF) documents into COLLAborative Design Activity (COLLADA) documents.
6 |
7 | Implements robot-specific COLLADA extensions as defined by
8 | http://openrave.programmingvision.com/index.php/Started:COLLADA
9 |
10 |
11 | Tim Field
12 | Rosen Diankov
13 | Ioan Sucan
14 | Jackie Kay
15 | Chris Lalancette
16 | Shane Loretz
17 |
18 | BSD
19 |
20 | http://ros.org/wiki/collada_urdf
21 | https://github.com/ros/collada_urdf
22 | https://github.com/ros/collada_urdf/issues
23 |
24 | catkin
25 |
26 | liburdfdom-headers-dev
27 |
28 | angles
29 | assimp-dev
30 | cmake_modules
31 | collada-dom
32 | collada_parser
33 | eigen
34 | geometric_shapes
35 | liburdfdom-dev
36 | liburdfdom-headers-dev
37 | resource_retriever
38 | rosconsole
39 | urdf
40 |
41 | assimp
42 | collada-dom
43 | collada_parser
44 | geometric_shapes
45 | liburdfdom-dev
46 | resource_retriever
47 | rosconsole
48 | urdf
49 |
50 |
51 |
--------------------------------------------------------------------------------
/collada_parser/include/collada_parser/collada_parser.h:
--------------------------------------------------------------------------------
1 | /*********************************************************************
2 | * Software License Agreement (BSD License)
3 | *
4 | * Copyright (c) 2008, Willow Garage, Inc.
5 | * All rights reserved.
6 | *
7 | * Redistribution and use in source and binary forms, with or without
8 | * modification, are permitted provided that the following conditions
9 | * are met:
10 | *
11 | * * Redistributions of source code must retain the above copyright
12 | * notice, this list of conditions and the following disclaimer.
13 | * * Redistributions in binary form must reproduce the above
14 | * copyright notice, this list of conditions and the following
15 | * disclaimer in the documentation and/or other materials provided
16 | * with the distribution.
17 | * * Neither the name of the Willow Garage nor the names of its
18 | * contributors may be used to endorse or promote products derived
19 | * from this software without specific prior written permission.
20 | *
21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 | * POSSIBILITY OF SUCH DAMAGE.
33 | *********************************************************************/
34 |
35 | /* Author: Wim Meeussen */
36 |
37 | #ifndef COLLADA_PARSER_COLLADA_PARSER_H
38 | #define COLLADA_PARSER_COLLADA_PARSER_H
39 |
40 | #include
41 |
42 | #include
43 |
44 | namespace urdf {
45 |
46 | /// \brief Load Model from string
47 | urdf::ModelInterfaceSharedPtr parseCollada(const std::string& xml_string);
48 |
49 | }
50 |
51 | #endif
52 |
--------------------------------------------------------------------------------
/collada_parser/src/collada_parser_plugin.cpp:
--------------------------------------------------------------------------------
1 | /*********************************************************************
2 | * Software License Agreement (BSD License)
3 | *
4 | * Copyright (c) 2013, Willow Garage, Inc.
5 | * All rights reserved.
6 | *
7 | * Redistribution and use in source and binary forms, with or without
8 | * modification, are permitted provided that the following conditions
9 | * are met:
10 | *
11 | * * Redistributions of source code must retain the above copyright
12 | * notice, this list of conditions and the following disclaimer.
13 | * * Redistributions in binary form must reproduce the above
14 | * copyright notice, this list of conditions and the following
15 | * disclaimer in the documentation and/or other materials provided
16 | * with the distribution.
17 | * * Neither the name of the Willow Garage nor the names of its
18 | * contributors may be used to endorse or promote products derived
19 | * from this software without specific prior written permission.
20 | *
21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 | * POSSIBILITY OF SUCH DAMAGE.
33 | *********************************************************************/
34 |
35 | /* Author: Ioan Sucan */
36 |
37 | #include "collada_parser/collada_parser_plugin.h"
38 | #include "collada_parser/collada_parser.h"
39 | #include
40 |
41 | urdf::ModelInterfaceSharedPtr urdf::ColladaURDFParser::parse(const std::string &xml_string)
42 | {
43 | return urdf::parseCollada(xml_string);
44 | }
45 |
46 | CLASS_LOADER_REGISTER_CLASS(urdf::ColladaURDFParser, urdf::URDFParser)
47 |
--------------------------------------------------------------------------------
/collada_parser/include/collada_parser/collada_parser_plugin.h:
--------------------------------------------------------------------------------
1 | /*********************************************************************
2 | * Software License Agreement (BSD License)
3 | *
4 | * Copyright (c) 2013, Willow Garage, Inc.
5 | * All rights reserved.
6 | *
7 | * Redistribution and use in source and binary forms, with or without
8 | * modification, are permitted provided that the following conditions
9 | * are met:
10 | *
11 | * * Redistributions of source code must retain the above copyright
12 | * notice, this list of conditions and the following disclaimer.
13 | * * Redistributions in binary form must reproduce the above
14 | * copyright notice, this list of conditions and the following
15 | * disclaimer in the documentation and/or other materials provided
16 | * with the distribution.
17 | * * Neither the name of the Willow Garage nor the names of its
18 | * contributors may be used to endorse or promote products derived
19 | * from this software without specific prior written permission.
20 | *
21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 | * POSSIBILITY OF SUCH DAMAGE.
33 | *********************************************************************/
34 |
35 | /* Author: Ioan Sucan */
36 |
37 | #ifndef COLLADA_PARSER_COLLADA_PARSER_PLUGIN_H
38 | #define COLLADA_PARSER_COLLADA_PARSER_PLUGIN_H
39 |
40 | #include
41 |
42 | #include
43 |
44 | namespace urdf
45 | {
46 |
47 | class ColladaURDFParser : public URDFParser
48 | {
49 | public:
50 |
51 | virtual urdf::ModelInterfaceSharedPtr parse(const std::string& xml_string);
52 | };
53 |
54 | }
55 |
56 | #endif
57 |
--------------------------------------------------------------------------------
/collada_parser/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.0.2)
2 | project(collada_parser)
3 |
4 | find_package(Boost REQUIRED system)
5 |
6 | find_package(catkin REQUIRED COMPONENTS class_loader rosconsole urdf urdf_parser_plugin)
7 | find_package(urdfdom_headers REQUIRED)
8 |
9 | add_compile_options(-std=c++11)
10 |
11 | include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${urdfdom_headers_INCLUDE_DIRS})
12 |
13 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake-extensions/)
14 | find_package(PkgConfig)
15 | find_package(COLLADA_DOM 2.4 REQUIRED COMPONENTS 1.5)
16 | if(COLLADA_DOM_FOUND)
17 | include_directories(${COLLADA_DOM_INCLUDE_DIRS})
18 | link_directories(${COLLADA_DOM_LIBRARY_DIRS})
19 | endif()
20 |
21 | catkin_package(
22 | LIBRARIES ${PROJECT_NAME}
23 | INCLUDE_DIRS include
24 | CATKIN_DEPENDS class_loader rosconsole urdf urdf_parser_plugin
25 | DEPENDS COLLADA_DOM
26 | )
27 |
28 | # necessary for collada reader to create the temporary dae files due
29 | # to limitations in the URDF geometry
30 | include (CheckFunctionExists)
31 | check_function_exists(mkstemps HAVE_MKSTEMPS)
32 | if(HAVE_MKSTEMPS)
33 | add_definitions("-DHAVE_MKSTEMPS")
34 | endif()
35 |
36 | # build the parser lib
37 | add_library(${PROJECT_NAME} src/collada_parser.cpp)
38 | target_link_libraries(${PROJECT_NAME}
39 | ${COLLADA_DOM_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}
40 | )
41 |
42 | # build the plugin for the parser
43 | add_library(${PROJECT_NAME}_plugin src/collada_parser_plugin.cpp)
44 | target_link_libraries(${PROJECT_NAME}_plugin
45 | ${PROJECT_NAME} ${Boost_LIBRARIES} ${catkin_LIBRARIES}
46 | )
47 |
48 | set_target_properties(${PROJECT_NAME}
49 | PROPERTIES COMPILER_FLAGS "${COLLADA_DOM_CFLAGS_OTHER}"
50 | )
51 | if(APPLE)
52 | set_target_properties(${PROJECT_NAME}
53 | PROPERTIES LINK_FLAGS
54 | "${COLLADA_DOM_LDFLAGS_OTHER} -undefined dynamic_lookup"
55 | )
56 | else()
57 | set_target_properties(${PROJECT_NAME}
58 | PROPERTIES LINK_FLAGS "${COLLADA_DOM_LDFLAGS_OTHER}"
59 | )
60 | endif()
61 |
62 | install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugin
63 | DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
64 |
65 | install(DIRECTORY include/${PROJECT_NAME}/
66 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
67 |
68 | install(FILES collada_parser_plugin_description.xml
69 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
70 |
--------------------------------------------------------------------------------
/collada_urdf/test/test_collada_urdf.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2010, Willow Garage, Inc.
2 | // All rights reserved.
3 | //
4 | // Redistribution and use in source and binary forms, with or without
5 | // modification, are permitted provided that the following conditions are met:
6 | //
7 | // * Redistributions of source code must retain the above copyright
8 | // notice, this list of conditions and the following disclaimer.
9 | // * Redistributions in binary form must reproduce the above copyright
10 | // notice, this list of conditions and the following disclaimer in the
11 | // documentation and/or other materials provided with the distribution.
12 | // * Neither the name of Willow Garage, Inc. nor the names of its
13 | // contributors may be used to endorse or promote products derived from
14 | // this software without specific prior written permission.
15 | //
16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 | // POSSIBILITY OF SUCH DAMAGE.
27 |
28 | /* Author: Tim Field */
29 |
30 | #include "collada_urdf/collada_urdf.h"
31 |
32 | #include
33 |
34 | TEST(collada_urdf, collada_from_urdf_file_works)
35 | {
36 | urdf::Model robot_model;
37 |
38 | ASSERT_TRUE(robot_model.initFile("pr2.urdf"));
39 | ASSERT_TRUE(collada_urdf::WriteUrdfModelToColladaFile(robot_model, "pr2.dae"));
40 | }
41 |
42 | TEST(collada_urdf, collada_output_dir_does_not_exist)
43 | {
44 | urdf::Model robot_model;
45 |
46 | ASSERT_TRUE(robot_model.initFile("pr2.urdf"));
47 | ASSERT_FALSE(collada_urdf::WriteUrdfModelToColladaFile(robot_model, "a/very/long/directory/path/that/should/not/exist/pr2.dae"));
48 | }
49 |
50 | int main(int argc, char **argv)
51 | {
52 | testing::InitGoogleTest(&argc, argv);
53 | return RUN_ALL_TESTS();
54 | }
55 |
--------------------------------------------------------------------------------
/collada_urdf/include/collada_urdf/collada_urdf.h:
--------------------------------------------------------------------------------
1 | /*********************************************************************
2 | * Software License Agreement (BSD License)
3 | *
4 | * Copyright (c) 2010, Willow Garage, Inc.
5 | * All rights reserved.
6 | *
7 | * Redistribution and use in source and binary forms, with or without
8 | * modification, are permitted provided that the following conditions
9 | * are met:
10 | *
11 | * * Redstributions of source code must retain the above copyright
12 | * notice, this list of conditions and the following disclaimer.
13 | * * Redistributions in binary form must reproduce the above
14 | * copyright notice, this list of conditions and the following
15 | * disclaimer in the documentation and/or other materials provided
16 | * with the distribution.
17 | * * Neither the name of the Willow Garage nor the names of its
18 | * contributors may be used to endorse or promote products derived
19 | * from this software without specific prior written permission.
20 | *
21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 | * POSSIBILITY OF SUCH DAMAGE.
33 | *********************************************************************/
34 |
35 | /* Authors: Tim Field */
36 |
37 | #ifndef COLLADA_URDF_COLLADA_URDF_H
38 | #define COLLADA_URDF_COLLADA_URDF_H
39 |
40 | #include
41 | #include
42 |
43 | #include "urdf/model.h"
44 |
45 | namespace collada_urdf {
46 |
47 | class ColladaUrdfException : public std::runtime_error
48 | {
49 | public:
50 | ColladaUrdfException(std::string const& what);
51 | };
52 |
53 | /** Write a COLLADA DOM to a file
54 | * \param robot_model The URDF robot model
55 | * \param file The filename to write the document to
56 | * \return true on success, false on failure
57 | */
58 | bool WriteUrdfModelToColladaFile(urdf::Model const& robot_model, std::string const& file);
59 |
60 | }
61 |
62 | #endif
63 |
--------------------------------------------------------------------------------
/collada_urdf/src/urdf_to_collada.cpp:
--------------------------------------------------------------------------------
1 | /*********************************************************************
2 | * Software License Agreement (BSD License)
3 | *
4 | * Copyright (c) 2010, Willow Garage, Inc.
5 | * All rights reserved.
6 | *
7 | * Redistribution and use in source and binary forms, with or without
8 | * modification, are permitted provided that the following conditions
9 | * are met:
10 | *
11 | * * Redstributions of source code must retain the above copyright
12 | * notice, this list of conditions and the following disclaimer.
13 | * * Redistributions in binary form must reproduce the above
14 | * copyright notice, this list of conditions and the following
15 | * disclaimer in the documentation and/or other materials provided
16 | * with the distribution.
17 | * * Neither the name of the Willow Garage nor the names of its
18 | * contributors may be used to endorse or promote products derived
19 | * from this software without specific prior written permission.
20 | *
21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 | * POSSIBILITY OF SUCH DAMAGE.
33 | *********************************************************************/
34 |
35 | /* Author: Tim Field */
36 |
37 | #include "collada_urdf/collada_urdf.h"
38 |
39 | int main(int argc, char** argv)
40 | {
41 | if (argc != 3) {
42 | std::cerr << "Usage: urdf_to_collada input.urdf output.dae" << std::endl;
43 | return -1;
44 | }
45 |
46 | std::string input_filename(argv[1]);
47 | std::string output_filename(argv[2]);
48 |
49 | urdf::Model robot_model;
50 | if (!robot_model.initFile(input_filename)) {
51 | std::cerr << "failed to open urdf file " << input_filename << std::endl;
52 | return -2;
53 | }
54 |
55 | collada_urdf::WriteUrdfModelToColladaFile(robot_model, output_filename);
56 | std::cout << std::endl << "Document successfully written to " << output_filename << std::endl;
57 |
58 | return 0;
59 | }
60 |
--------------------------------------------------------------------------------
/collada_urdf/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.0.2)
2 | project(collada_urdf)
3 |
4 | find_package(catkin REQUIRED COMPONENTS angles cmake_modules collada_parser geometric_shapes resource_retriever rosconsole urdf)
5 |
6 | find_package(urdfdom_headers REQUIRED)
7 |
8 | include(CheckCXXCompilerFlag)
9 | check_cxx_compiler_flag(-std=c++11 HAS_STD_CPP11_FLAG)
10 | if(HAS_STD_CPP11_FLAG)
11 | add_compile_options(-std=c++11)
12 | endif()
13 |
14 | include_directories(include)
15 |
16 | find_package(assimp QUIET)
17 | if ( NOT ASSIMP_FOUND )
18 | find_package(Assimp QUIET)
19 | if ( NOT ASSIMP_FOUND )
20 | find_package(PkgConfig REQUIRED)
21 | pkg_check_modules(ASSIMP assimp)
22 | endif()
23 | endif()
24 | if( ASSIMP_FOUND )
25 | if( NOT ${ASSIMP_VERSION} VERSION_LESS "2.0.1150" )
26 | add_definitions(-DASSIMP_UNIFIED_HEADER_NAMES)
27 | endif()
28 | if( NOT ${ASSIMP_VERSION} VERSION_LESS "2.0.885" )
29 | add_definitions(-DASSIMP_EXPORT_API)
30 | endif()
31 | include_directories(${ASSIMP_INCLUDE_DIRS})
32 | link_directories(${ASSIMP_LIBRARY_DIRS})
33 | else()
34 | message(STATUS "could not find assimp (perhaps available thorugh ROS package?), so assuming assimp v2")
35 | set(ASSIMP_LIBRARIES assimp)
36 | set(ASSIMP_LIBRARY_DIRS)
37 | set(ASSIMP_CXX_FLAGS)
38 | set(ASSIMP_CFLAGS_OTHER)
39 | set(ASSIMP_LINK_FLAGS)
40 | set(ASSIMP_INCLUDE_DIRS)
41 | endif()
42 |
43 | # Note: assimp 3.1 overwrites CMake Boost variables, so we need to check for
44 | # Boost after assimp.
45 | find_package(Boost REQUIRED COMPONENTS system filesystem program_options)
46 | include_directories(${Boost_INCLUDE_DIR})
47 |
48 | find_package(Eigen3 REQUIRED)
49 |
50 | find_package(COLLADA_DOM 2.3 COMPONENTS 1.5)
51 | if( COLLADA_DOM_FOUND )
52 | include_directories(${COLLADA_DOM_INCLUDE_DIRS})
53 | link_directories(${COLLADA_DOM_LIBRARY_DIRS})
54 | endif()
55 |
56 | catkin_package(
57 | LIBRARIES ${PROJECT_NAME}
58 | INCLUDE_DIRS include
59 | CATKIN_DEPENDS collada_parser geometric_shapes resource_retriever rosconsole urdf
60 | DEPENDS ASSIMP COLLADA_DOM urdfdom_headers
61 | )
62 |
63 | include_directories(${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS})
64 | link_directories(${catkin_LIBRARY_DIRS})
65 |
66 | add_library(${PROJECT_NAME} src/collada_urdf.cpp)
67 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES}
68 | ${Boost_LIBRARIES} ${ASSIMP_LIBRARIES})
69 | set_target_properties(${PROJECT_NAME} PROPERTIES COMPILER_FLAGS "${ASSIMP_CXX_FLAGS} ${ASSIMP_CFLAGS_OTHER}")
70 | set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS "${ASSIMP_LINK_FLAGS}")
71 |
72 | add_executable(urdf_to_collada src/urdf_to_collada.cpp)
73 | target_link_libraries(urdf_to_collada ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES}
74 | ${Boost_LIBRARIES} ${PROJECT_NAME})
75 |
76 | add_executable(collada_to_urdf src/collada_to_urdf.cpp)
77 | target_link_libraries(collada_to_urdf ${ASSIMP_LIBRARIES} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} ${Boost_LIBRARIES})
78 | set_target_properties(collada_to_urdf PROPERTIES COMPILER_FLAGS "${ASSIMP_CXX_FLAGS} ${ASSIMP_CFLAGS_OTHER}")
79 | set_target_properties(collada_to_urdf PROPERTIES LINK_FLAGS "${ASSIMP_LINK_FLAGS}")
80 |
81 | if(CATKIN_ENABLE_TESTING)
82 | catkin_add_gtest(test_collada_urdf test/test_collada_urdf.cpp WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test)
83 | target_link_libraries(test_collada_urdf ${PROJECT_NAME} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES}
84 | ${Boost_LIBRARIES})
85 | endif()
86 |
87 | install(TARGETS ${PROJECT_NAME} urdf_to_collada collada_to_urdf
88 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
89 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
90 |
91 | install(DIRECTORY include/${PROJECT_NAME}/
92 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
93 |
--------------------------------------------------------------------------------
/collada_parser/CHANGELOG.rst:
--------------------------------------------------------------------------------
1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2 | Changelog for package collada_parser
3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4 |
5 | 1.12.13 (2020-07-14)
6 | --------------------
7 | * Update to newer CMake to get rid of CMP0048 warning (`#38 `_)
8 | * Contributors: Chris Lalancette
9 |
10 | 1.12.12 (2018-05-08)
11 | --------------------
12 | * add exec_depend to package.xml of collada_parser for loading by pluginlib (`#27 `_)
13 | * Contributors: Yohei Kakiuchi
14 |
15 | 1.12.11 (2018-04-17)
16 | --------------------
17 | * Collada cleanup dependencies (`#26 `_)
18 | * update links now that this is in its own repo
19 | * Make CMakeLists.txt depend on collada-dom version 2.4. (`#11 `_)
20 | * Contributors: Chris Lalancette, Mikael Arguedas
21 |
22 | 1.12.10 (2017-05-04)
23 | --------------------
24 | * Moved collada_parser and collada_urdf to new repository
25 |
26 | 1.12.9 (2017-04-26)
27 | -------------------
28 |
29 | 1.12.8 (2017-03-27)
30 | -------------------
31 | * add Chris and Shane as maintainers (`#184 `_)
32 | * fix missed mandatory -std=c++11 flag (`#181 `_)
33 | collada_parser,kdl_parser,urdf: add c++11 flag,
34 | collada_parser: replace typeof with ansi __typeof\_\_
35 | builded/tested on gentoo
36 | Thanks den4ix for the contribution!
37 | * Contributors: Denis Romanchuk, William Woodall
38 |
39 | 1.12.7 (2017-01-26)
40 | -------------------
41 |
42 | 1.12.6 (2017-01-04)
43 | -------------------
44 | * Now using ``urdf::*ShredPtr`` instead of ``boost::shared_ptr`` (`#144 `_)
45 | * Contributors: Jochen Sprickerhof
46 |
47 | 1.12.5 (2016-10-27)
48 | -------------------
49 |
50 | 1.12.4 (2016-08-23)
51 | -------------------
52 |
53 | 1.12.3 (2016-06-10)
54 | -------------------
55 |
56 | 1.12.2 (2016-04-12)
57 | -------------------
58 |
59 | 1.12.1 (2016-04-10)
60 | -------------------
61 |
62 | 1.11.8 (2015-09-11)
63 | -------------------
64 |
65 | 1.11.7 (2015-04-22)
66 | -------------------
67 |
68 | 1.11.6 (2014-11-30)
69 | -------------------
70 | * fix rotation of joint axis when oriantation between parent link and child link is differ
71 | * Contributors: YoheiKakiuchi
72 |
73 | 1.11.5 (2014-07-24)
74 | -------------------
75 |
76 | 1.11.4 (2014-07-07)
77 | -------------------
78 | * collada_parser: add extract actuators, fix for using nominal torque
79 | * moving to new dependency for urdfdom and urdfdom_headers. https://github.com/ros/rosdistro/issues/4633
80 | * Contributors: Tully Foote, YoheiKakiuchi
81 |
82 | 1.11.3 (2014-06-24)
83 | -------------------
84 | * update usage of urdfdom_headers for indigo/trusty
85 | * Contributors: William Woodall
86 |
87 | 1.11.2 (2014-03-22)
88 | -------------------
89 |
90 | 1.11.1 (2014-03-20)
91 | -------------------
92 | * remove visual and collision if there is no vertices
93 | * do not use visual and collision group
94 | * fix debug message
95 | * fix problem of root coordinates
96 | * Contributors: YoheiKakiuchi
97 |
98 | 1.11.0 (2014-02-21)
99 | -------------------
100 | * fix, joint axis should be rotated depend on local coords
101 | * fix overwriting velocity limit
102 | * Contributors: YoheiKakiuchi
103 |
104 | 1.10.18 (2013-12-04)
105 | --------------------
106 | * add DEPENDS for kdl_parser
107 | * Contributors: Ioan Sucan
108 |
109 | 1.10.16 (2013-11-18)
110 | --------------------
111 | * fix for using collada_parser_plugin
112 |
113 | 1.10.15 (2013-08-17)
114 | --------------------
115 |
--------------------------------------------------------------------------------
/collada_urdf/CHANGELOG.rst:
--------------------------------------------------------------------------------
1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2 | Changelog for package collada_urdf
3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4 |
5 | 1.12.13 (2020-07-14)
6 | --------------------
7 | * Update to newer CMake to get rid of CMP0048 warning (`#38 `_)
8 | * Enable to output transmission_interface instead of pr2_mechanism_model (`#35 `_)
9 | * Contributors: Chris Lalancette, Shun Hasegawa
10 |
11 | 1.12.12 (2018-05-08)
12 | --------------------
13 |
14 | 1.12.11 (2018-04-17)
15 | --------------------
16 | * Collada cleanup dependencies (`#26 `_)
17 | * update links now that this is in its own repo
18 | * Switch to using Eigen for Quaternion and Matrix. (`#21 `_)
19 | * add relicensing comment (`#19 `_)
20 | * remove unused tinyxml from cmakelists (`#15 `_)
21 | * Contributors: Chris Lalancette, Mikael Arguedas, Rosen Diankov
22 |
23 | 1.12.10 (2017-05-04)
24 | --------------------
25 | * Moved collada_parser and collada_urdf to new repository
26 |
27 | 1.12.9 (2017-04-26)
28 | -------------------
29 |
30 | 1.12.8 (2017-03-27)
31 | -------------------
32 | * Remove old gazebo settings.
33 | Based on an initial patch from YoheiKakiuchi, just totally
34 | remove old Gazebo 1.0 settings, as they are never used and
35 | almost certainly will never be used.
36 | * add Chris and Shane as maintainers (`#184 `_)
37 | * Do a few cleanup tasks in collada_urdf (`#183 `_)
38 | * Style cleanup within collada_urdf.
39 | No functional change, just style.
40 | * Make sure to quit out of urdf_to_collada when invalid file is found.
41 | Otherwise, we'll just end up segfaulting later on.
42 | * Re-enable one of the collada-urdf tests.
43 | In point of fact, we delete the rest of the tests because
44 | they don't make much sense anymore. Just enable this one
45 | for now; we'll enable further ones in the future.
46 | * Add in another test for collada_urdf.
47 | * remove divide by 2 when writing boxes to collada format (`#133 `_)
48 | * Contributors: Chris Lalancette, Jackie Kay, William Woodall
49 |
50 | 1.12.7 (2017-01-26)
51 | -------------------
52 |
53 | 1.12.6 (2017-01-04)
54 | -------------------
55 | * Now using ``urdf::*ShredPtr`` instead of ``boost::shared_ptr`` (`#144 `_)
56 | * Contributors: Jochen Sprickerhof
57 |
58 | 1.12.5 (2016-10-27)
59 | -------------------
60 |
61 | 1.12.4 (2016-08-23)
62 | -------------------
63 | * Use the C++11 standard (`#145 `_)
64 | * Contributors: William Woodall
65 |
66 | 1.12.3 (2016-06-10)
67 | -------------------
68 |
69 | 1.12.2 (2016-04-12)
70 | -------------------
71 |
72 | 1.12.1 (2016-04-10)
73 | -------------------
74 |
75 | 1.11.8 (2015-09-11)
76 | -------------------
77 | * Removed pcre hack for newer released collada-dom.
78 | * Contributors: Kei Okada
79 |
80 | 1.11.7 (2015-04-22)
81 | -------------------
82 | * Fixed `#89 `_
83 | Accomplished by loading libpcrecpp before collada-dom.
84 | * Contributors: Kei Okada, William Woodall
85 |
86 | 1.11.6 (2014-11-30)
87 | -------------------
88 |
89 | 1.11.5 (2014-07-24)
90 | -------------------
91 |
92 | 1.11.4 (2014-07-07)
93 | -------------------
94 | * moving to new dependency for urdfdom and urdfdom_headers. https://github.com/ros/rosdistro/issues/4633
95 | * Fix clash with assimp 3.1 in CMake.
96 | * Contributors: Benjamin Chrétien, Tully Foote
97 |
98 | 1.11.3 (2014-06-24)
99 | -------------------
100 | * Merge pull request `#69 `_ from YoheiKakiuchi/indigo-devel-store-original-mesh-name
101 | storing original mesh file name and location
102 | * storing original mesh file name and location
103 | * Contributors: Ioan A Sucan, YoheiKakiuchi
104 |
105 | 1.11.2 (2014-03-22)
106 | -------------------
107 | * use new urdfdom_headers API
108 | * Contributors: Ioan Sucan
109 |
110 | 1.11.1 (2014-03-20)
111 | -------------------
112 | * Use assimp-dev dep for building
113 | * Contributors: Scott K Logan
114 |
115 | 1.11.0 (2014-02-21)
116 | -------------------
117 | * Use VERSION_LESS instead of STRLESS
118 | The version comparison routines were added in cmake 2.8.0
119 | * Fix export API detection (for assimp < 2.0.885)
120 | It looks like this API was added in Assimp 2.0.885:
121 | https://github.com/assimp/assimp/commit/ae23c03bd9a0b5f1227dc0042fd98f7206c770a8
122 | * Invert Assimp version detect logic for greater accuracy
123 | * Updated Assimp defines to be more flexible
124 | This commit is a follow-up to 85b20197671e142044e471df603debd0faf08baf
125 | Why was export.h removed from assimp < 3.0.0?
126 | * Better feature detection for assimp version
127 | The unified headers were introduced in Assimp 2.0.1150, so checking for Assimp 3.0.0 is not quite the best solution.
128 | See https://github.com/assimp/assimp/commit/6fa251c2f2e7a142bb861227dce0c26362927fbc
129 | * Contributors: Scott K Logan
130 |
131 | 1.10.18 (2013-12-04)
132 | --------------------
133 | * add DEPENDS for kdl_parser
134 | * Contributors: Ioan Sucan
135 |
136 | 1.10.16 (2013-11-18)
137 | --------------------
138 | * check for CATKIN_ENABLE_TESTING
139 | * fix for compiling collada_to_urdf, add dependency to tf
140 | * add collada_to_urdf.cpp for converting collada file to urdf file
141 |
142 | 1.10.15 (2013-08-17)
143 | --------------------
144 | * fix `#30 `_
145 |
--------------------------------------------------------------------------------
/collada_urdf/src/collada_to_urdf.cpp:
--------------------------------------------------------------------------------
1 | /* Author: Yohei Kakiuchi */
2 |
3 | #include
4 | #include
5 | #include
6 | #include
7 |
8 | #include
9 | #include
10 |
11 | #include
12 | #include
13 |
14 | #if defined(ASSIMP_UNIFIED_HEADER_NAMES)
15 | #include
16 | #include
17 | #include
18 | #include
19 | #include
20 | #include
21 | #else
22 | #include
23 | #if defined(ASSIMP_EXPORT_API)
24 | #include
25 | #endif
26 | #include
27 | #include
28 | #endif
29 |
30 | #include
31 | #include
32 | #include
33 |
34 | #include
35 |
36 | #undef GAZEBO_1_3
37 |
38 | #define GAZEBO_1_3
39 |
40 | using namespace urdf;
41 | using namespace std;
42 |
43 | bool use_simple_visual = false;
44 | bool use_simple_collision = false;
45 | bool add_gazebo_description = false;
46 | bool use_transmission_interface = false;
47 | bool use_assimp_export = false;
48 | bool use_same_collision_as_visual = true;
49 | bool rotate_inertia_frame = true;
50 | bool export_collision_mesh = false;
51 |
52 | string mesh_dir = "/tmp";
53 | string arobot_name = "";
54 | string output_file = "";
55 | string mesh_prefix = "";
56 |
57 | #define PRINT_ORIGIN(os, origin) \
58 | os << "xyz: " << origin.position.x << " " << origin.position.y << " " << origin.position.z << " "; \
59 | { double r,p,y; origin.rotation.getRPY(r, p, y); \
60 | os << "rpy: " << r << " " << p << " " << y; }
61 |
62 | #define PRINT_ORIGIN_XML(os, origin) \
63 | os << "xyz=\"" << origin.position.x << " " << origin.position.y << " " << origin.position.z << "\""; \
64 | { double h___r, h___p, h___y; \
65 | origin.rotation.getRPY(h___r, h___p, h___y); \
66 | os << " rpy=\"" << h___r << " " << h___p << " " << h___y << "\""; }
67 |
68 | #define PRINT_GEOM(os, geometry) \
69 | if ( geometry->type == urdf::Geometry::MESH ) { os << "geom: name: " << ((urdf::Mesh *)geometry.get())->filename; }
70 |
71 | void assimp_file_export(std::string fname, std::string ofname,
72 | std::string mesh_type = "collada") {
73 | #if defined(ASSIMP_EXPORT_API)
74 | if (fname.find("file://") == 0) {
75 | fname.erase(0, strlen("file://"));
76 | }
77 | Assimp::Importer importer;
78 | /*
79 | { // ignore UP_DIRECTION tag in collada
80 | bool existing;
81 | importer.SetPropertyBool(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, true, &existing);
82 | if(existing) {
83 | fprintf(stderr, ";; OverWrite : Ignore UP_DIRECTION", existing);
84 | }
85 | }
86 | */
87 | const aiScene* scene = importer.ReadFile(fname.c_str(),
88 | aiProcess_Triangulate |
89 | aiProcess_GenNormals |
90 | aiProcess_JoinIdenticalVertices |
91 | aiProcess_SplitLargeMeshes |
92 | aiProcess_OptimizeMeshes |
93 | aiProcess_SortByPType);
94 |
95 | if (!scene) {
96 | std::string str( importer.GetErrorString() );
97 | std::cerr << ";; " << str << std::endl;
98 | return;
99 | }
100 |
101 | Assimp::Exporter aexpt;
102 | aiReturn ret = aexpt.Export(scene, mesh_type, ofname);
103 | if ( ret != AI_SUCCESS ) {
104 | std::string str( "assimp error" );
105 | std::cerr << ";; " << str << std::endl;
106 | }
107 | #endif
108 | }
109 |
110 | // assimp bounding box calculation
111 | void assimp_calc_bbox(string fname, float &minx, float &miny, float &minz,
112 | float &maxx, float &maxy, float &maxz) {
113 |
114 | if (fname.find("file://") == 0) {
115 | fname.erase(0, strlen("file://"));
116 | }
117 |
118 | Assimp::Importer importer;
119 | const aiScene* scene = importer.ReadFile(fname.c_str(),
120 | aiProcess_Triangulate |
121 | aiProcess_JoinIdenticalVertices |
122 | aiProcess_SortByPType); // aiProcess_GenNormals
123 | // aiProcess_GenSmoothNormals
124 | // aiProcess_SplitLargeMeshes
125 | if (!scene) {
126 | std::string str( importer.GetErrorString() );
127 | std::cerr << ";; " << str << std::endl;
128 | return;
129 | }
130 |
131 | aiNode *node = scene->mRootNode;
132 |
133 | bool found = false;
134 | if(node->mNumMeshes > 0 && node->mMeshes != NULL) {
135 | std::cerr << "Root node has meshes " << node->mMeshes << std::endl;;
136 | found = true;
137 | } else {
138 | for (unsigned int i=0; i < node->mNumChildren; ++i) {
139 | if(node->mChildren[i]->mNumMeshes > 0 && node->mChildren[i]->mMeshes != NULL) {
140 | std::cerr << "Child " << i << " has meshes" << std::endl;
141 | node = node->mChildren[i];
142 | found = true;
143 | break;
144 | }
145 | }
146 | }
147 | if(found == false) {
148 | std::cerr << "Can't find meshes in file" << std::endl;
149 | return;
150 | }
151 |
152 | aiMatrix4x4 transform = node->mTransformation;
153 |
154 | // copy vertices
155 | maxx = maxy = maxz = -100000000.0;
156 | minx = miny = minz = 100000000.0;
157 |
158 | std::cerr << ";; num meshes: " << node->mNumMeshes << std::endl;
159 | for (unsigned int m = 0; m < node->mNumMeshes; m++) {
160 | aiMesh *a = scene->mMeshes[node->mMeshes[m]];
161 | std::cerr << ";; num vertices: " << a->mNumVertices << std::endl;
162 |
163 | for (unsigned int i = 0 ; i < a->mNumVertices ; ++i) {
164 | aiVector3D p;
165 | p.x = a->mVertices[i].x;
166 | p.y = a->mVertices[i].y;
167 | p.z = a->mVertices[i].z;
168 | p *= transform;
169 |
170 | if ( maxx < p.x ) {
171 | maxx = p.x;
172 | }
173 | if ( maxy < p.y ) {
174 | maxy = p.y;
175 | }
176 | if ( maxz < p.z ) {
177 | maxz = p.z;
178 | }
179 | if ( minx > p.x ) {
180 | minx = p.x;
181 | }
182 | if ( miny > p.y ) {
183 | miny = p.y;
184 | }
185 | if ( minz > p.z ) {
186 | minz = p.z;
187 | }
188 | }
189 | }
190 | }
191 |
192 | void addChildLinkNamesXML(urdf::LinkConstSharedPtr link, ofstream& os)
193 | {
194 | os << " name << "\">" << endl;
195 | if ( !!link->visual ) {
196 | os << " " << endl;
197 |
198 | if (!use_simple_visual) {
199 | os << " visual->origin);
201 | os << "/>" << endl;
202 | os << " " << endl;
203 | if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
204 | std::string ifname (((urdf::Mesh *)link->visual->geometry.get())->filename);
205 | if (ifname.find("file://") == 0) {
206 | ifname.erase(0, strlen("file://"));
207 | }
208 | std::string ofname (mesh_dir + "/" + link->name + "_mesh.dae");
209 |
210 | if (use_assimp_export) {
211 | // using collada export
212 | assimp_file_export (ifname, ofname);
213 | } else {
214 | // copy to ofname
215 | std::ofstream tmp_os;
216 | tmp_os.open(ofname.c_str());
217 | std::ifstream is;
218 | is.open(ifname.c_str());
219 | std::string buf;
220 | while(is && getline(is, buf)) tmp_os << buf << std::endl;
221 | is.close();
222 | tmp_os.close();
223 | }
224 | if (mesh_prefix != "") {
225 | os << " name + "_mesh.dae" << "\" scale=\"1 1 1\" />" << endl;
226 | } else {
227 | os << " " << endl;
228 | }
229 | }
230 | os << " " << endl;
231 | } else {
232 | // simple visual
233 | float ax,ay,az,bx,by,bz;
234 | if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
235 | assimp_calc_bbox(((urdf::Mesh *)link->visual->geometry.get())->filename,
236 | ax, ay, az, bx, by, bz);
237 | }
238 | os << " visual->origin;
240 |
241 | pp.position.x += ( ax + bx ) / 2 ;
242 | pp.position.y += ( ay + by ) / 2 ;
243 | pp.position.z += ( az + bz ) / 2 ;
244 | PRINT_ORIGIN_XML(os, pp);
245 | os << "/>" << endl;
246 |
247 | os << " " << endl;
248 | os << " " << endl;
249 | os << " " << endl;
250 | }
251 | os << " " << endl;
252 | }
253 | if ( !!link->collision ) {
254 | os << " " << endl;
255 | if (!use_simple_collision) {
256 | os << " collision->origin);
258 | os << "/>" << endl;
259 | os << " " << endl;
260 |
261 | if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
262 | std::string ifname;
263 | if (use_same_collision_as_visual) {
264 | ifname.assign (((urdf::Mesh *)link->visual->geometry.get())->filename);
265 | } else {
266 | ifname.assign (((urdf::Mesh *)link->collision->geometry.get())->filename);
267 | }
268 | if (ifname.find("file://") == 0) {
269 | ifname.erase(0, strlen("file://"));
270 | }
271 | std::string oofname;
272 | if (export_collision_mesh) {
273 | oofname = link->name + "_mesh.stl";
274 | } else {
275 | oofname = link->name + "_mesh.dae";
276 | }
277 | std::string ofname = (mesh_dir + "/" + oofname);
278 |
279 | if (use_assimp_export) {
280 | // using collada export
281 | if (export_collision_mesh) {
282 | assimp_file_export (ifname, ofname, "stl");
283 | } else {
284 | assimp_file_export (ifname, ofname);
285 | }
286 | } else {
287 | // copy to ofname
288 | std::ofstream tmp_os;
289 | tmp_os.open(ofname.c_str());
290 | std::ifstream is;
291 | is.open(ifname.c_str());
292 | std::string buf;
293 | while(is && getline(is, buf)) tmp_os << buf << std::endl;
294 | is.close();
295 | tmp_os.close();
296 | }
297 | if (mesh_prefix != "") {
298 | os << " " << endl;
303 | }
304 | os << " " << endl;
305 | } else {
306 | // simple collision
307 | float ax,ay,az,bx,by,bz;
308 | if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
309 | assimp_calc_bbox(std::string ( ((urdf::Mesh *)link->visual->geometry.get())->filename ),
310 | ax, ay, az, bx, by, bz);
311 | }
312 | os << " visual->origin;
314 | pp.position.x += ( ax + bx ) / 2 ;
315 | pp.position.y += ( ay + by ) / 2 ;
316 | pp.position.z += ( az + bz ) / 2 ;
317 | PRINT_ORIGIN_XML(os, pp);
318 | os << "/>" << endl;
319 |
320 | os << " " << endl;
321 | os << " " << endl;
322 | os << " " << endl;
323 | }
324 | os << " " << endl;
325 | }
326 | if ( !!link->inertial ) {
327 | if (!rotate_inertia_frame) {
328 | os << " " << endl;
329 | os << " inertial->mass << "\" />" << endl;
330 | os << " inertial->origin);
332 | os << "/>" << endl;
333 | os << " inertial->ixx << "\" ";
334 | os << "ixy=\"" << link->inertial->ixy << "\" ";
335 | os << "ixz=\"" << link->inertial->ixz << "\" ";
336 | os << "iyy=\"" << link->inertial->iyy << "\" ";
337 | os << "iyz=\"" << link->inertial->iyz << "\" ";
338 | os << "izz=\"" << link->inertial->izz << "\"/>" << endl;
339 | os << " " << endl;
340 | } else {
341 | // rotation should be identity
342 | os << " " << endl;
343 | os << " inertial->mass << "\" />" << endl;
344 | os << " inertial->origin.rotation.w,
347 | link->inertial->origin.rotation.x,
348 | link->inertial->origin.rotation.y,
349 | link->inertial->origin.rotation.z);
350 | Eigen::Matrix3d mat(qt);
351 | Eigen::Matrix3d tmat(mat.transpose());
352 | Eigen::Matrix3d imat;
353 | imat(0, 0) = link->inertial->ixx;
354 | imat(0, 1) = link->inertial->ixy;
355 | imat(0, 2) = link->inertial->ixz;
356 | imat(1, 0) = link->inertial->ixy;
357 | imat(1, 1) = link->inertial->iyy;
358 | imat(1, 2) = link->inertial->iyz;
359 | imat(2, 0) = link->inertial->ixz;
360 | imat(2, 1) = link->inertial->iyz;
361 | imat(2, 2) = link->inertial->izz;
362 |
363 | #define DEBUG_MAT(mat) \
364 | cout << "#2f((" << mat(0, 0) << " " << mat(0, 1) << " " << mat(0, 2) << ")"; \
365 | cout << "(" << mat(1, 0) << " " << mat(1, 1) << " " << mat(1, 2) << ")"; \
366 | cout << "(" << mat(2, 0) << " " << mat(2, 1) << " " << mat(2, 2) << "))" << endl;
367 |
368 | #if DEBUG
369 | DEBUG_MAT(mat);
370 | DEBUG_MAT(tmat);
371 | DEBUG_MAT(imat);
372 | #endif
373 |
374 | imat = ( mat * imat * tmat );
375 |
376 | #if DEBUG
377 | DEBUG_MAT(imat);
378 | #endif
379 |
380 | urdf::Pose t_pose (link->inertial->origin);
381 | t_pose.rotation.clear();
382 |
383 | PRINT_ORIGIN_XML(os, t_pose);
384 | os << "/>" << endl;
385 |
386 | os << " " << endl;
392 | os << " " << endl;
393 | }
394 | }
395 | os << " " << endl;
396 |
397 | #ifdef GAZEBO_1_3
398 | if ( add_gazebo_description ) {
399 | os << " name << "\">" << endl;
400 | os << " 0.9" << endl;
401 | os << " 0.9" << endl;
402 | os << " " << endl;
403 | }
404 | #endif
405 |
406 | for (std::vector::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
407 | addChildLinkNamesXML(*child, os);
408 | }
409 |
410 | void addChildJointNamesXML(urdf::LinkConstSharedPtr link, ofstream& os)
411 | {
412 | double r, p, y;
413 | for (std::vector::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){
414 | (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y);
415 | std::string jtype;
416 | if ( (*child)->parent_joint->type == urdf::Joint::UNKNOWN ) {
417 | jtype = std::string("unknown");
418 | } else if ( (*child)->parent_joint->type == urdf::Joint::REVOLUTE ) {
419 | jtype = std::string("revolute");
420 | } else if ( (*child)->parent_joint->type == urdf::Joint::CONTINUOUS ) {
421 | jtype = std::string("continuous");
422 | } else if ( (*child)->parent_joint->type == urdf::Joint::PRISMATIC ) {
423 | jtype = std::string("prismatic");
424 | } else if ( (*child)->parent_joint->type == urdf::Joint::FLOATING ) {
425 | jtype = std::string("floating");
426 | } else if ( (*child)->parent_joint->type == urdf::Joint::PLANAR ) {
427 | jtype = std::string("planar");
428 | } else if ( (*child)->parent_joint->type == urdf::Joint::FIXED ) {
429 | jtype = std::string("fixed");
430 | } else {
431 | ///error
432 | }
433 |
434 | os << " parent_joint->name << "\" type=\"" << jtype << "\">" << endl;
435 | os << " name << "\"/>" << endl;
436 | os << " name << "\"/>" << endl;
437 | os << " parent_joint->parent_to_joint_origin_transform.position.x << " ";
438 | os << (*child)->parent_joint->parent_to_joint_origin_transform.position.y << " ";
439 | os << (*child)->parent_joint->parent_to_joint_origin_transform.position.z;
440 | os << "\" rpy=\"" << r << " " << p << " " << y << " " << "\"/>" << endl;
441 | os << " parent_joint->axis.x << " ";
442 | os << (*child)->parent_joint->axis.y << " " << (*child)->parent_joint->axis.z << "\"/>" << endl;
443 | {
444 | urdf::JointSharedPtr jt((*child)->parent_joint);
445 |
446 | if ( !!jt->limits ) {
447 | os << " limits->lower << "\"";
449 | os << " upper=\"" << jt->limits->upper << "\"";
450 | if (jt->limits->effort == 0.0) {
451 | os << " effort=\"100\"";
452 | } else {
453 | os << " effort=\"" << jt->limits->effort << "\"";
454 | }
455 | os << " velocity=\"" << jt->limits->velocity << "\"";
456 | os << " />" << endl;
457 | }
458 | if ( !!jt->dynamics ) {
459 | os << " dynamics->damping << "\"";
461 | os << " friction=\"" << jt->dynamics->friction << "\"";
462 | os << " />" << endl;
463 | } else {
464 | os << " " << endl;
468 | }
469 | #ifdef GAZEBO_1_3
470 | #if 0
471 | os << " " << endl;
477 | #endif
478 | #endif
479 | }
480 |
481 | os << " " << endl;
482 |
483 | if ( add_gazebo_description ) {
484 | if ( !use_transmission_interface ) {
485 | os << " parent_joint->name << "_trans\" >" << endl;
487 | os << " parent_joint->name << "_motor\" />" << endl;
488 | os << " parent_joint->name << "\" />" << endl;
489 | os << " 1" << endl;
490 | //os << " 1" << endl;
491 | //os << " 90000" << endl;
492 | os << " " << endl;
493 | } else {
494 | os << " parent_joint->name << "_trans\">" << endl;
495 | os << " transmission_interface/SimpleTransmission" << endl;
496 | os << " parent_joint->name << "\">" << endl;
497 | os << " hardware_interface/EffortJointInterface" << endl;
498 | os << " " << endl;
499 | os << " parent_joint->name << "_motor\">" << endl;
500 | os << " hardware_interface/EffortJointInterface" << endl;
501 | os << " 1" << endl;
502 | os << " " << endl;
503 | os << " " << endl;
504 | }
505 | #ifdef GAZEBO_1_3
506 | os << " parent_joint->name << "\">" << endl;
507 | os << " 0.4" << endl;
508 | os << " " << endl;
509 | #endif
510 | }
511 | addChildJointNamesXML(*child, os);
512 | }
513 | }
514 |
515 | void printTreeXML(urdf::LinkConstSharedPtr link, string name, string file)
516 | {
517 | std::ofstream os;
518 | os.open(file.c_str());
519 | os << "" << endl;
520 | os << "" << endl;
522 |
523 | addChildLinkNamesXML(link, os);
524 |
525 | addChildJointNamesXML(link, os);
526 |
527 | os << "" << endl;
528 | os.close();
529 | }
530 |
531 | namespace po = boost::program_options;
532 | // using namespace std;
533 |
534 | int main(int argc, char** argv)
535 | {
536 | string inputfile;
537 |
538 | po::options_description desc("Usage: collada_to_urdf input.dae [options]\n Options for collada_to_urdf");
539 | desc.add_options()
540 | ("help", "produce help message")
541 | ("simple_visual,V", "use bounding box for visual")
542 | ("simple_collision,C", "use bounding box for collision")
543 | ("export_collision_mesh", "export collision mesh as STL")
544 | ("add_gazebo_description,G", "add description for using on gazebo")
545 | ("use_transmission_interface,T", "use transmission_interface as transmission")
546 | ("use_assimp_export,A", "use assimp library for exporting mesh")
547 | ("use_collision,U", "use collision geometry (default collision is the same as visual)")
548 | ("original_inertia_rotation,R", "does not rotate inertia frame")
549 | ("robot_name,N", po::value< vector >(), "output robot name")
550 | ("mesh_output_dir", po::value< vector >(), "directory for outputing")
551 | ("mesh_prefix", po::value< vector >(), "prefix of mesh files")
552 | ("output_file,O", po::value< vector >(), "output file")
553 | ("input_file", po::value< vector >(), "input file")
554 | ;
555 |
556 | po::positional_options_description p;
557 | p.add("input_file", -1);
558 |
559 | po::variables_map vm;
560 | try {
561 | po::store(po::command_line_parser(argc, argv).
562 | options(desc).positional(p).run(), vm);
563 | po::notify(vm);
564 | }
565 | catch (po::error e) {
566 | cerr << ";; option parse error / " << e.what() << endl;
567 | return 1;
568 | }
569 |
570 | if (vm.count("help")) {
571 | cout << desc << "\n";
572 | return 1;
573 | }
574 | if (vm.count("simple_visual")) {
575 | use_simple_visual = true;
576 | cerr << ";; Using simple_visual" << endl;
577 | }
578 | if (vm.count("simple_collision")) {
579 | use_simple_collision = true;
580 | cerr << ";; Using simple_collision" << endl;
581 | }
582 | if (vm.count("add_gazebo_description")) {
583 | add_gazebo_description = true;
584 | cerr << ";; Adding gazebo description" << endl;
585 | }
586 | if (vm.count("use_transmission_interface")) {
587 | use_transmission_interface = true;
588 | cerr << ";; Using transmission_interface as transmission" << endl;
589 | }
590 | if (vm.count("use_assimp_export")) {
591 | #if defined(ASSIMP_EXPORT_API)
592 | use_assimp_export = true;
593 | #endif
594 | cerr << ";; Use assimp export" << endl;
595 | }
596 | if (vm.count("original_inertia_rotation")) {
597 | rotate_inertia_frame = false;
598 | cerr << ";; Does not rotate inertia frame" << endl;
599 | }
600 | if (vm.count("export_collision_mesh")) {
601 | export_collision_mesh = true;
602 | cerr << ";; erxport collision mesh as STL" << endl;
603 | }
604 | if (vm.count("output_file")) {
605 | vector aa = vm["output_file"].as< vector >();
606 | cerr << ";; output file is: "
607 | << aa[0] << endl;
608 | output_file = aa[0];
609 | }
610 | if (vm.count("robot_name")) {
611 | vector aa = vm["robot_name"].as< vector >();
612 | cerr << ";; robot_name is: "
613 | << aa[0] << endl;
614 | arobot_name = aa[0];
615 | }
616 | if (vm.count("mesh_prefix")) {
617 | vector aa = vm["mesh_prefix"].as< vector >();
618 | cerr << ";; mesh_prefix is: "
619 | << aa[0] << endl;
620 | mesh_prefix = aa[0];
621 | }
622 | if (vm.count("mesh_output_dir")) {
623 | vector aa = vm["mesh_output_dir"].as< vector >();
624 | cerr << ";; Mesh output directory is: "
625 | << aa[0] << endl;
626 | mesh_dir = aa[0];
627 | // check directory existence
628 | boost::filesystem::path mpath( mesh_dir );
629 | try {
630 | if ( ! boost::filesystem::is_directory(mpath) ) {
631 | boost::filesystem::create_directory ( mpath );
632 | }
633 | }
634 | catch ( boost::filesystem::filesystem_error e ) {
635 | cerr << ";; mesh output directory error / " << e.what() << endl;
636 | return 1;
637 | }
638 | }
639 | if (vm.count("input_file")) {
640 | vector aa = vm["input_file"].as< vector >();
641 | cerr << ";; Input file is: "
642 | << aa[0] << endl;
643 | inputfile = aa[0];
644 | }
645 |
646 | if(inputfile == "") {
647 | cerr << desc << endl;
648 | return 1;
649 | }
650 |
651 | std::string xml_string;
652 | std::fstream xml_file(inputfile.c_str(), std::fstream::in);
653 | while ( xml_file.good() )
654 | {
655 | std::string line;
656 | std::getline( xml_file, line);
657 | xml_string += (line + "\n");
658 | }
659 | xml_file.close();
660 |
661 | urdf::ModelInterfaceSharedPtr robot;
662 | if( xml_string.find("getName();
679 | }
680 | if (output_file == "") {
681 | output_file = arobot_name + ".urdf";
682 | }
683 | printTreeXML (robot->getRoot(), arobot_name, output_file);
684 |
685 | return 0;
686 | }
687 |
--------------------------------------------------------------------------------
/collada_urdf/src/collada_urdf.cpp:
--------------------------------------------------------------------------------
1 | // -*- coding: utf-8 -*-
2 | /*********************************************************************
3 | * Software License Agreement (BSD License)
4 | *
5 | * Copyright (c) 2010, Willow Garage, Inc., University of Tokyo
6 | * All rights reserved.
7 | *
8 | * Redistribution and use in source and binary forms, with or without
9 | * modification, are permitted provided that the following conditions
10 | * are met:
11 | *
12 | * * Redstributions of source code must retain the above copyright
13 | * notice, this list of conditions and the following disclaimer.
14 | * * Redistributions in binary form must reproduce the above
15 | * copyright notice, this list of conditions and the following
16 | * disclaimer in the documentation and/or other materials provided
17 | * with the distribution.
18 | * * Neither the name of the Willow Garage nor the names of its
19 | * contributors may be used to endorse or promote products derived
20 | * from this software without specific prior written permission.
21 | *
22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 | * POSSIBILITY OF SUCH DAMAGE.
34 | *********************************************************************/
35 |
36 | /* Authors: Rosen Diankov, Tim Field */
37 |
38 | #include "collada_urdf/collada_urdf.h"
39 |
40 | #include
41 | #include