bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

remove deprecated files and methods

This commit is contained in:
meeussen 2009-10-05 19:54:13 +00:00
parent 37423202e1
commit 379c125184
7 changed files with 1 additions and 545 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
kdl_parser/CMakeLists.txt
View File

@ -23,10 +23,7 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined services
#gensrv()
rosbuild_add_library(${PROJECT_NAME} src/xml_parser.cpp src/kdl_parser.cpp)
rosbuild_add_executable(example_xml test/example_xml.cpp )
target_link_libraries(example_xml ${PROJECT_NAME})
rosbuild_add_library(${PROJECT_NAME} src/kdl_parser.cpp)
rosbuild_add_executable(example test/example.cpp )
target_link_libraries(example ${PROJECT_NAME})

4
kdl_parser/include/kdl_parser/dom_parser.hpp
View File

@ -1,4 +0,0 @@
#warning ____YOU ARE INCLUDING THE DEPRECATED KDL PARSER. PLEASE SWITCH TO THE NEW KDL PARSER. SEE www.ros.org/wiki/kdl_parser FOR DETAILS____
#include <kdl_parser/kdl_parser.hpp>

9
kdl_parser/include/kdl_parser/kdl_parser.hpp
View File

@ -66,15 +66,6 @@ bool treeFromString(const string& xml, KDL::Tree& tree);
*/
bool treeFromXml(TiXmlDocument *xml_doc, KDL::Tree& tree);
/** Constructs a KDL tree from a URDF robot model
* \param robot_model The URDF robot model
* \param tree The resulting KDL Tree
* returns true on success, false on failure
*/
bool treeFromRobotModel(const urdf::Model& robot_model, KDL::Tree& tree) __attribute__((deprecated));;
/** Constructs a KDL tree from a URDF robot model
* \param robot_model The URDF robot model
* \param tree The resulting KDL Tree

55
kdl_parser/include/kdl_parser/xml_parser.hpp
View File

@ -1,55 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef XML_PARSER_H
#define XML_PARSER_H
#warning ____YOU ARE INCLUDING THE DEPRECATED KDL PARSER. PLEASE SWITCH TO THE NEW KDL PARSER. SEE www.ros.org/wiki/kdl_parser FOR DETAILS____
#include <kdl/tree.hpp>
#include <string>
#include <tinyxml/tinyxml.h>
using namespace std;
namespace KDL{
bool treeFromFile(const string& file, Tree& tree);
bool treeFromString(const string& xml, Tree& tree);
bool treeFromXml(TiXmlElement *root, Tree& tree);
}
#endif

7
kdl_parser/src/kdl_parser.cpp
View File

@ -167,12 +167,5 @@ bool treeFromUrdfModel(const urdf::Model& robot_model, Tree& tree)
return true;
}
bool treeFromRobotModel(const urdf::Model& robot_model, Tree& tree)
{
ROS_ERROR("treeFromRobotModel function is deprecated and replaces by treeFromUrdfModel");
return treeFromUrdfModel(robot_model, tree);
}
}

367
kdl_parser/src/xml_parser.cpp
View File

@ -1,367 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include <boost/algorithm/string.hpp>
#include "kdl_parser/xml_parser.hpp"
#include <ros/ros.h>
using namespace std;
namespace KDL{
bool isNumber(const char& c)
{
return (c=='1' || c=='2' ||c=='3' ||c=='4' ||c=='5' ||c=='6' ||c=='7' ||c=='8' ||c=='9' ||c=='0' ||c=='.' ||c=='-' ||c==' ');
}
bool isNumber(const std::string& s)
{
for (unsigned int i=0; i<s.size(); i++)
if (!isNumber(s[i])) return false;
return true;
}
bool getAtribute(TiXmlElement *xml, const string& name, string& attr)
{
if (!xml) return false;
const char *attr_char = xml->Attribute(name.c_str());
if (!attr_char){
cerr << "No " << name << " found in xml" << endl;
return false;
}
attr = string(attr_char);
return true;
}
bool getVector(TiXmlElement *vector_xml, const string& field, Vector& vector)
{
if (!vector_xml) return false;
string vector_str;
if (!getAtribute(vector_xml, field, vector_str))
return false;
std::vector<std::string> pieces;
boost::split( pieces, vector_str, boost::is_any_of(" "));
unsigned int pos=0;
for (unsigned int i = 0; i < pieces.size(); ++i){
if (pieces[i] != ""){
if (pos < 3){
if (!isNumber(pieces[i]))
{cerr << "This is not a valid number: '" << pieces[i] << "'" << endl; return false;}
vector(pos) = atof(pieces[i].c_str());
}
pos++;
}
}
if (pos != 3) {
cerr << "Vector did not contain 3 pieces:" << endl;
pos = 1;
for (unsigned int i = 0; i < pieces.size(); ++i){
if (pieces[i] != ""){
cerr << " " << pos << ": '" << pieces[i] << "'" << endl;
pos++;
}
}
return false;
}
return true;
}
bool getValue(TiXmlElement *value_xml, const string& field, double& value)
{
if (!value_xml) return false;
string value_str;
if (!getAtribute(value_xml, field, value_str)) return false;
if (!isNumber(value_str))
{cerr << "This is not a valid number: '" << value_str << "'" << endl; return false;}
value = atof(value_str.c_str());
return true;
}
bool getFrame(TiXmlElement *frame_xml, Frame& frame)
{
if (!frame_xml) return false;
Vector origin, rpy;
if (!getVector(frame_xml, "xyz", origin))
{cerr << "Frame does not have xyz" << endl; return false;}
if (!getVector(frame_xml, "rpy", rpy))
{cerr << "Frame does not have rpy" << endl; return false;}
frame = Frame(Rotation::RPY(rpy(0), rpy(1), rpy(2)), origin);
return true;
}
bool getRotInertia(TiXmlElement *rot_inertia_xml, RotationalInertia& rot_inertia)
{
if (!rot_inertia_xml) return false;
double Ixx=0, Iyy=0, Izz=0, Ixy=0, Ixz=0, Iyz=0;
if (!getValue(rot_inertia_xml, "ixx", Ixx)) return false;
if (!getValue(rot_inertia_xml, "iyy", Iyy)) return false;
if (!getValue(rot_inertia_xml, "izz", Izz)) return false;
if (!getValue(rot_inertia_xml, "ixy", Ixy)) return false;
if (!getValue(rot_inertia_xml, "ixz", Ixz)) return false;
if (!getValue(rot_inertia_xml, "iyz", Iyz)) return false;
rot_inertia = RotationalInertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz);
return true;
}
bool getInertia(TiXmlElement *inertia_xml, RigidBodyInertia& inertia)
{
if (!inertia_xml) return false;
Vector cog;
if (!getVector(inertia_xml->FirstChildElement("com"), "xyz", cog))
{cerr << "Inertia does not specify center of gravity" << endl; return false;}
double mass = 0.0;
if (!getValue(inertia_xml->FirstChildElement("mass"), "value", mass))
{cerr << "Inertia does not specify mass" << endl; return false;}
RotationalInertia rot_inertia;
if (!getRotInertia(inertia_xml->FirstChildElement("inertia"), rot_inertia))
{cerr << "Inertia does not specify rotational inertia" << endl; return false;}
inertia = RigidBodyInertia(mass, cog, rot_inertia);
return true;
}
bool getJoint(TiXmlElement *joint_xml, Joint& joint)
{
if (!joint_xml) return false;
// get joint name
string joint_name;
if (!getAtribute(joint_xml, "name", joint_name))
{cerr << "Joint does not have name" << endl; return false;}
// get joint type
string joint_type;
if (!getAtribute(joint_xml, "type", joint_type))
{cerr << "Joint does not have type" << endl; return false;}
if (joint_type == "revolute"){
Vector axis, origin;
// mandatory axis
if (!getVector(joint_xml->FirstChildElement("axis"), "xyz", axis))
{cerr << "Revolute joint does not spacify axis" << endl; return false;}
// optional origin
if (!getVector(joint_xml->FirstChildElement("anchor"), "xyz", origin))
origin = Vector::Zero();
joint = Joint(joint_name, origin, axis, Joint::RotAxis);
}
else if (joint_type == "prismatic"){
Vector axis, origin;
// mandatory axis
if (!getVector(joint_xml->FirstChildElement("axis"), "xyz", axis))
{cerr << "Prismatic joint does not spacify axis" << endl; return false;};
// optional origin
if (!getVector(joint_xml->FirstChildElement("anchor"), "xyz", origin))
origin = Vector::Zero();
joint = Joint(joint_name, origin, axis, Joint::TransAxis);
}
else if (joint_type == "fixed"){
joint = Joint(joint_name, Joint::None);
}
else{
cerr << "Unknown joint type '" << joint_type << "'. Using fixed joint instead" << endl;
joint = Joint(joint_name, Joint::None);
}
return true;
}
bool getSegment(TiXmlElement *segment_xml, map<string, Joint>& joints, Segment& segment)
{
if (!segment_xml) return false;
// get segment name
string segment_name;
if (!getAtribute(segment_xml, "name", segment_name))
{cerr << "Segment does not have name" << endl; return false;}
// get mandetory frame
Frame frame;
if (!getFrame(segment_xml->FirstChildElement("origin"), frame))
{cerr << "Segment does not have origin" << endl; return false;}
// get mandetory joint
string joint_name;
if (!getAtribute(segment_xml->FirstChildElement("joint"), "name", joint_name))
{cerr << "Segment does not specify joint name" << endl; return false;}
Joint joint;
map<string, Joint>::iterator it = joints.find(joint_name);
if (it != joints.end())
joint = it->second;
else if (getJoint(segment_xml->FirstChildElement("joint"), joint));
else {cerr << "Could not find joint " << joint_name << " in segment" << endl; return false;}
if (joint.getType() != Joint::None)
joint = Joint(joint_name, frame*(joint.JointOrigin()), joint.JointAxis(), joint.getType());
// get optional inertia
RigidBodyInertia inertia(0.0);
getInertia(segment_xml->FirstChildElement("inertial"), inertia);
segment = Segment(segment_name, joint, frame, inertia);
return true;
}
void addChildrenToTree(const string& root, const map<string, Segment>& segments, const map<string, string>& parents, Tree& tree)
{
// add root segments
if (tree.addSegment(segments.find(root)->second, parents.find(root)->second)){
cout << "Added segment " << root << " to " << parents.find(root)->second << endl;
// find children
for (map<string, string>::const_iterator p=parents.begin(); p!=parents.end(); p++)
if (p->second == root) addChildrenToTree(p->first, segments, parents, tree);
}
else {cerr << "Failed to add segment to tree" << endl;}
}
bool getTree(TiXmlElement *robot_xml, Tree& tree)
{
ROS_ERROR("You are using the deprecated kdl parser. Please update to the new kdl parser. See www.ros.org/wiki/kdl_parser for more details");
cout << "Parsing robot xml" << endl;
if (!robot_xml) return false;
// Constructs the joints
TiXmlElement *joint_xml = NULL;
Joint joint;
map<string, Joint> joints;
for (joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")){
// get joint name
string joint_name;
if (!getAtribute(joint_xml, "name", joint_name))
{cerr << "Joint does not have name" << endl; return false;}
// build joint
if (!getJoint(joint_xml, joint))
{cerr << "Constructing joint " << joint_name << " failed" << endl; return false;}
joints[joint.getName()] = joint;
}
// Constructs the segments
TiXmlElement *segment_xml = NULL;
Segment segment;
map<string, Segment> segments;
map<string, string> parents;
for (segment_xml = robot_xml->FirstChildElement("link"); segment_xml; segment_xml = segment_xml->NextSiblingElement("link")){
// get segment name
string segment_name;
if (!getAtribute(segment_xml, "name", segment_name))
{cerr << "Segment does not have name" << endl; return false;}
// get segment parent
string segment_parent;
if (!getAtribute(segment_xml->FirstChildElement("parent"), "name", segment_parent))
{cerr << "Segment " << segment_name << " does not have parent" << endl; return false;}
// build segment
if (!getSegment(segment_xml, joints, segment))
{cerr << "Constructing segment " << segment_name << " failed" << endl; return false;}
segments[segment.getName()] = segment;
parents[segment.getName()] = segment_parent;
}
// fail if no segments were found
if (segments.empty()){
cerr << "Did not find any segments" << endl;
return false;
}
// find the root segment: the parent segment that does not exist
string root;
for (map<string, string>::const_iterator p=parents.begin(); p!=parents.end(); p++)
if (segments.find(p->second) == segments.end())
root = p->first;
cout << parents[root] << " is root segment " << endl;
tree = Tree(parents[root]);
// add all segments to tree
addChildrenToTree(root, segments, parents, tree);
return true;
}
bool treeFromFile(const string& file, Tree& tree)
{
TiXmlDocument urdf_xml;
urdf_xml.LoadFile(file);
TiXmlElement *root = urdf_xml.FirstChildElement("robot");
if (!root){
cerr << "Could not parse the xml" << endl;
return false;
}
return getTree(root, tree);
}
bool treeFromString(const string& xml, Tree& tree)
{
TiXmlDocument urdf_xml;
urdf_xml.Parse(xml.c_str());
TiXmlElement *root = urdf_xml.FirstChildElement("robot");
if (!root){
cerr << "Could not parse the xml" << endl;
return false;
}
return getTree(root, tree);
}
bool treeFromXml(TiXmlElement *root, Tree& tree)
{
return getTree(root, tree);
}
}

99
kdl_parser/test/example_xml.cpp
View File

@ -1,99 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include "kdl_parser/xml_parser.hpp"
#include <iostream>
using namespace KDL;
using namespace std;
int main()
{
Tree my_tree;
if (!treeFromFile("pr2_desc.xml", my_tree)) return -1;
// walk through tree
SegmentMap::const_iterator root = my_tree.getRootSegment();
cout << "Found root " << root->second.segment.getName() << " with " << root->second.children.size() << " children" << endl;
for (unsigned int i=0; i<root->second.children.size(); i++){
SegmentMap::const_iterator child = root->second.children[i];
cout << " - child " << i+1 << ": " << child->second.segment.getName() << " has joint " << child->second.segment.getJoint().getName()
<< " and " << child->second.children.size() << " children" << endl;
for (unsigned int j=0; j<child->second.children.size(); j++){
SegmentMap::const_iterator grandchild = child->second.children[j];
cout << " - grandchild " << j+1 << ": " << grandchild->second.segment.getName() << " has joint " << grandchild->second.segment.getJoint().getName()
<< " and " << grandchild->second.children.size() << " children" << endl;
}
}
// extract chains from tree
Chain chain1, chain2;
my_tree.getChain("l_gripper_palm_link", "r_gripper_palm_link", chain1);
my_tree.getChain("r_gripper_palm_link", "l_gripper_palm_link", chain2);
cout << "Got chain1 with " << chain1.getNrOfJoints() << " joints and " << chain1.getNrOfSegments() << " segments" << endl;
cout << "Got chain2 with " << chain2.getNrOfJoints() << " joints and " << chain2.getNrOfSegments() << " segments" << endl;
JntArray jnt1(chain1.getNrOfJoints());
JntArray jnt2(chain1.getNrOfJoints());
for (int i=0; i<(int)chain1.getNrOfJoints(); i++){
jnt1(i) = (i+1)*2;
jnt2((int)chain1.getNrOfJoints()-i-1) = -jnt1(i);
}
for (int i=0; i<(int)chain1.getNrOfJoints(); i++)
cout << "jnt 1 -- jnt 2 " << jnt1(i) << " -- " << jnt2(i) << endl;
ChainFkSolverPos_recursive solver1(chain1);
ChainFkSolverPos_recursive solver2(chain2);
Frame f1, f2;
solver1.JntToCart(jnt1, f1);
solver2.JntToCart(jnt2, f2);
cout << "frame 1 " << f1 << endl;
cout << "frame 2 " << f2.Inverse() << endl;
// copy tree
Tree copy = my_tree;
copy.getChain("l_gripper_palm_link", "r_gripper_palm_link", chain1);
copy.getChain("r_gripper_palm_link", "l_gripper_palm_link", chain2);
cout << "Got chain1 with " << chain1.getNrOfJoints() << " joints and " << chain1.getNrOfSegments() << " segments" << endl;
cout << "Got chain2 with " << chain2.getNrOfJoints() << " joints and " << chain2.getNrOfSegments() << " segments" << endl;
}