55 #include <kdl_parser/kdl_parser.h> 56 #include <kdl_parser/model.h> 57 #include <urdf_model/pose.h> 58 #include <kdl/frames_io.hpp> 59 #include <kdl_parser/exceptions.h> 66 namespace kdl_parser {
75 Vector to_kdl(urdf::Vector3 v)
77 return Vector(v.x, v.y, v.z);
81 Rotation to_kdl(urdf::Rotation r)
83 return Rotation::Quaternion(r.x, r.y, r.z, r.w);
87 Frame to_kdl(urdf::Pose p)
89 return Frame(to_kdl(p.rotation), to_kdl(p.position));
93 Joint to_kdl(boost::shared_ptr<urdf::Joint> jnt)
95 Frame F_parent_jnt = to_kdl(jnt->parent_to_joint_origin_transform);
98 case urdf::Joint::FIXED:{
99 return Joint(jnt->name, Joint::None);
101 case urdf::Joint::REVOLUTE:{
102 Vector axis = to_kdl(jnt->axis);
103 return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
105 case urdf::Joint::CONTINUOUS:{
106 Vector axis = to_kdl(jnt->axis);
107 return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
109 case urdf::Joint::PRISMATIC:{
110 Vector axis = to_kdl(jnt->axis);
111 return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis);
114 throw KDLParserUnknownJointTypeException(jnt->name.c_str());
115 return Joint(jnt->name, Joint::None);
122 RigidBodyInertia to_kdl(boost::shared_ptr<urdf::Inertial> i)
124 Frame origin = to_kdl(i->origin);
126 return origin.M * RigidBodyInertia(i->mass, origin.p, RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz));
131 bool add_children_to_tree(boost::shared_ptr<const urdf::Link> root, Tree& tree)
133 std::vector<boost::shared_ptr<urdf::Link> > children = root->child_links;
137 RigidBodyInertia inert(0);
139 inert = to_kdl(root->inertial);
142 Joint jnt = to_kdl(root->parent_joint);
145 Segment sgm(root->name, jnt, to_kdl(root->parent_joint->parent_to_joint_origin_transform), inert);
148 tree.addSegment(sgm, root->parent_joint->parent_link_name);
151 for (
size_t i=0; i<children.size(); i++){
152 if (!add_children_to_tree(children[i], tree))
159 bool tree_from_file(
const string& file, Tree& tree)
161 TiXmlDocument urdf_xml;
162 urdf_xml.LoadFile(file);
163 return tree_from_xml(&urdf_xml, tree);
166 bool tree_from_string(
const string& xml, Tree& tree)
168 TiXmlDocument urdf_xml;
169 urdf_xml.Parse(xml.c_str());
170 return tree_from_xml(&urdf_xml, tree);
173 bool tree_from_xml(TiXmlDocument *xml_doc, Tree& tree)
176 if (!robot_model.
initXml(xml_doc)){
177 throw KDLParserModelGenerationFailedException();
179 return tree_from_urdf_model(robot_model, tree);
183 bool tree_from_urdf_model(
const urdf::ModelInterface& robot_model, Tree& tree)
185 tree = Tree(robot_model.getRoot()->name);
188 for (
size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
189 if (!add_children_to_tree(robot_model.getRoot()->child_links[i], tree))
Fawkes library namespace.
bool initXml(TiXmlElement *xml)
Initialize the model using a XML Element.
This class represents an URDF model.