65 if( !
srdf_model_->initString( robot_model, srdf_string ) )
109 if( !
srdf_model_->initString( robot_model, srdf_string ) )
111 throw std::runtime_error(
"Unable to update the SRDF Model" );
124 return document.SaveFile( file_path );
136 TiXmlPrinter printer;
137 printer.SetIndent(
" " );
138 document.Accept( &printer );
141 return printer.CStr();
149 TiXmlDocument document;
150 TiXmlDeclaration* decl =
new TiXmlDeclaration(
"1.0",
"",
"" );
151 document.LinkEndChild( decl );
154 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" );
155 document.LinkEndChild( comment );
158 TiXmlElement* robot_root =
new TiXmlElement(
"robot");
160 document.LinkEndChild( robot_root );
195 TiXmlComment *comment;
196 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" );
197 root->LinkEndChild( comment );
198 comment =
new TiXmlComment(
"LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included" );
199 root->LinkEndChild( comment );
200 comment =
new TiXmlComment(
"JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included" );
201 root->LinkEndChild( comment );
202 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");
203 root->LinkEndChild( comment );
204 comment =
new TiXmlComment(
"SUBGROUPS: Groups can also be formed by referencing to already defined group names" );
205 root->LinkEndChild( comment );
209 for( std::vector<srdf::Model::Group>::iterator group_it =
groups_.begin();
210 group_it !=
groups_.end(); ++group_it )
214 TiXmlElement *group =
new TiXmlElement(
"group");
215 group->SetAttribute(
"name", group_it->name_ );
216 root->LinkEndChild(group);
219 for( std::vector<std::string>::const_iterator link_it = group_it->links_.begin();
220 link_it != group_it->links_.end(); ++link_it )
222 TiXmlElement *link =
new TiXmlElement(
"link");
223 link->SetAttribute(
"name", *link_it );
224 group->LinkEndChild( link );
228 for( std::vector<std::string>::const_iterator joint_it = group_it->joints_.begin();
229 joint_it != group_it->joints_.end(); ++joint_it )
231 TiXmlElement *joint =
new TiXmlElement(
"joint");
232 joint->SetAttribute(
"name", *joint_it );
233 group->LinkEndChild( joint );
237 for( std::vector<std::pair<std::string,std::string> >::const_iterator chain_it = group_it->chains_.begin();
238 chain_it != group_it->chains_.end(); ++chain_it )
240 TiXmlElement *chain =
new TiXmlElement(
"chain");
241 chain->SetAttribute(
"base_link", chain_it->first );
242 chain->SetAttribute(
"tip_link", chain_it->second );
243 group->LinkEndChild( chain );
247 for( std::vector<std::string>::const_iterator subgroup_it = group_it->subgroups_.begin();
248 subgroup_it != group_it->subgroups_.end(); ++subgroup_it )
250 TiXmlElement *subgroup =
new TiXmlElement(
"group");
251 subgroup->SetAttribute(
"name", *subgroup_it );
252 group->LinkEndChild( subgroup );
267 TiXmlComment *comment =
new TiXmlComment();
268 comment->SetValue(
"COLLISION SPHERES: Purpose: Define a set of spheres that bounds a link." );
269 root->LinkEndChild( comment );
275 if (link_sphere_it->spheres_.empty())
279 TiXmlElement *link =
new TiXmlElement(
"link_sphere_approximation");
280 link->SetAttribute(
"link", link_sphere_it->link_);
281 root->LinkEndChild( link );
284 for( std::vector<srdf::Model::Sphere>::const_iterator sphere_it = link_sphere_it->spheres_.begin();
285 sphere_it != link_sphere_it->spheres_.end(); ++sphere_it )
287 TiXmlElement *sphere =
new TiXmlElement(
"sphere");
288 std::stringstream center;
289 center.precision(20);
290 center << sphere_it->center_x_ <<
" " << sphere_it->center_y_ <<
" " << sphere_it->center_z_;
291 sphere->SetAttribute(
"center", center.str() );
292 sphere->SetDoubleAttribute(
"radius", sphere_it->radius_ );
293 link->LinkEndChild( sphere );
306 TiXmlComment *comment =
new TiXmlComment();
307 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. " );
308 root->LinkEndChild( comment );
311 for ( std::vector<srdf::Model::DisabledCollision>::const_iterator pair_it =
disabled_collisions_.begin();
315 TiXmlElement *link_pair =
new TiXmlElement(
"disable_collisions");
316 link_pair->SetAttribute(
"link1", pair_it->link1_ );
317 link_pair->SetAttribute(
"link2", pair_it->link2_ );
318 link_pair->SetAttribute(
"reason", pair_it->reason_ );
320 root->LinkEndChild( link_pair );
332 TiXmlComment *comment =
new TiXmlComment();
333 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'" );
334 root->LinkEndChild( comment );
337 for ( std::vector<srdf::Model::GroupState>::const_iterator state_it =
group_states_.begin();
341 TiXmlElement *state =
new TiXmlElement(
"group_state");
342 state->SetAttribute(
"name", state_it->name_ );
343 state->SetAttribute(
"group", state_it->group_ );
344 root->LinkEndChild( state );
347 for( std::map<std::string, std::vector<double> >::const_iterator value_it = state_it->joint_values_.begin();
348 value_it != state_it->joint_values_.end(); ++value_it )
350 TiXmlElement *joint =
new TiXmlElement(
"joint");
351 joint->SetAttribute(
"name", value_it->first );
352 joint->SetDoubleAttribute(
"value", value_it->second[0] );
355 state->LinkEndChild( joint );
368 TiXmlComment *comment =
new TiXmlComment();
369 comment->SetValue(
"END EFFECTOR: Purpose: Represent information about an end effector." );
370 root->LinkEndChild( comment );
373 for ( std::vector<srdf::Model::EndEffector>::const_iterator effector_it =
end_effectors_.begin();
377 TiXmlElement *effector =
new TiXmlElement(
"end_effector");
378 effector->SetAttribute(
"name", effector_it->name_ );
379 effector->SetAttribute(
"parent_link", effector_it->parent_link_ );
380 effector->SetAttribute(
"group", effector_it->component_group_ );
381 if (!effector_it->parent_group_.empty())
382 effector->SetAttribute(
"parent_group", effector_it->parent_group_ );
383 root->LinkEndChild( effector );
395 TiXmlComment *comment =
new TiXmlComment();
396 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)" );
397 root->LinkEndChild( comment );
400 for ( std::vector<srdf::Model::VirtualJoint>::const_iterator virtual_it =
virtual_joints_.begin();
404 TiXmlElement *virtual_joint =
new TiXmlElement(
"virtual_joint");
405 virtual_joint->SetAttribute(
"name", virtual_it->name_ );
406 virtual_joint->SetAttribute(
"type", virtual_it->type_ );
407 virtual_joint->SetAttribute(
"parent_frame", virtual_it->parent_frame_ );
408 virtual_joint->SetAttribute(
"child_link", virtual_it->child_link_ );
410 root->LinkEndChild( virtual_joint );
418 TiXmlComment *comment =
new TiXmlComment();
419 comment->SetValue(
"PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated" );
420 root->LinkEndChild( comment );
422 for ( std::vector<srdf::Model::PassiveJoint>::const_iterator p_it =
passive_joints_.begin();
426 TiXmlElement *p_joint =
new TiXmlElement(
"passive_joint");
427 p_joint->SetAttribute(
"name", p_it->name_ );
428 root->LinkEndChild( p_joint );
void createEndEffectorsXML(TiXmlElement *root)
void createDisabledCollisionsXML(TiXmlElement *root)
Representation of semantic information about the robot.
void createGroupStatesXML(TiXmlElement *root)
void updateSRDFModel(const urdf::ModelInterface &robot_model)
void initModel(const urdf::ModelInterface &robot_model, const srdf::Model &srdf_model)
std::vector< srdf::Model::VirtualJoint > virtual_joints_
void createPassiveJointsXML(TiXmlElement *root)
std::vector< srdf::Model::DisabledCollision > disabled_collisions_
std::vector< srdf::Model::EndEffector > end_effectors_
std::vector< srdf::Model::LinkSpheres > link_sphere_approximations_
void createVirtualJointsXML(TiXmlElement *root)
srdf::ModelSharedPtr srdf_model_
void createLinkSphereApproximationsXML(TiXmlElement *root)
bool writeSRDF(const std::string &file_path)
TiXmlDocument generateSRDF()
void createGroupsXML(TiXmlElement *root)
bool initString(const urdf::ModelInterface &robot_model, const std::string &srdf_string)
std::vector< srdf::Model::Group > groups_
std::string getSRDFString()
std::vector< srdf::Model::GroupState > group_states_
std::vector< srdf::Model::PassiveJoint > passive_joints_