cob_kinematics: urdf_openrave.cpp Source File

Go to the documentation of this file.
00001 
00057 #include <ros/ros.h>
00058 #include <urdf/model.h>
00059 #include <fstream>
00060 
00061 using namespace std;
00062 
00063 
00064 std::string exec(const char* cmd) {
00065     FILE* pipe = popen(cmd, "r");
00066     if (!pipe) return "ERROR";
00067     char buffer[128];
00068     std::string result = "";
00069     while(!feof(pipe)) {
00070         if(fgets(buffer, 128, pipe) != NULL)
00071                 result += buffer;
00072     }
00073     pclose(pipe);
00074     return result;
00075 }
00076 
00077 struct body_data{
00078     urdf::Pose pose;
00079     string name;
00080     string parent;
00081 };
00082 struct joint_data{
00083     string name;
00084     string parent;
00085     string child;
00086     float lower, upper;
00087     urdf::Vector3 axis;
00088 };
00089 int main(int argc, char **argv){
00090     
00091     srand(time(NULL));
00092     ros::init(argc, argv, "urdf_openrave",ros::init_options::AnonymousName);
00093 
00094     boost::shared_ptr<urdf::Model> urdf_(new urdf::Model);
00095 
00096     // parse params
00097 
00098     string file_urdf, file_out;
00099     
00100     list<body_data> bodies;
00101     list<joint_data> joints;
00102 
00103     map<string,pair<string,string> > arms;
00104 
00105     {   int i = 1;
00106         while(i < argc){
00107                 if(strcmp(argv[i],"--urdf") == 0){
00108                         file_urdf = argv[++i];
00109                 }else if(strcmp(argv[i],"--output") == 0){
00110                         file_out = argv[++i];
00111                 }else{
00112                         arms.insert(make_pair(string(argv[i]),make_pair(string(argv[i+1]),string(argv[i+2]))));
00113                         i+=2;
00114                 }
00115                 ++i;
00116         }
00117         if(i > argc){
00118                 ROS_ERROR("planning_description_generator --urdf PATH [--output PATH] ARM_NAME ARM_ROOT ARM_TIP [...]");
00119                 return -1;
00120         }
00121     }
00122 
00123     if(arms.empty()){
00124         ROS_ERROR("No kinematic chains specified!");
00125         return -1;
00126     }
00127     
00128     // load model
00129 
00130     string xml;
00131 
00132     if(file_urdf.empty()){
00133                 stringstream sstream;
00134                 string line;
00135                 while( getline(cin, line)) {
00136                         sstream << line << endl;
00137                 }
00138                 xml = sstream.str();
00139     } else xml = exec((string("rosrun xacro xacro.py ")+file_urdf).c_str());
00140     
00141     if(!urdf_->initString(xml))
00142     {
00143                 ROS_ERROR_STREAM("Parsing URDF failed!");
00144                 return -1;
00145     }
00146 
00147     for(map<string,pair<string,string> >::iterator it=arms.begin(); it != arms.end(); ++it){
00148                 string link_name = it->second.second;
00149                 
00150                 while( link_name != it->second.first ){
00151                     boost::shared_ptr<urdf::Joint >  joint = urdf_->getLink(link_name)->parent_joint;
00152                     body_data bd;
00153                     bd.name = link_name;
00154                     bd.parent = joint->parent_link_name;
00155                     bd.pose = joint->parent_to_joint_origin_transform;
00156                     bodies.push_front(bd);
00157 
00158                     joint_data jd;
00159                     jd.name = joint->name;
00160                     jd.child = link_name;
00161                     jd.parent = joint->parent_link_name;
00162                     jd.axis = joint->axis;
00163                     if(joint->limits){
00164                         jd.lower = joint->limits->lower;
00165                         jd.upper = joint->limits->upper;
00166                     }else{
00167                         jd.lower = jd.upper = 0;
00168                     }
00169                     joints.push_front(jd);
00170 
00171                     link_name = joint->parent_link_name;
00172                 }
00173                 
00174     }
00175     
00176     
00177     stringstream sout;
00178     sout << "<Robot name=\"" << urdf_->getName() << "\">" << endl;
00179     sout << "\t" << "<KinBody>" << endl;
00180     
00181 /*        <Body name="arm_7_link" type="dynamic">
00182             <offsetfrom>arm_6_link</offsetfrom>
00183             <Translation>0 0.057 0.0455</Translation>
00184             <rotationaxis>1 0 0 -90</rotationaxis>
00185         </Body>
00186 */
00187     for(map<string,pair<string,string> >::iterator it=arms.begin(); it != arms.end(); ++it){
00188         sout << "\t" << "\t" << "<Body type=\"dynamic\" name=\"" << it->second.first << "\"/>" << endl;
00189     }
00190     for(list<body_data>::iterator it=bodies.begin(); it != bodies.end(); ++it){
00191         sout << "\t" << "\t" << "<Body type=\"dynamic\" name=\"" << it->name << "\">" << endl;
00192         sout << "\t" << "\t" << "\t" << "<offsetfrom>" << it->parent << "</offsetfrom>" << endl;
00193         sout << "\t" << "\t" << "\t" << "<Translation>" << it->pose.position.x << " "<< it->pose.position.y << " " << it->pose.position.z  << "</Translation>" << endl;
00194         sout << "\t" << "\t" << "\t" << "<quat>" << it->pose.rotation.w << " " << it->pose.rotation.x << " "<< it->pose.rotation.y << " " << it->pose.rotation.z  << "</quat>" << endl;
00195         sout << "\t" << "\t" << "</Body>" << endl;
00196     }
00197         
00198 /*        <Joint name="arm_1_joint" type="hinge">
00199             <offsetfrom>arm_0_link</offsetfrom>
00200             <Body>arm_0_link</Body>
00201             <Body>arm_1_link</Body>
00202             <Axis>0 0 1</Axis>
00203             <limitsrad>-2.9670 2.9670</limitsrad>
00204         </Joint>
00205 */        
00206     for(list<joint_data>::iterator it=joints.begin(); it != joints.end(); ++it){
00207         if( it->lower == 0 && it->upper == 0){
00208             sout << "\t" << "\t" << "<Joint type=\"hinge\" enable=\"false\" name=\"" << it->name << "\">" << endl;
00209         }else{
00210             sout << "\t" << "\t" << "<Joint type=\"hinge\" name=\"" << it->name << "\">" << endl;
00211             sout << "\t" << "\t" << "\t" << "<Axis>" << it->axis.x << " "<< it->axis.y << " " << it->axis.z  << "</Axis>" << endl;
00212         }
00213     sout << "\t" << "\t" << "\t" << "<limitsrad>" << it->lower << " "<< it->upper  << "</limitsrad>" << endl;
00214         sout << "\t" << "\t" "\t" << "<offsetfrom>" << it->child << "</offsetfrom>" << endl;
00215         sout << "\t" << "\t" << "\t" << "<Body>" << it->parent << "</Body>" << endl;
00216         sout << "\t" << "\t" << "\t" << "<Body>" << it->child << "</Body>" << endl;
00217         sout << "\t" << "\t" << "</Joint>" << endl;
00218     }
00219     sout << "\t" << "</KinBody>" << endl;
00220     for(map<string,pair<string,string> >::iterator it=arms.begin(); it != arms.end(); ++it){
00221         sout << "\t" << "<Manipulator name=\"" << it->first <<"\">" << endl;
00222         sout << "\t" << "\t" << "<base>" << it->second.first << "</base>" << endl;
00223         sout << "\t" << "\t" << "<effector>" << it->second.second << "</effector>" << endl;
00224         sout << "\t" << "</Manipulator>" << endl;
00225     }
00226 
00227     sout << "</Robot>" << endl;
00228       
00229     if(file_out.empty())
00230         cout << sout.str();
00231     else{
00232         ofstream of(file_out.c_str());
00233         of <<  sout.str();
00234     }
00235     
00236     return 0;
00237 }
00238