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> 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; } 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 << "\""; } 68 #define PRINT_GEOM(os, geometry) \ 69 if ( geometry->type == urdf::Geometry::MESH ) { os << "geom: name: " << ((urdf::Mesh *)geometry.get())->filename; } 72 std::string mesh_type =
"collada") {
73 #if defined(ASSIMP_EXPORT_API) 74 if (fname.find(
"file://") == 0) {
75 fname.erase(0, strlen(
"file://"));
77 Assimp::Importer importer;
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);
96 std::string str( importer.GetErrorString() );
97 std::cerr <<
";; " << str << std::endl;
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;
112 float &maxx,
float &maxy,
float &maxz) {
114 if (fname.find(
"file://") == 0) {
115 fname.erase(0, strlen(
"file://"));
118 Assimp::Importer importer;
119 const aiScene* scene = importer.ReadFile(fname.c_str(),
120 aiProcess_Triangulate |
121 aiProcess_JoinIdenticalVertices |
122 aiProcess_SortByPType);
126 std::string str( importer.GetErrorString() );
127 std::cerr <<
";; " << str << std::endl;
131 aiNode *node = scene->mRootNode;
134 if(node->mNumMeshes > 0 && node->mMeshes != NULL) {
135 std::cerr <<
"Root node has meshes " << node->mMeshes << std::endl;;
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];
148 std::cerr <<
"Can't find meshes in file" << std::endl;
152 aiMatrix4x4 transform = node->mTransformation;
155 maxx = maxy = maxz = -100000000.0;
156 minx = miny = minz = 100000000.0;
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;
163 for (
unsigned int i = 0 ; i < a->mNumVertices ; ++i) {
165 p.x = a->mVertices[i].x;
166 p.y = a->mVertices[i].y;
167 p.z = a->mVertices[i].z;
194 os <<
" <link name=\"" << link->name <<
"\">" << endl;
195 if ( !!link->visual ) {
196 os <<
" <visual>" << endl;
202 os <<
" <geometry>" << 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://"));
208 std::string ofname (
mesh_dir +
"/" + link->name +
"_mesh.dae");
215 std::ofstream tmp_os;
216 tmp_os.open(ofname.c_str());
218 is.open(ifname.c_str());
220 while(is && getline(is, buf)) tmp_os << buf << std::endl;
225 os <<
" <mesh filename=\"" <<
mesh_prefix +
"/" + link->name +
"_mesh.dae" <<
"\" scale=\"1 1 1\" />" << endl;
227 os <<
" <mesh filename=\"" <<
"file://" << ofname <<
"\" scale=\"1 1 1\" />" << endl;
230 os <<
" </geometry>" << endl;
233 float ax,ay,az,bx,by,bz;
234 if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
236 ax, ay, az, bx, by, bz);
239 urdf::Pose pp = link->visual->origin;
241 pp.position.x += ( ax + bx ) / 2 ;
242 pp.position.y += ( ay + by ) / 2 ;
243 pp.position.z += ( az + bz ) / 2 ;
247 os <<
" <geometry>" << endl;
248 os <<
" <box size=\"" << bx - ax <<
" " << by - ay <<
" " << bz - az <<
"\"/>" << endl;
249 os <<
" </geometry>" << endl;
251 os <<
" </visual>" << endl;
253 if ( !!link->collision ) {
254 os <<
" <collision>" << endl;
259 os <<
" <geometry>" << endl;
261 if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
264 ifname.assign (((urdf::Mesh *)link->visual->geometry.get())->filename);
266 ifname.assign (((urdf::Mesh *)link->collision->geometry.get())->filename);
268 if (ifname.find(
"file://") == 0) {
269 ifname.erase(0, strlen(
"file://"));
273 oofname = link->name +
"_mesh.stl";
275 oofname = link->name +
"_mesh.dae";
277 std::string ofname = (
mesh_dir +
"/" + oofname);
288 std::ofstream tmp_os;
289 tmp_os.open(ofname.c_str());
291 is.open(ifname.c_str());
293 while(is && getline(is, buf)) tmp_os << buf << std::endl;
298 os <<
" <mesh filename=\"" <<
mesh_prefix +
"/" + oofname;
300 os <<
" <mesh filename=\"" <<
"file://" << ofname;
302 os <<
"\" scale=\"1 1 1\" />" << endl;
304 os <<
" </geometry>" << endl;
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);
313 urdf::Pose pp = link->visual->origin;
314 pp.position.x += ( ax + bx ) / 2 ;
315 pp.position.y += ( ay + by ) / 2 ;
316 pp.position.z += ( az + bz ) / 2 ;
320 os <<
" <geometry>" << endl;
321 os <<
" <box size=\"" << bx - ax <<
" " << by - ay <<
" " << bz - az <<
"\"/>" << endl;
322 os <<
" </geometry>" << endl;
324 os <<
" </collision>" << endl;
326 if ( !!link->inertial ) {
328 os <<
" <inertial>" << endl;
329 os <<
" <mass value=\"" << link->inertial->mass <<
"\" />" << endl;
333 os <<
" <inertia ixx=\"" << link->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 <<
" </inertial>" << endl;
342 os <<
" <inertial>" << endl;
343 os <<
" <mass value=\"" << link->inertial->mass <<
"\" />" << endl;
346 Eigen::Quaterniond qt(link->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;
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; 374 imat = ( mat * imat * tmat );
380 urdf::Pose t_pose (link->inertial->origin);
381 t_pose.rotation.clear();
386 os <<
" <inertia ixx=\"" << imat(0, 0) <<
"\" ";
387 os <<
"ixy=\"" << imat(0, 1) <<
"\" ";
388 os <<
"ixz=\"" << imat(0, 2) <<
"\" ";
389 os <<
"iyy=\"" << imat(1, 1) <<
"\" ";
390 os <<
"iyz=\"" << imat(1, 2) <<
"\" ";
391 os <<
"izz=\"" << imat(2, 2) <<
"\"/>" << endl;
392 os <<
" </inertial>" << endl;
395 os <<
" </link>" << endl;
399 os <<
" <gazebo reference=\"" << link->name <<
"\">" << endl;
400 os <<
" <mu1>0.9</mu1>" << endl;
401 os <<
" <mu2>0.9</mu2>" << endl;
402 os <<
" </gazebo>" << endl;
406 for (std::vector<urdf::LinkSharedPtr >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
413 for (std::vector<urdf::LinkSharedPtr >::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);
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");
434 os <<
" <joint name=\"" << (*child)->parent_joint->name <<
"\" type=\"" << jtype <<
"\">" << endl;
435 os <<
" <parent link=\"" << link->name <<
"\"/>" << endl;
436 os <<
" <child link=\"" << (*child)->name <<
"\"/>" << endl;
437 os <<
" <origin xyz=\"" << (*child)->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 <<
" <axis xyz=\"" << (*child)->parent_joint->axis.x <<
" ";
442 os << (*child)->parent_joint->axis.y <<
" " << (*child)->parent_joint->axis.z <<
"\"/>" << endl;
444 urdf::JointSharedPtr jt((*child)->parent_joint);
446 if ( !!jt->limits ) {
448 os <<
"lower=\"" << jt->limits->lower <<
"\"";
449 os <<
" upper=\"" << jt->limits->upper <<
"\"";
450 if (jt->limits->effort == 0.0) {
451 os <<
" effort=\"100\"";
453 os <<
" effort=\"" << jt->limits->effort <<
"\"";
455 os <<
" velocity=\"" << jt->limits->velocity <<
"\"";
458 if ( !!jt->dynamics ) {
460 os <<
"damping=\"" << jt->dynamics->damping <<
"\"";
461 os <<
" friction=\"" << jt->dynamics->friction <<
"\"";
465 os <<
"damping=\"0.2\"";
466 os <<
" friction=\"0\"";
471 os <<
" <safety_controller";
472 os <<
" k_position=\"10\"";
473 os <<
" k_velocity=\"10\"";
474 os <<
" soft_lower_limit=\"-10\"";
475 os <<
" soft_upper_limit=\"10\"";
481 os <<
" </joint>" << endl;
485 os <<
" <transmission type=\"pr2_mechanism_model/SimpleTransmission\" name=\"";
486 os << (*child)->parent_joint->name <<
"_trans\" >" << endl;
487 os <<
" <actuator name=\"" << (*child)->parent_joint->name <<
"_motor\" />" << endl;
488 os <<
" <joint name=\"" << (*child)->parent_joint->name <<
"\" />" << endl;
489 os <<
" <mechanicalReduction>1</mechanicalReduction>" << endl;
492 os <<
" </transmission>" << endl;
494 os <<
" <transmission name=\"" << (*child)->parent_joint->name <<
"_trans\">" << endl;
495 os <<
" <type>transmission_interface/SimpleTransmission</type>" << endl;
496 os <<
" <joint name=\"" << (*child)->parent_joint->name <<
"\">" << endl;
497 os <<
" <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>" << endl;
498 os <<
" </joint>" << endl;
499 os <<
" <actuator name=\"" << (*child)->parent_joint->name <<
"_motor\">" << endl;
500 os <<
" <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>" << endl;
501 os <<
" <mechanicalReduction>1</mechanicalReduction>" << endl;
502 os <<
" </actuator>" << endl;
503 os <<
" </transmission>" << endl;
506 os <<
" <gazebo reference=\"" << (*child)->parent_joint->name <<
"\">" << endl;
507 os <<
" <cfmDamping>0.4</cfmDamping>" << endl;
508 os <<
" </gazebo>" << endl;
515 void printTreeXML(urdf::LinkConstSharedPtr link,
string name,
string file)
518 os.open(file.c_str());
519 os <<
"<?xml version=\"1.0\"?>" << endl;
520 os <<
"<robot name=\"" << name <<
"\"" << endl;
521 os <<
" xmlns:xi=\"http://www.w3.org/2001/XInclude\">" << endl;
527 os <<
"</robot>" << endl;
531 namespace po = boost::program_options;
534 int main(
int argc,
char** argv)
538 po::options_description desc(
"Usage: collada_to_urdf input.dae [options]\n Options for collada_to_urdf");
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<string> >(),
"output robot name")
550 (
"mesh_output_dir", po::value< vector<string> >(),
"directory for outputing")
551 (
"mesh_prefix", po::value< vector<string> >(),
"prefix of mesh files")
552 (
"output_file,O", po::value< vector<string> >(),
"output file")
553 (
"input_file", po::value< vector<string> >(),
"input file")
556 po::positional_options_description p;
557 p.add(
"input_file", -1);
559 po::variables_map vm;
561 po::store(po::command_line_parser(argc, argv).
562 options(desc).positional(p).
run(), vm);
565 catch (po::error e) {
566 cerr <<
";; option parse error / " << e.what() << endl;
570 if (vm.count(
"help")) {
571 cout << desc <<
"\n";
574 if (vm.count(
"simple_visual")) {
576 cerr <<
";; Using simple_visual" << endl;
578 if (vm.count(
"simple_collision")) {
580 cerr <<
";; Using simple_collision" << endl;
582 if (vm.count(
"add_gazebo_description")) {
584 cerr <<
";; Adding gazebo description" << endl;
586 if (vm.count(
"use_transmission_interface")) {
588 cerr <<
";; Using transmission_interface as transmission" << endl;
590 if (vm.count(
"use_assimp_export")) {
591 #if defined(ASSIMP_EXPORT_API) 594 cerr <<
";; Use assimp export" << endl;
596 if (vm.count(
"original_inertia_rotation")) {
598 cerr <<
";; Does not rotate inertia frame" << endl;
600 if (vm.count(
"export_collision_mesh")) {
602 cerr <<
";; erxport collision mesh as STL" << endl;
604 if (vm.count(
"output_file")) {
605 vector<string> aa = vm[
"output_file"].as< vector<string> >();
606 cerr <<
";; output file is: " 610 if (vm.count(
"robot_name")) {
611 vector<string> aa = vm[
"robot_name"].as< vector<string> >();
612 cerr <<
";; robot_name is: " 616 if (vm.count(
"mesh_prefix")) {
617 vector<string> aa = vm[
"mesh_prefix"].as< vector<string> >();
618 cerr <<
";; mesh_prefix is: " 622 if (vm.count(
"mesh_output_dir")) {
623 vector<string> aa = vm[
"mesh_output_dir"].as< vector<string> >();
624 cerr <<
";; Mesh output directory is: " 628 boost::filesystem::path mpath(
mesh_dir );
630 if ( ! boost::filesystem::is_directory(mpath) ) {
631 boost::filesystem::create_directory ( mpath );
634 catch ( boost::filesystem::filesystem_error e ) {
635 cerr <<
";; mesh output directory error / " << e.what() << endl;
639 if (vm.count(
"input_file")) {
640 vector<string> aa = vm[
"input_file"].as< vector<string> >();
641 cerr <<
";; Input file is: " 646 if(inputfile ==
"") {
647 cerr << desc << endl;
651 std::string xml_string;
652 std::fstream xml_file(inputfile.c_str(), std::fstream::in);
653 while ( xml_file.good() )
656 std::getline( xml_file, line);
657 xml_string += (line +
"\n");
661 urdf::ModelInterfaceSharedPtr robot;
662 if( xml_string.find(
"<COLLADA") != std::string::npos )
664 ROS_DEBUG(
"Parsing robot collada xml string");
669 ROS_DEBUG(
"Parsing robot urdf xml string");
670 robot = parseURDF(xml_string);
673 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
bool use_transmission_interface
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