remove deprecated files and methods
This commit is contained in:
meeussen
parent
37423202e1
commit
379c125184
|
|
@ -23,10 +23,7 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
|
||||||
#uncomment if you have defined services
|
#uncomment if you have defined services
|
||||||
#gensrv()
|
#gensrv()
|
||||||
|
|
||||||
rosbuild_add_library(${PROJECT_NAME} src/xml_parser.cpp src/kdl_parser.cpp)
|
rosbuild_add_library(${PROJECT_NAME} src/kdl_parser.cpp)
|
||||||
|
|
||||||
rosbuild_add_executable(example_xml test/example_xml.cpp )
|
|
||||||
target_link_libraries(example_xml ${PROJECT_NAME})
|
|
||||||
|
|
||||||
rosbuild_add_executable(example test/example.cpp )
|
rosbuild_add_executable(example test/example.cpp )
|
||||||
target_link_libraries(example ${PROJECT_NAME})
|
target_link_libraries(example ${PROJECT_NAME})
|
||||||
|
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -66,15 +66,6 @@ bool treeFromString(const string& xml, KDL::Tree& tree);
|
||||||
*/
|
*/
|
||||||
bool treeFromXml(TiXmlDocument *xml_doc, 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
|
/** Constructs a KDL tree from a URDF robot model
|
||||||
* \param robot_model The URDF robot model
|
* \param robot_model The URDF robot model
|
||||||
* \param tree The resulting KDL Tree
|
* \param tree The resulting KDL Tree
|
||||||
|
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -167,12 +167,5 @@ bool treeFromUrdfModel(const urdf::Model& robot_model, Tree& tree)
|
||||||
return true;
|
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);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -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);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
@ -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;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue