55 #include "kdl_parser.h"
59 #include <kdl_parser/exceptions.h>
60 #include <urdf_model/pose.h>
62 #include <kdl/frames_io.hpp>
69 namespace kdl_parser {
73 to_kdl(urdf::Vector3 v)
75 return Vector(v.x, v.y, v.z);
80 to_kdl(urdf::Rotation r)
82 return Rotation::Quaternion(r.x, r.y, r.z, r.w);
89 return Frame(to_kdl(p.rotation), to_kdl(p.position));
94 to_kdl(urdf::JointSharedPtr jnt)
96 Frame F_parent_jnt = to_kdl(jnt->parent_to_joint_origin_transform);
99 case urdf::Joint::FIXED: {
100 return Joint(jnt->name, Joint::None);
102 case urdf::Joint::REVOLUTE: {
103 Vector axis = to_kdl(jnt->axis);
104 return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
106 case urdf::Joint::CONTINUOUS: {
107 Vector axis = to_kdl(jnt->axis);
108 return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
110 case urdf::Joint::PRISMATIC: {
111 Vector axis = to_kdl(jnt->axis);
112 return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis);
115 throw KDLParserUnknownJointTypeException(jnt->name.c_str());
116 return Joint(jnt->name, Joint::None);
124 to_kdl(urdf::InertialSharedPtr i)
126 Frame origin = to_kdl(i->origin);
129 * RigidBodyInertia(i->mass,
131 RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz));
136 add_children_to_tree(urdf::LinkSharedPtr root, Tree &tree)
138 const std::vector<urdf::LinkSharedPtr> children = root->child_links;
142 RigidBodyInertia inert(0);
144 inert = to_kdl(root->inertial);
147 Joint jnt = to_kdl(root->parent_joint);
150 Segment sgm(root->name, jnt, to_kdl(root->parent_joint->parent_to_joint_origin_transform), inert);
153 tree.addSegment(sgm, root->parent_joint->parent_link_name);
156 for (
size_t i = 0; i < children.size(); i++) {
157 if (!add_children_to_tree(children[i], tree))
164 tree_from_file(
const string &file, Tree &tree)
166 TiXmlDocument urdf_xml;
167 urdf_xml.LoadFile(file);
168 return tree_from_xml(&urdf_xml, tree);
172 tree_from_string(
const string &xml, Tree &tree)
174 TiXmlDocument urdf_xml;
175 urdf_xml.Parse(xml.c_str());
176 return tree_from_xml(&urdf_xml, tree);
180 tree_from_xml(TiXmlDocument *xml_doc, Tree &tree)
183 if (!robot_model.
initXml(xml_doc)) {
184 throw KDLParserModelGenerationFailedException();
186 return tree_from_urdf_model(robot_model, tree);
190 tree_from_urdf_model(
const urdf::ModelInterface &robot_model, Tree &tree)
192 tree = Tree(robot_model.getRoot()->name);
195 for (
size_t i = 0; i < robot_model.getRoot()->child_links.size(); i++)
196 if (!add_children_to_tree(robot_model.getRoot()->child_links[i], tree))
This class represents an URDF model.
bool initXml(TiXmlElement *xml)
Initialize the model using a XML Element.
Fawkes library namespace.