add collada_to_urdf.cpp for converting collada file to urdf file
This commit is contained in:
YoheiKakiuchi
parent
334bb56453
commit
0bdfef0985
|
|
@ -12,7 +12,7 @@ catkin_package(
|
||||||
|
|
||||||
include_directories(include)
|
include_directories(include)
|
||||||
|
|
||||||
find_package(Boost REQUIRED COMPONENTS system)
|
find_package(Boost REQUIRED COMPONENTS system filesystem program_options)
|
||||||
include_directories(${Boost_INCLUDE_DIR})
|
include_directories(${Boost_INCLUDE_DIR})
|
||||||
|
|
||||||
find_package(assimp QUIET)
|
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}
|
target_link_libraries(urdf_to_collada ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES}
|
||||||
${Boost_LIBRARIES} ${PROJECT_NAME})
|
${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)
|
catkin_add_gtest(test_collada_writer test/test_collada_urdf.cpp)
|
||||||
target_link_libraries(test_collada_writer ${PROJECT_NAME} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES}
|
target_link_libraries(test_collada_writer ${PROJECT_NAME} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES}
|
||||||
${Boost_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}
|
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
|
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -25,6 +25,8 @@
|
||||||
<build_depend>collada_parser</build_depend>
|
<build_depend>collada_parser</build_depend>
|
||||||
<build_depend>roscpp</build_depend>
|
<build_depend>roscpp</build_depend>
|
||||||
<build_depend>urdf</build_depend>
|
<build_depend>urdf</build_depend>
|
||||||
|
<build_depend>urdfdom</build_depend>
|
||||||
|
<build_depend>urdfdom_headers</build_depend>
|
||||||
<build_depend>geometric_shapes</build_depend>
|
<build_depend>geometric_shapes</build_depend>
|
||||||
<build_depend>cmake_modules</build_depend>
|
<build_depend>cmake_modules</build_depend>
|
||||||
|
|
||||||
|
|
@ -35,6 +37,8 @@
|
||||||
<run_depend>resource_retriever</run_depend>
|
<run_depend>resource_retriever</run_depend>
|
||||||
<run_depend>roscpp</run_depend>
|
<run_depend>roscpp</run_depend>
|
||||||
<run_depend>urdf</run_depend>
|
<run_depend>urdf</run_depend>
|
||||||
|
<run_depend>urdfdom</run_depend>
|
||||||
|
<run_depend>urdfdom_headers</run_depend>
|
||||||
<run_depend>geometric_shapes</run_depend>
|
<run_depend>geometric_shapes</run_depend>
|
||||||
|
|
||||||
</package>
|
</package>
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,693 @@
|
||||||
|
/* Author: Yohei Kakiuchi */
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <urdf/model.h>
|
||||||
|
|
||||||
|
#include <collada_parser/collada_parser.h>
|
||||||
|
#include <urdf_parser/urdf_parser.h>
|
||||||
|
|
||||||
|
#if IS_ASSIMP3
|
||||||
|
// assimp 3 (assimp_devel)
|
||||||
|
#include <assimp/IOSystem.hpp>
|
||||||
|
#include <assimp/IOStream.hpp>
|
||||||
|
#include <assimp/Importer.hpp>
|
||||||
|
#include <assimp/Exporter.hpp>
|
||||||
|
#include <assimp/postprocess.h>
|
||||||
|
#include <assimp/scene.h>
|
||||||
|
#else
|
||||||
|
// assimp 2
|
||||||
|
#include <assimp.hpp>
|
||||||
|
//#include <assimp/export.hpp>
|
||||||
|
#include <aiScene.h>
|
||||||
|
#include <aiPostProcess.h>
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
#include <fstream>
|
||||||
|
|
||||||
|
#include <boost/program_options.hpp>
|
||||||
|
#include <boost/filesystem/path.hpp>
|
||||||
|
#include <boost/filesystem/operations.hpp>
|
||||||
|
|
||||||
|
#include <tf/LinearMath/Transform.h>
|
||||||
|
#include <tf/LinearMath/Quaternion.h>
|
||||||
|
|
||||||
|
#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<const Link> link, ofstream& os)
|
||||||
|
{
|
||||||
|
os << " <link name=\"" << link->name << "\">" << endl;
|
||||||
|
if ( !!link->visual ) {
|
||||||
|
os << " <visual>" << endl;
|
||||||
|
|
||||||
|
if (!use_simple_visual) {
|
||||||
|
os << " <origin ";
|
||||||
|
PRINT_ORIGIN_XML(os, link->visual->origin);
|
||||||
|
os << "/>" << endl;
|
||||||
|
os << " <geometry>" << 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 << " <mesh filename=\"" << mesh_prefix + "/" + link->name + "_mesh.dae" << "\" scale=\"1 1 1\" />" << endl;
|
||||||
|
} else {
|
||||||
|
os << " <mesh filename=\"" << "file://" << ofname << "\" scale=\"1 1 1\" />" << endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
os << " </geometry>" << 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 << " <origin ";
|
||||||
|
urdf::Pose pp = link->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 << " <geometry>" << endl;
|
||||||
|
os << " <box size=\"" << bx - ax << " " << by - ay << " " << bz - az << "\"/>" << endl;
|
||||||
|
os << " </geometry>" << endl;
|
||||||
|
}
|
||||||
|
os << " </visual>" << endl;
|
||||||
|
}
|
||||||
|
if ( !!link->collision ) {
|
||||||
|
os << " <collision>" << endl;
|
||||||
|
if (!use_simple_collision) {
|
||||||
|
os << " <origin ";
|
||||||
|
PRINT_ORIGIN_XML(os, link->collision->origin);
|
||||||
|
os << "/>" << endl;
|
||||||
|
os << " <geometry>" << 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 << " <mesh filename=\"" << mesh_prefix + "/" + oofname;
|
||||||
|
} else {
|
||||||
|
os << " <mesh filename=\"" << "file://" << ofname;
|
||||||
|
}
|
||||||
|
os << "\" scale=\"1 1 1\" />" << endl;
|
||||||
|
}
|
||||||
|
os << " </geometry>" << 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 << " <origin ";
|
||||||
|
urdf::Pose pp = link->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 << " <geometry>" << endl;
|
||||||
|
os << " <box size=\"" << bx - ax << " " << by - ay << " " << bz - az << "\"/>" << endl;
|
||||||
|
os << " </geometry>" << endl;
|
||||||
|
}
|
||||||
|
os << " </collision>" << endl;
|
||||||
|
}
|
||||||
|
if ( !!link->inertial ) {
|
||||||
|
if (!rotate_inertia_frame) {
|
||||||
|
os << " <inertial>" << endl;
|
||||||
|
os << " <mass value=\"" << link->inertial->mass << "\" />" << endl;
|
||||||
|
os << " <origin ";
|
||||||
|
PRINT_ORIGIN_XML(os, link->inertial->origin);
|
||||||
|
os << "/>" << endl;
|
||||||
|
os << " <inertia ixx=\"" << link->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 << " </inertial>" << endl;
|
||||||
|
} else {
|
||||||
|
// rotation should be identity
|
||||||
|
os << " <inertial>" << endl;
|
||||||
|
os << " <mass value=\"" << link->inertial->mass << "\" />" << endl;
|
||||||
|
os << " <origin ";
|
||||||
|
|
||||||
|
tf::Quaternion qt (link->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 << " <inertia ixx=\"" << imat[0][0] << "\" ";
|
||||||
|
os << "ixy=\"" << imat[0][1] << "\" ";
|
||||||
|
os << "ixz=\"" << imat[0][2] << "\" ";
|
||||||
|
os << "iyy=\"" << imat[1][1] << "\" ";
|
||||||
|
os << "iyz=\"" << imat[1][2] << "\" ";
|
||||||
|
os << "izz=\"" << imat[2][2] << "\"/>" << endl;
|
||||||
|
os << " </inertial>" << endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
os << " </link>" << endl;
|
||||||
|
|
||||||
|
#ifdef GAZEBO_1_0
|
||||||
|
if ( add_gazebo_description ) {
|
||||||
|
os << " <gazebo reference=\"" << link->name << "\">" << endl;
|
||||||
|
os << " <material>Gazebo/Grey</material>" << endl;
|
||||||
|
//os << " <mu1>0.9</mu1>" << endl;
|
||||||
|
//os << " <mu2>0.9</mu2>" << endl;
|
||||||
|
os << " <turnGravityOff>false</turnGravityOff>" << endl;
|
||||||
|
os << " </gazebo>" << endl;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef GAZEBO_1_3
|
||||||
|
if ( add_gazebo_description ) {
|
||||||
|
os << " <gazebo reference=\"" << link->name << "\">" << endl;
|
||||||
|
os << " <mu1>0.9</mu1>" << endl;
|
||||||
|
os << " <mu2>0.9</mu2>" << endl;
|
||||||
|
os << " </gazebo>" << endl;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
|
||||||
|
addChildLinkNamesXML(*child, os);
|
||||||
|
}
|
||||||
|
|
||||||
|
void addChildJointNamesXML(boost::shared_ptr<const Link> link, ofstream& os)
|
||||||
|
{
|
||||||
|
double r, p, y;
|
||||||
|
for (std::vector<boost::shared_ptr<Link> >::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 << " <joint name=\"" << (*child)->parent_joint->name << "\" type=\"" << jtype << "\">" << endl;
|
||||||
|
os << " <parent link=\"" << link->name << "\"/>" << endl;
|
||||||
|
os << " <child link=\"" << (*child)->name << "\"/>" << endl;
|
||||||
|
os << " <origin xyz=\"" << (*child)->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 << " <axis xyz=\"" << (*child)->parent_joint->axis.x << " ";
|
||||||
|
os << (*child)->parent_joint->axis.y << " " << (*child)->parent_joint->axis.z << "\"/>" << endl;
|
||||||
|
{
|
||||||
|
boost::shared_ptr<urdf::Joint> jt((*child)->parent_joint);
|
||||||
|
|
||||||
|
if ( !!jt->limits ) {
|
||||||
|
os << " <limit ";
|
||||||
|
os << "lower=\"" << jt->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 ";
|
||||||
|
os << "damping=\"" << jt->dynamics->damping << "\"";
|
||||||
|
os << " friction=\"" << jt->dynamics->friction << "\"";
|
||||||
|
os << " />" << endl;
|
||||||
|
} else {
|
||||||
|
os << " <dynamics ";
|
||||||
|
os << "damping=\"0.2\"";
|
||||||
|
os << " friction=\"0\"";
|
||||||
|
os << " />" << endl;
|
||||||
|
}
|
||||||
|
#ifdef GAZEBO_1_3
|
||||||
|
#if 0
|
||||||
|
os << " <safety_controller";
|
||||||
|
os << " k_position=\"10\"";
|
||||||
|
os << " k_velocity=\"10\"";
|
||||||
|
os << " soft_lower_limit=\"-10\"";
|
||||||
|
os << " soft_upper_limit=\"10\"";
|
||||||
|
os << "/>" << endl;
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
os << " </joint>" << endl;
|
||||||
|
|
||||||
|
if ( add_gazebo_description ) {
|
||||||
|
os << " <transmission type=\"pr2_mechanism_model/SimpleTransmission\" name=\"";
|
||||||
|
os << (*child)->parent_joint->name << "_trans\" >" << endl;
|
||||||
|
os << " <actuator name=\"" << (*child)->parent_joint->name << "_motor\" />" << endl;
|
||||||
|
os << " <joint name=\"" << (*child)->parent_joint->name << "\" />" << endl;
|
||||||
|
os << " <mechanicalReduction>1</mechanicalReduction>" << endl;
|
||||||
|
//os << " <motorTorqueConstant>1</motorTorqueConstant>" << endl;
|
||||||
|
//os << " <pulsesPerRevolution>90000</pulsesPerRevolution>" << endl;
|
||||||
|
os << " </transmission>" << endl;
|
||||||
|
#ifdef GAZEBO_1_3
|
||||||
|
os << " <gazebo reference=\"" << (*child)->parent_joint->name << "\">" << endl;
|
||||||
|
os << " <cfmDamping>0.4</cfmDamping>" << endl;
|
||||||
|
os << " </gazebo>" << endl;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
addChildJointNamesXML(*child, os);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void printTreeXML(boost::shared_ptr<const Link> link, string name, string file)
|
||||||
|
{
|
||||||
|
std::ofstream os;
|
||||||
|
os.open(file.c_str());
|
||||||
|
os << "<?xml version=\"1.0\"?>" << endl;
|
||||||
|
os << "<robot name=\"" << name << "\"" << endl;
|
||||||
|
os << " xmlns:xi=\"http://www.w3.org/2001/XInclude\"" << endl;
|
||||||
|
os << " xmlns:gazebo=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#gz\"" << endl;
|
||||||
|
os << " xmlns:model=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#model\"" << endl;
|
||||||
|
os << " xmlns:sensor=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor\"" << endl;
|
||||||
|
os << " xmlns:body=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#body\"" << endl;
|
||||||
|
os << " xmlns:geom=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#geom\"" << endl;
|
||||||
|
os << " xmlns:joint=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#joint\"" << endl;
|
||||||
|
os << " xmlns:interface=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#interface\"" << endl;
|
||||||
|
os << " xmlns:rendering=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering\"" << endl;
|
||||||
|
os << " xmlns:renderable=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable\"" << endl;
|
||||||
|
os << " xmlns:controller=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#controller\"" << endl;
|
||||||
|
os << " xmlns:physics=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#physics\">" << endl;
|
||||||
|
|
||||||
|
addChildLinkNamesXML(link, os);
|
||||||
|
|
||||||
|
addChildJointNamesXML(link, os);
|
||||||
|
|
||||||
|
if ( add_gazebo_description ) {
|
||||||
|
#ifdef GAZEBO_1_0
|
||||||
|
// old gazebo (gazebo on ROS Fuerte)
|
||||||
|
os << " <gazebo>" << endl;
|
||||||
|
os << " <controller:gazebo_ros_controller_manager" << endl;
|
||||||
|
os << " name=\"gazebo_ros_controller_manager\"" << endl;
|
||||||
|
os << " plugin=\"libgazebo_ros_controller_manager.so\">" << endl;
|
||||||
|
os << " <alwaysOn>true</alwaysOn>" << endl;
|
||||||
|
os << " <updateRate>1000.0</updateRate>" << endl;
|
||||||
|
os << " </controller:gazebo_ros_controller_manager>" << endl;
|
||||||
|
os << " </gazebo>" << endl;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
os << "</robot>" << 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<string> >(), "output robot name")
|
||||||
|
("mesh_output_dir", po::value< vector<string> >(), "directory for outputing")
|
||||||
|
("mesh_prefix", po::value< vector<string> >(), "prefix of mesh files")
|
||||||
|
("output_file,O", po::value< vector<string> >(), "output file")
|
||||||
|
("input_file", po::value< vector<string> >(), "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<string> aa = vm["output_file"].as< vector<string> >();
|
||||||
|
cerr << ";; output file is: "
|
||||||
|
<< aa[0] << endl;
|
||||||
|
output_file = aa[0];
|
||||||
|
}
|
||||||
|
if (vm.count("robot_name")) {
|
||||||
|
vector<string> aa = vm["robot_name"].as< vector<string> >();
|
||||||
|
cerr << ";; robot_name is: "
|
||||||
|
<< aa[0] << endl;
|
||||||
|
arobot_name = aa[0];
|
||||||
|
}
|
||||||
|
if (vm.count("mesh_prefix")) {
|
||||||
|
vector<string> aa = vm["mesh_prefix"].as< vector<string> >();
|
||||||
|
cerr << ";; mesh_prefix is: "
|
||||||
|
<< aa[0] << endl;
|
||||||
|
mesh_prefix = aa[0];
|
||||||
|
}
|
||||||
|
if (vm.count("mesh_output_dir")) {
|
||||||
|
vector<string> aa = vm["mesh_output_dir"].as< vector<string> >();
|
||||||
|
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<string> aa = vm["input_file"].as< vector<string> >();
|
||||||
|
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<ModelInterface> robot;
|
||||||
|
if( xml_string.find("<COLLADA") != std::string::npos )
|
||||||
|
{
|
||||||
|
ROS_DEBUG("Parsing robot collada xml string");
|
||||||
|
robot = parseCollada(xml_string);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
ROS_DEBUG("Parsing robot urdf xml string");
|
||||||
|
robot = parseURDF(xml_string);
|
||||||
|
}
|
||||||
|
if (!robot){
|
||||||
|
std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (arobot_name == "") {
|
||||||
|
arobot_name = robot->getName();
|
||||||
|
}
|
||||||
|
if (output_file == "") {
|
||||||
|
output_file = arobot_name + ".urdf";
|
||||||
|
}
|
||||||
|
printTreeXML (robot->getRoot(), arobot_name, output_file);
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue