From 0bdfef0985a554bedddfaf48cd0b3b85c315f930 Mon Sep 17 00:00:00 2001 From: YoheiKakiuchi Date: Tue, 27 Aug 2013 17:34:35 +0900 Subject: [PATCH 1/2] add collada_to_urdf.cpp for converting collada file to urdf file --- collada_urdf/CMakeLists.txt | 9 +- collada_urdf/package.xml | 4 + collada_urdf/src/collada_to_urdf.cpp | 693 +++++++++++++++++++++++++++ 3 files changed, 704 insertions(+), 2 deletions(-) create mode 100644 collada_urdf/src/collada_to_urdf.cpp diff --git a/collada_urdf/CMakeLists.txt b/collada_urdf/CMakeLists.txt index f0ae921..a5c0456 100644 --- a/collada_urdf/CMakeLists.txt +++ b/collada_urdf/CMakeLists.txt @@ -12,7 +12,7 @@ catkin_package( include_directories(include) -find_package(Boost REQUIRED COMPONENTS system) +find_package(Boost REQUIRED COMPONENTS system filesystem program_options) include_directories(${Boost_INCLUDE_DIR}) find_package(assimp QUIET) @@ -60,11 +60,16 @@ add_executable(urdf_to_collada src/urdf_to_collada.cpp) target_link_libraries(urdf_to_collada ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} ${Boost_LIBRARIES} ${PROJECT_NAME}) +add_executable(collada_to_urdf src/collada_to_urdf.cpp) +target_link_libraries(collada_to_urdf ${ASSIMP_LIBRARIES} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} ${Boost_LIBRARIES}) +set_target_properties(collada_to_urdf PROPERTIES COMPILER_FLAGS "${ASSIMP_CXX_FLAGS} ${ASSIMP_CFLAGS_OTHER}") +set_target_properties(collada_to_urdf PROPERTIES LINK_FLAGS "${ASSIMP_LINK_FLAGS}") + catkin_add_gtest(test_collada_writer test/test_collada_urdf.cpp) target_link_libraries(test_collada_writer ${PROJECT_NAME} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${PROJECT_NAME} urdf_to_collada +install(TARGETS ${PROJECT_NAME} urdf_to_collada collada_to_urdf LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/collada_urdf/package.xml b/collada_urdf/package.xml index cd47f1b..2cc5a06 100644 --- a/collada_urdf/package.xml +++ b/collada_urdf/package.xml @@ -25,6 +25,8 @@ collada_parser roscpp urdf + urdfdom + urdfdom_headers geometric_shapes cmake_modules @@ -35,6 +37,8 @@ resource_retriever roscpp urdf + urdfdom + urdfdom_headers geometric_shapes diff --git a/collada_urdf/src/collada_to_urdf.cpp b/collada_urdf/src/collada_to_urdf.cpp new file mode 100644 index 0000000..4e88c7e --- /dev/null +++ b/collada_urdf/src/collada_to_urdf.cpp @@ -0,0 +1,693 @@ +/* Author: Yohei Kakiuchi */ +#include +#include + +#include +#include + +#if IS_ASSIMP3 +// assimp 3 (assimp_devel) +#include +#include +#include +#include +#include +#include +#else +// assimp 2 +#include +//#include +#include +#include +#endif + +#include +#include + +#include +#include +#include + +#include +#include + +#undef GAZEBO_1_0 +#undef GAZEBO_1_3 + +//#define GAZEBO_1_0 +#define GAZEBO_1_3 + +using namespace urdf; +using namespace std; + +bool use_simple_visual = false; +bool use_simple_collision = false; +bool add_gazebo_description = false; +bool use_assimp_export = false; +bool use_same_collision_as_visual = true; +bool rotate_inertia_frame = true; +bool export_collision_mesh = false; + +string mesh_dir = "/tmp"; +string arobot_name = ""; +string output_file = ""; +string mesh_prefix = ""; + +#define PRINT_ORIGIN(os, origin) \ +os << "xyz: " << origin.position.x << " " << origin.position.y << " " << origin.position.z << " "; \ +{ double r,p,y; origin.rotation.getRPY(r, p, y); \ + os << "rpy: " << r << " " << p << " " << y; } + +#define PRINT_ORIGIN_XML(os, origin) \ + os << "xyz=\"" << origin.position.x << " " << origin.position.y << " " << origin.position.z << "\""; \ + { double h___r, h___p, h___y; \ + origin.rotation.getRPY(h___r, h___p, h___y); \ + os << " rpy=\"" << h___r << " " << h___p << " " << h___y << "\""; } + +#define PRINT_GEOM(os, geometry) \ + if ( geometry->type == urdf::Geometry::MESH ) { os << "geom: name: " << ((urdf::Mesh *)geometry.get())->filename; } + +void assimp_file_export(std::string fname, std::string ofname, + std::string mesh_type = "collada") { +#if IS_ASSIMP3 + if (fname.find("file://") == 0) { + fname.erase(0, strlen("file://")); + } + Assimp::Importer importer; + { // ignore UP_DIRECTION tag in collada + bool existing; + importer.SetPropertyBool(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, true, &existing); + if(existing) { + fprintf(stderr, ";; OverWrite : Ignore UP_DIRECTION", existing); + } + } + const aiScene* scene = importer.ReadFile(fname.c_str(), + aiProcess_Triangulate | + aiProcess_GenNormals | + aiProcess_JoinIdenticalVertices | + aiProcess_SplitLargeMeshes | + aiProcess_OptimizeMeshes | + aiProcess_SortByPType); + + if (!scene) { + std::string str( importer.GetErrorString() ); + std::cerr << ";; " << str << std::endl; + return; + } + + Assimp::Exporter aexpt; + aiReturn ret = aexpt.Export(scene, mesh_type, ofname); + if ( ret != AI_SUCCESS ) { + std::string str( "assimp error" ); + std::cerr << ";; " << str << std::endl; + } +#endif +} + +// assimp bounding box calculation +void assimp_calc_bbox(string fname, float &minx, float &miny, float &minz, + float &maxx, float &maxy, float &maxz) { + + if (fname.find("file://") == 0) { + fname.erase(0, strlen("file://")); + } + + Assimp::Importer importer; + const aiScene* scene = importer.ReadFile(fname.c_str(), + aiProcess_Triangulate | + aiProcess_JoinIdenticalVertices | + aiProcess_SortByPType); // aiProcess_GenNormals + // aiProcess_GenSmoothNormals + // aiProcess_SplitLargeMeshes + if (!scene) { + std::string str( importer.GetErrorString() ); + std::cerr << ";; " << str << std::endl; + return; + } + + aiNode *node = scene->mRootNode; + + bool found = false; + if(node->mNumMeshes > 0 && node->mMeshes != NULL) { + std::cerr << "Root node has meshes " << node->mMeshes << std::endl;; + found = true; + } else { + for (unsigned int i=0; i < node->mNumChildren; ++i) { + if(node->mChildren[i]->mNumMeshes > 0 && node->mChildren[i]->mMeshes != NULL) { + std::cerr << "Child " << i << " has meshes" << std::endl; + node = node->mChildren[i]; + found = true; + break; + } + } + } + if(found == false) { + std::cerr << "Can't find meshes in file" << std::endl; + return; + } + + aiMatrix4x4 transform = node->mTransformation; + + // copy vertices + maxx = maxy = maxz = -100000000.0; + minx = miny = minz = 100000000.0; + + std::cerr << ";; num meshes: " << node->mNumMeshes << std::endl; + for (unsigned int m = 0; m < node->mNumMeshes; m++) { + aiMesh *a = scene->mMeshes[node->mMeshes[m]]; + std::cerr << ";; num vertices: " << a->mNumVertices << std::endl; + + for (unsigned int i = 0 ; i < a->mNumVertices ; ++i) { + aiVector3D p; + p.x = a->mVertices[i].x; + p.y = a->mVertices[i].y; + p.z = a->mVertices[i].z; + p *= transform; + + if ( maxx < p.x ) { + maxx = p.x; + } + if ( maxy < p.y ) { + maxy = p.y; + } + if ( maxz < p.z ) { + maxz = p.z; + } + if ( minx > p.x ) { + minx = p.x; + } + if ( miny > p.y ) { + miny = p.y; + } + if ( minz > p.z ) { + minz = p.z; + } + } + } +} + +void addChildLinkNamesXML(boost::shared_ptr link, ofstream& os) +{ + os << " name << "\">" << endl; + if ( !!link->visual ) { + os << " " << endl; + + if (!use_simple_visual) { + os << " visual->origin); + os << "/>" << endl; + os << " " << endl; + if ( link->visual->geometry->type == urdf::Geometry::MESH ) { + std::string ifname (((urdf::Mesh *)link->visual->geometry.get())->filename); + if (ifname.find("file://") == 0) { + ifname.erase(0, strlen("file://")); + } + std::string ofname (mesh_dir + "/" + link->name + "_mesh.dae"); + + if (use_assimp_export) { + // using collada export + assimp_file_export (ifname, ofname); + } else { + // copy to ofname + std::ofstream tmp_os; + tmp_os.open(ofname.c_str()); + std::ifstream is; + is.open(ifname.c_str()); + std::string buf; + while(is && getline(is, buf)) tmp_os << buf << std::endl; + is.close(); + tmp_os.close(); + } + if (mesh_prefix != "") { + os << " name + "_mesh.dae" << "\" scale=\"1 1 1\" />" << endl; + } else { + os << " " << endl; + } + } + os << " " << endl; + } else { + // simple visual + float ax,ay,az,bx,by,bz; + if ( link->visual->geometry->type == urdf::Geometry::MESH ) { + assimp_calc_bbox(((urdf::Mesh *)link->visual->geometry.get())->filename, + ax, ay, az, bx, by, bz); + } + os << " visual->origin; + + pp.position.x += ( ax + bx ) / 2 ; + pp.position.y += ( ay + by ) / 2 ; + pp.position.z += ( az + bz ) / 2 ; + PRINT_ORIGIN_XML(os, pp); + os << "/>" << endl; + + os << " " << endl; + os << " " << endl; + os << " " << endl; + } + os << " " << endl; + } + if ( !!link->collision ) { + os << " " << endl; + if (!use_simple_collision) { + os << " collision->origin); + os << "/>" << endl; + os << " " << endl; + + if ( link->visual->geometry->type == urdf::Geometry::MESH ) { + std::string ifname; + if (use_same_collision_as_visual) { + ifname.assign (((urdf::Mesh *)link->visual->geometry.get())->filename); + } else { + ifname.assign (((urdf::Mesh *)link->collision->geometry.get())->filename); + } + if (ifname.find("file://") == 0) { + ifname.erase(0, strlen("file://")); + } + std::string oofname; + if (export_collision_mesh) { + oofname = link->name + "_mesh.stl"; + } else { + oofname = link->name + "_mesh.dae"; + } + std::string ofname = (mesh_dir + "/" + oofname); + + if (use_assimp_export) { + // using collada export + if (export_collision_mesh) { + assimp_file_export (ifname, ofname, "stl"); + } else { + assimp_file_export (ifname, ofname); + } + } else { + // copy to ofname + std::ofstream tmp_os; + tmp_os.open(ofname.c_str()); + std::ifstream is; + is.open(ifname.c_str()); + std::string buf; + while(is && getline(is, buf)) tmp_os << buf << std::endl; + is.close(); + tmp_os.close(); + } + if (mesh_prefix != "") { + os << " " << endl; + } + os << " " << endl; + } else { + // simple collision + float ax,ay,az,bx,by,bz; + if ( link->visual->geometry->type == urdf::Geometry::MESH ) { + assimp_calc_bbox(std::string ( ((urdf::Mesh *)link->visual->geometry.get())->filename ), + ax, ay, az, bx, by, bz); + } + os << " visual->origin; + pp.position.x += ( ax + bx ) / 2 ; + pp.position.y += ( ay + by ) / 2 ; + pp.position.z += ( az + bz ) / 2 ; + PRINT_ORIGIN_XML(os, pp); + os << "/>" << endl; + + os << " " << endl; + os << " " << endl; + os << " " << endl; + } + os << " " << endl; + } + if ( !!link->inertial ) { + if (!rotate_inertia_frame) { + os << " " << endl; + os << " inertial->mass << "\" />" << endl; + os << " inertial->origin); + os << "/>" << endl; + os << " inertial->ixx << "\" "; + os << "ixy=\"" << link->inertial->ixy << "\" "; + os << "ixz=\"" << link->inertial->ixz << "\" "; + os << "iyy=\"" << link->inertial->iyy << "\" "; + os << "iyz=\"" << link->inertial->iyz << "\" "; + os << "izz=\"" << link->inertial->izz << "\"/>" << endl; + os << " " << endl; + } else { + // rotation should be identity + os << " " << endl; + os << " inertial->mass << "\" />" << endl; + os << " inertial->origin.rotation.x, + link->inertial->origin.rotation.y, + link->inertial->origin.rotation.z, + link->inertial->origin.rotation.w); + tf::Matrix3x3 mat (qt); + tf::Matrix3x3 tmat (mat.transpose()); + tf::Matrix3x3 imat (link->inertial->ixx, link->inertial->ixy, link->inertial->ixz, + link->inertial->ixy, link->inertial->iyy, link->inertial->iyz, + link->inertial->ixz, link->inertial->iyz, link->inertial->izz); +#define DEBUG_MAT(mat) \ + cout << "#2f((" << mat[0][0] << " " << mat[0][1] << " " << mat[0][2] << ")"; \ + cout << "(" << mat[1][0] << " " << mat[1][1] << " " << mat[1][2] << ")"; \ + cout << "(" << mat[2][0] << " " << mat[2][1] << " " << mat[2][2] << "))" << endl; + +#if DEBUG + DEBUG_MAT(mat); + DEBUG_MAT(tmat); + DEBUG_MAT(imat); +#endif + + imat = ( mat * imat * tmat ); + +#if DEBUG + DEBUG_MAT(imat); +#endif + + urdf::Pose t_pose (link->inertial->origin); + t_pose.rotation.clear(); + + PRINT_ORIGIN_XML(os, t_pose); + os << "/>" << endl; + + os << " " << endl; + os << " " << endl; + } + } + os << " " << endl; + +#ifdef GAZEBO_1_0 + if ( add_gazebo_description ) { + os << " name << "\">" << endl; + os << " Gazebo/Grey" << endl; + //os << " 0.9" << endl; + //os << " 0.9" << endl; + os << " false" << endl; + os << " " << endl; + } +#endif + +#ifdef GAZEBO_1_3 + if ( add_gazebo_description ) { + os << " name << "\">" << endl; + os << " 0.9" << endl; + os << " 0.9" << endl; + os << " " << endl; + } +#endif + + for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) + addChildLinkNamesXML(*child, os); +} + +void addChildJointNamesXML(boost::shared_ptr link, ofstream& os) +{ + double r, p, y; + for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){ + (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y); + std::string jtype; + if ( (*child)->parent_joint->type == urdf::Joint::UNKNOWN ) { + jtype = std::string("unknown"); + } else if ( (*child)->parent_joint->type == urdf::Joint::REVOLUTE ) { + jtype = std::string("revolute"); + } else if ( (*child)->parent_joint->type == urdf::Joint::CONTINUOUS ) { + jtype = std::string("continuous"); + } else if ( (*child)->parent_joint->type == urdf::Joint::PRISMATIC ) { + jtype = std::string("prismatic"); + } else if ( (*child)->parent_joint->type == urdf::Joint::FLOATING ) { + jtype = std::string("floating"); + } else if ( (*child)->parent_joint->type == urdf::Joint::PLANAR ) { + jtype = std::string("planar"); + } else if ( (*child)->parent_joint->type == urdf::Joint::FIXED ) { + jtype = std::string("fixed"); + } else { + ///error + } + + os << " parent_joint->name << "\" type=\"" << jtype << "\">" << endl; + os << " name << "\"/>" << endl; + os << " name << "\"/>" << endl; + os << " parent_joint->parent_to_joint_origin_transform.position.x << " "; + os << (*child)->parent_joint->parent_to_joint_origin_transform.position.y << " "; + os << (*child)->parent_joint->parent_to_joint_origin_transform.position.z; + os << "\" rpy=\"" << r << " " << p << " " << y << " " << "\"/>" << endl; + os << " parent_joint->axis.x << " "; + os << (*child)->parent_joint->axis.y << " " << (*child)->parent_joint->axis.z << "\"/>" << endl; + { + boost::shared_ptr jt((*child)->parent_joint); + + if ( !!jt->limits ) { + os << " limits->lower << "\""; + os << " upper=\"" << jt->limits->upper << "\""; + if (jt->limits->effort == 0.0) { + os << " effort=\"100\""; + } else { + os << " effort=\"" << jt->limits->effort << "\""; + } + os << " velocity=\"" << jt->limits->velocity << "\""; + os << " />" << endl; + } + if ( !!jt->dynamics ) { + os << " dynamics->damping << "\""; + os << " friction=\"" << jt->dynamics->friction << "\""; + os << " />" << endl; + } else { + os << " " << endl; + } +#ifdef GAZEBO_1_3 +#if 0 + os << " " << endl; +#endif +#endif + } + + os << " " << endl; + + if ( add_gazebo_description ) { + os << " parent_joint->name << "_trans\" >" << endl; + os << " parent_joint->name << "_motor\" />" << endl; + os << " parent_joint->name << "\" />" << endl; + os << " 1" << endl; + //os << " 1" << endl; + //os << " 90000" << endl; + os << " " << endl; +#ifdef GAZEBO_1_3 + os << " parent_joint->name << "\">" << endl; + os << " 0.4" << endl; + os << " " << endl; +#endif + } + addChildJointNamesXML(*child, os); + } +} + +void printTreeXML(boost::shared_ptr link, string name, string file) +{ + std::ofstream os; + os.open(file.c_str()); + os << "" << endl; + os << "" << endl; + + addChildLinkNamesXML(link, os); + + addChildJointNamesXML(link, os); + + if ( add_gazebo_description ) { +#ifdef GAZEBO_1_0 + // old gazebo (gazebo on ROS Fuerte) + os << " " << endl; + os << " " << endl; + os << " true" << endl; + os << " 1000.0" << endl; + os << " " << endl; + os << " " << endl; +#endif + } + + os << "" << endl; + os.close(); +} + +namespace po = boost::program_options; +// using namespace std; + +int main(int argc, char** argv) +{ + string inputfile; + + po::options_description desc("Usage: collada_to_urdf input.dae [options]\n Options for collada_to_urdf"); + desc.add_options() + ("help", "produce help message") + ("simple_visual,V", "use bounding box for visual") + ("simple_collision,C", "use bounding box for collision") + ("export_collision_mesh", "export collision mesh as STL") + ("add_gazebo_description,G", "add description for using on gazebo") + ("use_assimp_export,A", "use assimp library for exporting mesh") + ("use_collision,U", "use collision geometry (default collision is the same as visual)") + ("original_inertia_rotation,R", "does not rotate inertia frame") + ("robot_name,N", po::value< vector >(), "output robot name") + ("mesh_output_dir", po::value< vector >(), "directory for outputing") + ("mesh_prefix", po::value< vector >(), "prefix of mesh files") + ("output_file,O", po::value< vector >(), "output file") + ("input_file", po::value< vector >(), "input file") + ; + + po::positional_options_description p; + p.add("input_file", -1); + + po::variables_map vm; + try { + po::store(po::command_line_parser(argc, argv). + options(desc).positional(p).run(), vm); + po::notify(vm); + } + catch (po::error e) { + cerr << ";; option parse error / " << e.what() << endl; + return 1; + } + + if (vm.count("help")) { + cout << desc << "\n"; + return 1; + } + if (vm.count("simple_visual")) { + use_simple_visual = true; + cerr << ";; Using simple_visual" << endl; + } + if (vm.count("simple_collision")) { + use_simple_collision = true; + cerr << ";; Using simple_collision" << endl; + } + if (vm.count("add_gazebo_description")) { + add_gazebo_description = true; + cerr << ";; Adding gazebo description" << endl; + } + if (vm.count("use_assimp_export")) { +#if IS_ASSIMP3 + use_assimp_export = true; +#endif + cerr << ";; Use assimp export" << endl; + } + if (vm.count("original_inertia_rotation")) { + rotate_inertia_frame = false; + cerr << ";; Does not rotate inertia frame" << endl; + } + if (vm.count("export_collision_mesh")) { + export_collision_mesh = true; + cerr << ";; erxport collision mesh as STL" << endl; + } + if (vm.count("output_file")) { + vector aa = vm["output_file"].as< vector >(); + cerr << ";; output file is: " + << aa[0] << endl; + output_file = aa[0]; + } + if (vm.count("robot_name")) { + vector aa = vm["robot_name"].as< vector >(); + cerr << ";; robot_name is: " + << aa[0] << endl; + arobot_name = aa[0]; + } + if (vm.count("mesh_prefix")) { + vector aa = vm["mesh_prefix"].as< vector >(); + cerr << ";; mesh_prefix is: " + << aa[0] << endl; + mesh_prefix = aa[0]; + } + if (vm.count("mesh_output_dir")) { + vector aa = vm["mesh_output_dir"].as< vector >(); + cerr << ";; Mesh output directory is: " + << aa[0] << endl; + mesh_dir = aa[0]; + // check directory existence + boost::filesystem::path mpath( mesh_dir ); + try { + if ( ! boost::filesystem::is_directory(mpath) ) { + boost::filesystem::create_directory ( mpath ); + } + } + catch ( boost::filesystem::filesystem_error e ) { + cerr << ";; mesh output directory error / " << e.what() << endl; + return 1; + } + } + if (vm.count("input_file")) { + vector aa = vm["input_file"].as< vector >(); + cerr << ";; Input file is: " + << aa[0] << endl; + inputfile = aa[0]; + } + + if(inputfile == "") { + cerr << desc << endl; + return 1; + } + + std::string xml_string; + std::fstream xml_file(inputfile.c_str(), std::fstream::in); + while ( xml_file.good() ) + { + std::string line; + std::getline( xml_file, line); + xml_string += (line + "\n"); + } + xml_file.close(); + + boost::shared_ptr robot; + if( xml_string.find("getName(); + } + if (output_file == "") { + output_file = arobot_name + ".urdf"; + } + printTreeXML (robot->getRoot(), arobot_name, output_file); + + return 0; +} From 9ac73acff67fb59f4c391710535dd888359c5f91 Mon Sep 17 00:00:00 2001 From: YoheiKakiuchi Date: Tue, 27 Aug 2013 17:38:47 +0900 Subject: [PATCH 2/2] fix for using collada_parser_plugin --- collada_parser/package.xml | 4 ++++ urdf/src/model.cpp | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/collada_parser/package.xml b/collada_parser/package.xml index f781d1f..12b2f8c 100644 --- a/collada_parser/package.xml +++ b/collada_parser/package.xml @@ -31,5 +31,9 @@ urdf_parser_plugin class_loader + + + + diff --git a/urdf/src/model.cpp b/urdf/src/model.cpp index ff980de..7a6d4ff 100644 --- a/urdf/src/model.cpp +++ b/urdf/src/model.cpp @@ -153,7 +153,7 @@ bool Model::initString(const std::string& xml_string) const std::vector &classes = PARSER_PLUGIN_LOADER->getDeclaredClasses(); bool found = false; for (std::size_t i = 0 ; i < classes.size() ; ++i) - if (classes[i].find("collada") != std::string::npos) + if (classes[i].find("urdf/ColladaURDFParser") != std::string::npos) { boost::shared_ptr instance = PARSER_PLUGIN_LOADER->createInstance(classes[i]); if (instance)