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 "kdl_parser/kdl_parser.hpp"
00038 #include <urdf/model.h>
00039 #include <kdl/frames_io.hpp>
00040 #include <ros/console.h>
00041
00042 using namespace std;
00043 using namespace KDL;
00044
00045 namespace kdl_parser{
00046
00047
00048
00049 Vector toKdl(urdf::Vector3 v)
00050 {
00051 return Vector(v.x, v.y, v.z);
00052 }
00053
00054
00055 Rotation toKdl(urdf::Rotation r)
00056 {
00057 return Rotation::Quaternion(r.x, r.y, r.z, r.w);
00058 }
00059
00060
00061 Frame toKdl(urdf::Pose p)
00062 {
00063 return Frame(toKdl(p.rotation), toKdl(p.position));
00064 }
00065
00066
00067 Joint toKdl(boost::shared_ptr<urdf::Joint> jnt)
00068 {
00069 Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
00070
00071 switch (jnt->type){
00072 case urdf::Joint::FIXED:{
00073 return Joint(jnt->name, Joint::None);
00074 }
00075 case urdf::Joint::REVOLUTE:{
00076 Vector axis = toKdl(jnt->axis);
00077 return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
00078 }
00079 case urdf::Joint::CONTINUOUS:{
00080 Vector axis = toKdl(jnt->axis);
00081 return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
00082 }
00083 case urdf::Joint::PRISMATIC:{
00084 Vector axis = toKdl(jnt->axis);
00085 return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis);
00086 }
00087 default:{
00088 ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint", jnt->name.c_str());
00089 return Joint(jnt->name, Joint::None);
00090 }
00091 }
00092 return Joint();
00093 }
00094
00095
00096 RigidBodyInertia toKdl(boost::shared_ptr<urdf::Inertial> i)
00097 {
00098 Frame origin = toKdl(i->origin);
00099
00100 return origin.M * RigidBodyInertia(i->mass, origin.p, RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz));
00101 }
00102
00103
00104
00105 bool addChildrenToTree(boost::shared_ptr<const urdf::Link> root, Tree& tree)
00106 {
00107 std::vector<boost::shared_ptr<urdf::Link> > children = root->child_links;
00108 ROS_DEBUG("Link %s had %i children", root->name.c_str(), (int)children.size());
00109
00110
00111 RigidBodyInertia inert(0);
00112 if (root->inertial)
00113 inert = toKdl(root->inertial);
00114
00115
00116 Joint jnt = toKdl(root->parent_joint);
00117
00118
00119 Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform), inert);
00120
00121
00122 tree.addSegment(sgm, root->parent_joint->parent_link_name);
00123
00124
00125 for (size_t i=0; i<children.size(); i++){
00126 if (!addChildrenToTree(children[i], tree))
00127 return false;
00128 }
00129 return true;
00130 }
00131
00132
00133 bool treeFromFile(const string& file, Tree& tree)
00134 {
00135 TiXmlDocument urdf_xml;
00136 urdf_xml.LoadFile(file);
00137 return treeFromXml(&urdf_xml, tree);
00138 }
00139
00140 bool treeFromParam(const string& param, Tree& tree)
00141 {
00142 urdf::Model robot_model;
00143 if (!robot_model.initParam(param)){
00144 ROS_ERROR("Could not generate robot model");
00145 return false;
00146 }
00147 return treeFromUrdfModel(robot_model, tree);
00148 }
00149
00150 bool treeFromString(const string& xml, Tree& tree)
00151 {
00152 TiXmlDocument urdf_xml;
00153 urdf_xml.Parse(xml.c_str());
00154 return treeFromXml(&urdf_xml, tree);
00155 }
00156
00157 bool treeFromXml(TiXmlDocument *xml_doc, Tree& tree)
00158 {
00159 urdf::Model robot_model;
00160 if (!robot_model.initXml(xml_doc)){
00161 ROS_ERROR("Could not generate robot model");
00162 return false;
00163 }
00164 return treeFromUrdfModel(robot_model, tree);
00165 }
00166
00167
00168 bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree)
00169 {
00170 tree = Tree(robot_model.getRoot()->name);
00171
00172
00173 if (robot_model.getRoot()->inertial)
00174 ROS_WARN("The root link %s has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.", robot_model.getRoot()->name.c_str());
00175
00176
00177 for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
00178 if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
00179 return false;
00180
00181 return true;
00182 }
00183
00184 }
00185