12 #include <urdf_parser/urdf_parser.h> 14 #if defined(ASSIMP_UNIFIED_HEADER_NAMES) 15 #include <assimp/IOSystem.hpp> 16 #include <assimp/IOStream.hpp> 17 #include <assimp/Importer.hpp> 18 #include <assimp/Exporter.hpp> 19 #include <assimp/postprocess.h> 20 #include <assimp/scene.h> 23 #if defined(ASSIMP_EXPORT_API) 24 #include <assimp/export.hpp> 27 #include <aiPostProcess.h> 30 #include <boost/program_options.hpp> 31 #include <boost/filesystem/path.hpp> 32 #include <boost/filesystem/operations.hpp> 34 #include <Eigen/Geometry> 56 #define PRINT_ORIGIN(os, origin) \ 57 os << "xyz: " << origin.position.x << " " << origin.position.y << " " << origin.position.z << " "; \ 58 { double r,p,y; origin.rotation.getRPY(r, p, y); \ 59 os << "rpy: " << r << " " << p << " " << y; } 61 #define PRINT_ORIGIN_XML(os, origin) \ 62 os << "xyz=\"" << origin.position.x << " " << origin.position.y << " " << origin.position.z << "\""; \ 63 { double h___r, h___p, h___y; \ 64 origin.rotation.getRPY(h___r, h___p, h___y); \ 65 os << " rpy=\"" << h___r << " " << h___p << " " << h___y << "\""; } 67 #define PRINT_GEOM(os, geometry) \ 68 if ( geometry->type == urdf::Geometry::MESH ) { os << "geom: name: " << ((urdf::Mesh *)geometry.get())->filename; } 71 std::string mesh_type =
"collada") {
72 #if defined(ASSIMP_EXPORT_API) 73 if (fname.find(
"file://") == 0) {
74 fname.erase(0, strlen(
"file://"));
76 Assimp::Importer importer;
86 const aiScene* scene = importer.ReadFile(fname.c_str(),
87 aiProcess_Triangulate |
88 aiProcess_GenNormals |
89 aiProcess_JoinIdenticalVertices |
90 aiProcess_SplitLargeMeshes |
91 aiProcess_OptimizeMeshes |
92 aiProcess_SortByPType);
95 std::string str( importer.GetErrorString() );
96 std::cerr <<
";; " << str << std::endl;
100 Assimp::Exporter aexpt;
101 aiReturn ret = aexpt.Export(scene, mesh_type, ofname);
102 if ( ret != AI_SUCCESS ) {
103 std::string str(
"assimp error" );
104 std::cerr <<
";; " << str << std::endl;
111 float &maxx,
float &maxy,
float &maxz) {
113 if (fname.find(
"file://") == 0) {
114 fname.erase(0, strlen(
"file://"));
117 Assimp::Importer importer;
118 const aiScene* scene = importer.ReadFile(fname.c_str(),
119 aiProcess_Triangulate |
120 aiProcess_JoinIdenticalVertices |
121 aiProcess_SortByPType);
125 std::string str( importer.GetErrorString() );
126 std::cerr <<
";; " << str << std::endl;
130 aiNode *node = scene->mRootNode;
133 if(node->mNumMeshes > 0 && node->mMeshes != NULL) {
134 std::cerr <<
"Root node has meshes " << node->mMeshes << std::endl;;
137 for (
unsigned int i=0; i < node->mNumChildren; ++i) {
138 if(node->mChildren[i]->mNumMeshes > 0 && node->mChildren[i]->mMeshes != NULL) {
139 std::cerr <<
"Child " << i <<
" has meshes" << std::endl;
140 node = node->mChildren[i];
147 std::cerr <<
"Can't find meshes in file" << std::endl;
151 aiMatrix4x4 transform = node->mTransformation;
154 maxx = maxy = maxz = -100000000.0;
155 minx = miny = minz = 100000000.0;
157 std::cerr <<
";; num meshes: " << node->mNumMeshes << std::endl;
158 for (
unsigned int m = 0; m < node->mNumMeshes; m++) {
159 aiMesh *a = scene->mMeshes[node->mMeshes[m]];
160 std::cerr <<
";; num vertices: " << a->mNumVertices << std::endl;
162 for (
unsigned int i = 0 ; i < a->mNumVertices ; ++i) {
164 p.x = a->mVertices[i].x;
165 p.y = a->mVertices[i].y;
166 p.z = a->mVertices[i].z;
193 os <<
" <link name=\"" << link->name <<
"\">" << endl;
194 if ( !!link->visual ) {
195 os <<
" <visual>" << endl;
201 os <<
" <geometry>" << endl;
202 if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
203 std::string ifname (((urdf::Mesh *)link->visual->geometry.get())->filename);
204 if (ifname.find(
"file://") == 0) {
205 ifname.erase(0, strlen(
"file://"));
207 std::string ofname (
mesh_dir +
"/" + link->name +
"_mesh.dae");
214 std::ofstream tmp_os;
215 tmp_os.open(ofname.c_str());
217 is.open(ifname.c_str());
219 while(is && getline(is, buf)) tmp_os << buf << std::endl;
224 os <<
" <mesh filename=\"" <<
mesh_prefix +
"/" + link->name +
"_mesh.dae" <<
"\" scale=\"1 1 1\" />" << endl;
226 os <<
" <mesh filename=\"" <<
"file://" << ofname <<
"\" scale=\"1 1 1\" />" << endl;
229 os <<
" </geometry>" << endl;
232 float ax,ay,az,bx,by,bz;
233 if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
235 ax, ay, az, bx, by, bz);
238 urdf::Pose pp = link->visual->origin;
240 pp.position.x += ( ax + bx ) / 2 ;
241 pp.position.y += ( ay + by ) / 2 ;
242 pp.position.z += ( az + bz ) / 2 ;
246 os <<
" <geometry>" << endl;
247 os <<
" <box size=\"" << bx - ax <<
" " << by - ay <<
" " << bz - az <<
"\"/>" << endl;
248 os <<
" </geometry>" << endl;
250 os <<
" </visual>" << endl;
252 if ( !!link->collision ) {
253 os <<
" <collision>" << endl;
258 os <<
" <geometry>" << endl;
260 if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
263 ifname.assign (((urdf::Mesh *)link->visual->geometry.get())->filename);
265 ifname.assign (((urdf::Mesh *)link->collision->geometry.get())->filename);
267 if (ifname.find(
"file://") == 0) {
268 ifname.erase(0, strlen(
"file://"));
272 oofname = link->name +
"_mesh.stl";
274 oofname = link->name +
"_mesh.dae";
276 std::string ofname = (
mesh_dir +
"/" + oofname);
287 std::ofstream tmp_os;
288 tmp_os.open(ofname.c_str());
290 is.open(ifname.c_str());
292 while(is && getline(is, buf)) tmp_os << buf << std::endl;
297 os <<
" <mesh filename=\"" <<
mesh_prefix +
"/" + oofname;
299 os <<
" <mesh filename=\"" <<
"file://" << ofname;
301 os <<
"\" scale=\"1 1 1\" />" << endl;
303 os <<
" </geometry>" << endl;
306 float ax,ay,az,bx,by,bz;
307 if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
308 assimp_calc_bbox(std::string ( ((urdf::Mesh *)link->visual->geometry.get())->filename ),
309 ax, ay, az, bx, by, bz);
312 urdf::Pose pp = link->visual->origin;
313 pp.position.x += ( ax + bx ) / 2 ;
314 pp.position.y += ( ay + by ) / 2 ;
315 pp.position.z += ( az + bz ) / 2 ;
319 os <<
" <geometry>" << endl;
320 os <<
" <box size=\"" << bx - ax <<
" " << by - ay <<
" " << bz - az <<
"\"/>" << endl;
321 os <<
" </geometry>" << endl;
323 os <<
" </collision>" << endl;
325 if ( !!link->inertial ) {
327 os <<
" <inertial>" << endl;
328 os <<
" <mass value=\"" << link->inertial->mass <<
"\" />" << endl;
332 os <<
" <inertia ixx=\"" << link->inertial->ixx <<
"\" ";
333 os <<
"ixy=\"" << link->inertial->ixy <<
"\" ";
334 os <<
"ixz=\"" << link->inertial->ixz <<
"\" ";
335 os <<
"iyy=\"" << link->inertial->iyy <<
"\" ";
336 os <<
"iyz=\"" << link->inertial->iyz <<
"\" ";
337 os <<
"izz=\"" << link->inertial->izz <<
"\"/>" << endl;
338 os <<
" </inertial>" << endl;
341 os <<
" <inertial>" << endl;
342 os <<
" <mass value=\"" << link->inertial->mass <<
"\" />" << endl;
345 Eigen::Quaterniond qt(link->inertial->origin.rotation.w,
346 link->inertial->origin.rotation.x,
347 link->inertial->origin.rotation.y,
348 link->inertial->origin.rotation.z);
349 Eigen::Matrix3d mat(qt);
350 Eigen::Matrix3d tmat(mat.transpose());
351 Eigen::Matrix3d imat;
352 imat(0, 0) = link->inertial->ixx;
353 imat(0, 1) = link->inertial->ixy;
354 imat(0, 2) = link->inertial->ixz;
355 imat(1, 0) = link->inertial->ixy;
356 imat(1, 1) = link->inertial->iyy;
357 imat(1, 2) = link->inertial->iyz;
358 imat(2, 0) = link->inertial->ixz;
359 imat(2, 1) = link->inertial->iyz;
360 imat(2, 2) = link->inertial->izz;
362 #define DEBUG_MAT(mat) \ 363 cout << "#2f((" << mat(0, 0) << " " << mat(0, 1) << " " << mat(0, 2) << ")"; \ 364 cout << "(" << mat(1, 0) << " " << mat(1, 1) << " " << mat(1, 2) << ")"; \ 365 cout << "(" << mat(2, 0) << " " << mat(2, 1) << " " << mat(2, 2) << "))" << endl; 373 imat = ( mat * imat * tmat );
379 urdf::Pose t_pose (link->inertial->origin);
380 t_pose.rotation.clear();
385 os <<
" <inertia ixx=\"" << imat(0, 0) <<
"\" ";
386 os <<
"ixy=\"" << imat(0, 1) <<
"\" ";
387 os <<
"ixz=\"" << imat(0, 2) <<
"\" ";
388 os <<
"iyy=\"" << imat(1, 1) <<
"\" ";
389 os <<
"iyz=\"" << imat(1, 2) <<
"\" ";
390 os <<
"izz=\"" << imat(2, 2) <<
"\"/>" << endl;
391 os <<
" </inertial>" << endl;
394 os <<
" </link>" << endl;
398 os <<
" <gazebo reference=\"" << link->name <<
"\">" << endl;
399 os <<
" <mu1>0.9</mu1>" << endl;
400 os <<
" <mu2>0.9</mu2>" << endl;
401 os <<
" </gazebo>" << endl;
405 for (std::vector<urdf::LinkSharedPtr >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
412 for (std::vector<urdf::LinkSharedPtr >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){
413 (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y);
415 if ( (*child)->parent_joint->type == urdf::Joint::UNKNOWN ) {
416 jtype = std::string(
"unknown");
417 }
else if ( (*child)->parent_joint->type == urdf::Joint::REVOLUTE ) {
418 jtype = std::string(
"revolute");
419 }
else if ( (*child)->parent_joint->type == urdf::Joint::CONTINUOUS ) {
420 jtype = std::string(
"continuous");
421 }
else if ( (*child)->parent_joint->type == urdf::Joint::PRISMATIC ) {
422 jtype = std::string(
"prismatic");
423 }
else if ( (*child)->parent_joint->type == urdf::Joint::FLOATING ) {
424 jtype = std::string(
"floating");
425 }
else if ( (*child)->parent_joint->type == urdf::Joint::PLANAR ) {
426 jtype = std::string(
"planar");
427 }
else if ( (*child)->parent_joint->type == urdf::Joint::FIXED ) {
428 jtype = std::string(
"fixed");
433 os <<
" <joint name=\"" << (*child)->parent_joint->name <<
"\" type=\"" << jtype <<
"\">" << endl;
434 os <<
" <parent link=\"" << link->name <<
"\"/>" << endl;
435 os <<
" <child link=\"" << (*child)->name <<
"\"/>" << endl;
436 os <<
" <origin xyz=\"" << (*child)->parent_joint->parent_to_joint_origin_transform.position.x <<
" ";
437 os << (*child)->parent_joint->parent_to_joint_origin_transform.position.y <<
" ";
438 os << (*child)->parent_joint->parent_to_joint_origin_transform.position.z;
439 os <<
"\" rpy=\"" << r <<
" " << p <<
" " << y <<
" " <<
"\"/>" << endl;
440 os <<
" <axis xyz=\"" << (*child)->parent_joint->axis.x <<
" ";
441 os << (*child)->parent_joint->axis.y <<
" " << (*child)->parent_joint->axis.z <<
"\"/>" << endl;
443 urdf::JointSharedPtr jt((*child)->parent_joint);
445 if ( !!jt->limits ) {
447 os <<
"lower=\"" << jt->limits->lower <<
"\"";
448 os <<
" upper=\"" << jt->limits->upper <<
"\"";
449 if (jt->limits->effort == 0.0) {
450 os <<
" effort=\"100\"";
452 os <<
" effort=\"" << jt->limits->effort <<
"\"";
454 os <<
" velocity=\"" << jt->limits->velocity <<
"\"";
457 if ( !!jt->dynamics ) {
459 os <<
"damping=\"" << jt->dynamics->damping <<
"\"";
460 os <<
" friction=\"" << jt->dynamics->friction <<
"\"";
464 os <<
"damping=\"0.2\"";
465 os <<
" friction=\"0\"";
470 os <<
" <safety_controller";
471 os <<
" k_position=\"10\"";
472 os <<
" k_velocity=\"10\"";
473 os <<
" soft_lower_limit=\"-10\"";
474 os <<
" soft_upper_limit=\"10\"";
480 os <<
" </joint>" << endl;
483 os <<
" <transmission type=\"pr2_mechanism_model/SimpleTransmission\" name=\"";
484 os << (*child)->parent_joint->name <<
"_trans\" >" << endl;
485 os <<
" <actuator name=\"" << (*child)->parent_joint->name <<
"_motor\" />" << endl;
486 os <<
" <joint name=\"" << (*child)->parent_joint->name <<
"\" />" << endl;
487 os <<
" <mechanicalReduction>1</mechanicalReduction>" << endl;
490 os <<
" </transmission>" << endl;
492 os <<
" <gazebo reference=\"" << (*child)->parent_joint->name <<
"\">" << endl;
493 os <<
" <cfmDamping>0.4</cfmDamping>" << endl;
494 os <<
" </gazebo>" << endl;
501 void printTreeXML(urdf::LinkConstSharedPtr link,
string name,
string file)
504 os.open(file.c_str());
505 os <<
"<?xml version=\"1.0\"?>" << endl;
506 os <<
"<robot name=\"" << name <<
"\"" << endl;
507 os <<
" xmlns:xi=\"http://www.w3.org/2001/XInclude\">" << endl;
513 os <<
"</robot>" << endl;
517 namespace po = boost::program_options;
520 int main(
int argc,
char** argv)
524 po::options_description desc(
"Usage: collada_to_urdf input.dae [options]\n Options for collada_to_urdf");
526 (
"help",
"produce help message")
527 (
"simple_visual,V",
"use bounding box for visual")
528 (
"simple_collision,C",
"use bounding box for collision")
529 (
"export_collision_mesh",
"export collision mesh as STL")
530 (
"add_gazebo_description,G",
"add description for using on gazebo")
531 (
"use_assimp_export,A",
"use assimp library for exporting mesh")
532 (
"use_collision,U",
"use collision geometry (default collision is the same as visual)")
533 (
"original_inertia_rotation,R",
"does not rotate inertia frame")
534 (
"robot_name,N", po::value< vector<string> >(),
"output robot name")
535 (
"mesh_output_dir", po::value< vector<string> >(),
"directory for outputing")
536 (
"mesh_prefix", po::value< vector<string> >(),
"prefix of mesh files")
537 (
"output_file,O", po::value< vector<string> >(),
"output file")
538 (
"input_file", po::value< vector<string> >(),
"input file")
541 po::positional_options_description p;
542 p.add(
"input_file", -1);
544 po::variables_map vm;
546 po::store(po::command_line_parser(argc, argv).
547 options(desc).positional(p).
run(), vm);
550 catch (po::error e) {
551 cerr <<
";; option parse error / " << e.what() << endl;
555 if (vm.count(
"help")) {
556 cout << desc <<
"\n";
559 if (vm.count(
"simple_visual")) {
561 cerr <<
";; Using simple_visual" << endl;
563 if (vm.count(
"simple_collision")) {
565 cerr <<
";; Using simple_collision" << endl;
567 if (vm.count(
"add_gazebo_description")) {
569 cerr <<
";; Adding gazebo description" << endl;
571 if (vm.count(
"use_assimp_export")) {
572 #if defined(ASSIMP_EXPORT_API) 575 cerr <<
";; Use assimp export" << endl;
577 if (vm.count(
"original_inertia_rotation")) {
579 cerr <<
";; Does not rotate inertia frame" << endl;
581 if (vm.count(
"export_collision_mesh")) {
583 cerr <<
";; erxport collision mesh as STL" << endl;
585 if (vm.count(
"output_file")) {
586 vector<string> aa = vm[
"output_file"].as< vector<string> >();
587 cerr <<
";; output file is: " 591 if (vm.count(
"robot_name")) {
592 vector<string> aa = vm[
"robot_name"].as< vector<string> >();
593 cerr <<
";; robot_name is: " 597 if (vm.count(
"mesh_prefix")) {
598 vector<string> aa = vm[
"mesh_prefix"].as< vector<string> >();
599 cerr <<
";; mesh_prefix is: " 603 if (vm.count(
"mesh_output_dir")) {
604 vector<string> aa = vm[
"mesh_output_dir"].as< vector<string> >();
605 cerr <<
";; Mesh output directory is: " 609 boost::filesystem::path mpath(
mesh_dir );
611 if ( ! boost::filesystem::is_directory(mpath) ) {
612 boost::filesystem::create_directory ( mpath );
615 catch ( boost::filesystem::filesystem_error e ) {
616 cerr <<
";; mesh output directory error / " << e.what() << endl;
620 if (vm.count(
"input_file")) {
621 vector<string> aa = vm[
"input_file"].as< vector<string> >();
622 cerr <<
";; Input file is: " 627 if(inputfile ==
"") {
628 cerr << desc << endl;
632 std::string xml_string;
633 std::fstream xml_file(inputfile.c_str(), std::fstream::in);
634 while ( xml_file.good() )
637 std::getline( xml_file, line);
638 xml_string += (line +
"\n");
642 urdf::ModelInterfaceSharedPtr robot;
643 if( xml_string.find(
"<COLLADA") != std::string::npos )
645 ROS_DEBUG(
"Parsing robot collada xml string");
650 ROS_DEBUG(
"Parsing robot urdf xml string");
651 robot = parseURDF(xml_string);
654 std::cerr <<
"ERROR: Model Parsing the xml failed" << std::endl;
int main(int argc, char **argv)
bool export_collision_mesh
void printTreeXML(urdf::LinkConstSharedPtr link, string name, string file)
void assimp_file_export(std::string fname, std::string ofname, std::string mesh_type="collada")
bool use_same_collision_as_visual
void addChildJointNamesXML(urdf::LinkConstSharedPtr link, ofstream &os)
bool rotate_inertia_frame
void addChildLinkNamesXML(urdf::LinkConstSharedPtr link, ofstream &os)
urdf::ModelInterfaceSharedPtr parseCollada(const std::string &xml_string)
#define PRINT_ORIGIN_XML(os, origin)
bool use_simple_collision
void run(ClassLoader *loader)
void assimp_calc_bbox(string fname, float &minx, float &miny, float &minz, float &maxx, float &maxy, float &maxz)
bool add_gazebo_description