00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <tinyxml.h>
00038 #include <srdfdom/srdf_writer.h>
00039
00040 namespace srdf
00041 {
00042
00043
00044
00045
00046 SRDFWriter::SRDFWriter()
00047 {
00048
00049 srdf_model_.reset( new srdf::Model() );
00050 }
00051
00052
00053
00054
00055 SRDFWriter::~SRDFWriter()
00056 {
00057 }
00058
00059
00060
00061
00062 bool SRDFWriter::initString( const urdf::ModelInterface &robot_model, const std::string &srdf_string )
00063 {
00064
00065 if( !srdf_model_->initString( robot_model, srdf_string ) )
00066 {
00067 return false;
00068 }
00069
00070
00071 initModel( robot_model, *srdf_model_ );
00072
00073 return true;
00074 }
00075
00076
00077
00078
00079 void SRDFWriter::initModel( const urdf::ModelInterface &robot_model, const srdf::Model &srdf_model )
00080 {
00081
00082 if (srdf_model_.get() != &srdf_model)
00083 {
00084 *srdf_model_ = srdf_model;
00085 }
00086
00087
00088 disabled_collisions_ = srdf_model_->getDisabledCollisionPairs();
00089 link_sphere_approximations_ = srdf_model_->getLinkSphereApproximations();
00090 groups_ = srdf_model_->getGroups();
00091 virtual_joints_ = srdf_model_->getVirtualJoints();
00092 end_effectors_ = srdf_model_->getEndEffectors();
00093 group_states_ = srdf_model_->getGroupStates();
00094 passive_joints_ = srdf_model_->getPassiveJoints();
00095
00096
00097 robot_name_ = robot_model.getName();
00098 }
00099
00100
00101
00102
00103 void SRDFWriter::updateSRDFModel( const urdf::ModelInterface &robot_model )
00104 {
00105
00106 const std::string srdf_string = getSRDFString();
00107
00108
00109 if( !srdf_model_->initString( robot_model, srdf_string ) )
00110 {
00111 throw std::runtime_error( "Unable to update the SRDF Model" );
00112 }
00113 }
00114
00115
00116
00117
00118 bool SRDFWriter::writeSRDF( const std::string &file_path )
00119 {
00120
00121 TiXmlDocument document = generateSRDF();
00122
00123
00124 return document.SaveFile( file_path );
00125 }
00126
00127
00128
00129
00130 std::string SRDFWriter::getSRDFString()
00131 {
00132
00133 TiXmlDocument document = generateSRDF();
00134
00135
00136 TiXmlPrinter printer;
00137 printer.SetIndent( " " );
00138 document.Accept( &printer );
00139
00140
00141 return printer.CStr();
00142 }
00143
00144
00145
00146
00147 TiXmlDocument SRDFWriter::generateSRDF()
00148 {
00149 TiXmlDocument document;
00150 TiXmlDeclaration* decl = new TiXmlDeclaration( "1.0", "", "" );
00151 document.LinkEndChild( decl );
00152
00153
00154 TiXmlComment * comment = new TiXmlComment( "This does not replace URDF, and is not an extension of URDF.\n This is a format for representing semantic information about the robot structure.\n A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined\n" );
00155 document.LinkEndChild( comment );
00156
00157
00158 TiXmlElement* robot_root = new TiXmlElement("robot");
00159 robot_root->SetAttribute("name", robot_name_ );
00160 document.LinkEndChild( robot_root );
00161
00162
00163 createGroupsXML( robot_root );
00164
00165
00166 createGroupStatesXML( robot_root );
00167
00168
00169 createEndEffectorsXML( robot_root );
00170
00171
00172 createVirtualJointsXML( robot_root );
00173
00174
00175 createPassiveJointsXML( robot_root );
00176
00177
00178 createLinkSphereApproximationsXML( robot_root );
00179
00180
00181 createDisabledCollisionsXML( robot_root );
00182
00183
00184 return document;
00185 }
00186
00187
00188
00189
00190 void SRDFWriter::createGroupsXML( TiXmlElement *root )
00191 {
00192
00193 if( groups_.size() )
00194 {
00195 TiXmlComment *comment;
00196 comment = new TiXmlComment( "GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc" );
00197 root->LinkEndChild( comment );
00198 comment = new TiXmlComment( "LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included" );
00199 root->LinkEndChild( comment );
00200 comment = new TiXmlComment( "JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included" );
00201 root->LinkEndChild( comment );
00202 comment = new TiXmlComment( "CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group");
00203 root->LinkEndChild( comment );
00204 comment = new TiXmlComment( "SUBGROUPS: Groups can also be formed by referencing to already defined group names" );
00205 root->LinkEndChild( comment );
00206 }
00207
00208
00209 for( std::vector<srdf::Model::Group>::iterator group_it = groups_.begin();
00210 group_it != groups_.end(); ++group_it )
00211 {
00212
00213
00214 TiXmlElement *group = new TiXmlElement("group");
00215 group->SetAttribute("name", group_it->name_ );
00216 root->LinkEndChild(group);
00217
00218
00219 for( std::vector<std::string>::const_iterator link_it = group_it->links_.begin();
00220 link_it != group_it->links_.end(); ++link_it )
00221 {
00222 TiXmlElement *link = new TiXmlElement("link");
00223 link->SetAttribute("name", *link_it );
00224 group->LinkEndChild( link );
00225 }
00226
00227
00228 for( std::vector<std::string>::const_iterator joint_it = group_it->joints_.begin();
00229 joint_it != group_it->joints_.end(); ++joint_it )
00230 {
00231 TiXmlElement *joint = new TiXmlElement("joint");
00232 joint->SetAttribute("name", *joint_it );
00233 group->LinkEndChild( joint );
00234 }
00235
00236
00237 for( std::vector<std::pair<std::string,std::string> >::const_iterator chain_it = group_it->chains_.begin();
00238 chain_it != group_it->chains_.end(); ++chain_it )
00239 {
00240 TiXmlElement *chain = new TiXmlElement("chain");
00241 chain->SetAttribute("base_link", chain_it->first );
00242 chain->SetAttribute("tip_link", chain_it->second );
00243 group->LinkEndChild( chain );
00244 }
00245
00246
00247 for( std::vector<std::string>::const_iterator subgroup_it = group_it->subgroups_.begin();
00248 subgroup_it != group_it->subgroups_.end(); ++subgroup_it )
00249 {
00250 TiXmlElement *subgroup = new TiXmlElement("group");
00251 subgroup->SetAttribute("name", *subgroup_it );
00252 group->LinkEndChild( subgroup );
00253 }
00254
00255 }
00256 }
00257
00258
00259
00260
00261 void SRDFWriter::createLinkSphereApproximationsXML( TiXmlElement *root )
00262 {
00263 if( link_sphere_approximations_.empty() )
00264 return;
00265
00266
00267 TiXmlComment *comment = new TiXmlComment();
00268 comment->SetValue( "COLLISION SPHERES: Purpose: Define a set of spheres that bounds a link." );
00269 root->LinkEndChild( comment );
00270
00271
00272 for ( std::vector<srdf::Model::LinkSpheres>::const_iterator link_sphere_it = link_sphere_approximations_.begin();
00273 link_sphere_it != link_sphere_approximations_.end() ; ++link_sphere_it)
00274 {
00275 if (link_sphere_it->spheres_.empty())
00276 continue;
00277
00278
00279 TiXmlElement *link = new TiXmlElement("link_sphere_approximation");
00280 link->SetAttribute("link", link_sphere_it->link_);
00281 root->LinkEndChild( link );
00282
00283
00284 for( std::vector<srdf::Model::Sphere>::const_iterator sphere_it = link_sphere_it->spheres_.begin();
00285 sphere_it != link_sphere_it->spheres_.end(); ++sphere_it )
00286 {
00287 TiXmlElement *sphere = new TiXmlElement("sphere");
00288 std::stringstream center;
00289 center.precision(20);
00290 center << sphere_it->center_x_ << " " << sphere_it->center_y_ << " " << sphere_it->center_z_;
00291 sphere->SetAttribute("center", center.str() );
00292 sphere->SetDoubleAttribute("radius", sphere_it->radius_ );
00293 link->LinkEndChild( sphere );
00294 }
00295 }
00296 }
00297
00298
00299
00300
00301 void SRDFWriter::createDisabledCollisionsXML( TiXmlElement *root )
00302 {
00303
00304 if( disabled_collisions_.size() )
00305 {
00306 TiXmlComment *comment = new TiXmlComment();
00307 comment->SetValue( "DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. " );
00308 root->LinkEndChild( comment );
00309 }
00310
00311 for ( std::vector<srdf::Model::DisabledCollision>::const_iterator pair_it = disabled_collisions_.begin();
00312 pair_it != disabled_collisions_.end() ; ++pair_it)
00313 {
00314
00315 TiXmlElement *link_pair = new TiXmlElement("disable_collisions");
00316 link_pair->SetAttribute("link1", pair_it->link1_ );
00317 link_pair->SetAttribute("link2", pair_it->link2_ );
00318 link_pair->SetAttribute("reason", pair_it->reason_ );
00319
00320 root->LinkEndChild( link_pair );
00321 }
00322 }
00323
00324
00325
00326
00327 void SRDFWriter::createGroupStatesXML( TiXmlElement *root )
00328 {
00329
00330 if( group_states_.size() )
00331 {
00332 TiXmlComment *comment = new TiXmlComment();
00333 comment->SetValue( "GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'" );
00334 root->LinkEndChild( comment );
00335 }
00336
00337 for ( std::vector<srdf::Model::GroupState>::const_iterator state_it = group_states_.begin();
00338 state_it != group_states_.end() ; ++state_it)
00339 {
00340
00341 TiXmlElement *state = new TiXmlElement("group_state");
00342 state->SetAttribute("name", state_it->name_ );
00343 state->SetAttribute("group", state_it->group_ );
00344 root->LinkEndChild( state );
00345
00346
00347 for( std::map<std::string, std::vector<double> >::const_iterator value_it = state_it->joint_values_.begin();
00348 value_it != state_it->joint_values_.end(); ++value_it )
00349 {
00350 TiXmlElement *joint = new TiXmlElement("joint");
00351 joint->SetAttribute("name", value_it->first );
00352 joint->SetDoubleAttribute("value", value_it->second[0] );
00353
00354
00355 state->LinkEndChild( joint );
00356 }
00357 }
00358 }
00359
00360
00361
00362
00363 void SRDFWriter::createEndEffectorsXML( TiXmlElement *root )
00364 {
00365
00366 if( end_effectors_.size() )
00367 {
00368 TiXmlComment *comment = new TiXmlComment();
00369 comment->SetValue( "END EFFECTOR: Purpose: Represent information about an end effector." );
00370 root->LinkEndChild( comment );
00371 }
00372
00373 for ( std::vector<srdf::Model::EndEffector>::const_iterator effector_it = end_effectors_.begin();
00374 effector_it != end_effectors_.end() ; ++effector_it)
00375 {
00376
00377 TiXmlElement *effector = new TiXmlElement("end_effector");
00378 effector->SetAttribute("name", effector_it->name_ );
00379 effector->SetAttribute("parent_link", effector_it->parent_link_ );
00380 effector->SetAttribute("group", effector_it->component_group_ );
00381 if (!effector_it->parent_group_.empty())
00382 effector->SetAttribute("parent_group", effector_it->parent_group_ );
00383 root->LinkEndChild( effector );
00384 }
00385 }
00386
00387
00388
00389
00390 void SRDFWriter::createVirtualJointsXML( TiXmlElement *root )
00391 {
00392
00393 if( virtual_joints_.size() )
00394 {
00395 TiXmlComment *comment = new TiXmlComment();
00396 comment->SetValue( "VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)" );
00397 root->LinkEndChild( comment );
00398 }
00399
00400 for ( std::vector<srdf::Model::VirtualJoint>::const_iterator virtual_it = virtual_joints_.begin();
00401 virtual_it != virtual_joints_.end() ; ++virtual_it)
00402 {
00403
00404 TiXmlElement *virtual_joint = new TiXmlElement("virtual_joint");
00405 virtual_joint->SetAttribute("name", virtual_it->name_ );
00406 virtual_joint->SetAttribute("type", virtual_it->type_ );
00407 virtual_joint->SetAttribute("parent_frame", virtual_it->parent_frame_ );
00408 virtual_joint->SetAttribute("child_link", virtual_it->child_link_ );
00409
00410 root->LinkEndChild( virtual_joint );
00411 }
00412 }
00413
00414 void SRDFWriter::createPassiveJointsXML( TiXmlElement *root )
00415 {
00416 if ( passive_joints_.size() )
00417 {
00418 TiXmlComment *comment = new TiXmlComment();
00419 comment->SetValue( "PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated" );
00420 root->LinkEndChild( comment );
00421 }
00422 for ( std::vector<srdf::Model::PassiveJoint>::const_iterator p_it = passive_joints_.begin();
00423 p_it != passive_joints_.end() ; ++p_it)
00424 {
00425
00426 TiXmlElement *p_joint = new TiXmlElement("passive_joint");
00427 p_joint->SetAttribute("name", p_it->name_ );
00428 root->LinkEndChild( p_joint );
00429 }
00430 }
00431
00432
00433 }