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

urdfdom as dep

This commit is contained in:
Ioan Sucan 2012-06-04 14:49:47 -07:00
parent c20f647b03
commit bca3cb4ba9
19 changed files with 12 additions and 2935 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
kdl_parser/src/kdl_parser.cpp
View File

@ -36,7 +36,7 @@
#include "kdl_parser/kdl_parser.hpp" #include "kdl_parser/kdl_parser.hpp"
#include <kdl/frames_io.hpp> #include <kdl/frames_io.hpp>
#include <ros/console.h>
using namespace std; using namespace std;
using namespace KDL; using namespace KDL;

19
urdf_interface/CMakeLists.txt
View File

@ -1,19 +0,0 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
#rosbuild_init()
#set the default path for built executables to the "bin" directory
#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#rosbuild_add_boost_directories()

1
urdf_interface/Makefile
View File

@ -1 +0,0 @@
include $(shell rospack find mk)/cmake.mk

104
urdf_interface/include/urdf_interface/color.h
View File

@ -1,104 +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: Josh Faust */
#ifndef URDF_INTERFACE_COLOR_H
#define URDF_INTERFACE_COLOR_H
#include <string>
#include <vector>
#include <math.h>
#include <ros/console.h>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
namespace urdf
{
class Color
{
public:
Color() {this->clear();};
float r;
float g;
float b;
float a;
void clear()
{
r = g = b = 0.0f;
a = 1.0f;
}
bool init(const std::string &vector_str)
{
this->clear();
std::vector<std::string> pieces;
std::vector<float> rgba;
boost::split( pieces, vector_str, boost::is_any_of(" "));
for (unsigned int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
{
try
{
rgba.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("color rgba element (%s) is not a valid float",pieces[i].c_str());
return false;
}
}
}
if (rgba.size() != 4)
{
ROS_ERROR("Color contains %i elements instead of 4 elements", (int)rgba.size());
return false;
}
this->r = rgba[0];
this->g = rgba[1];
this->b = rgba[2];
this->a = rgba[3];
return true;
};
};
}
#endif

235
urdf_interface/include/urdf_interface/joint.h
View File

@ -1,235 +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 URDF_INTERFACE_JOINT_H
#define URDF_INTERFACE_JOINT_H
#include <string>
#include <vector>
#include <tinyxml.h>
#include <boost/shared_ptr.hpp>
#include "pose.h"
namespace urdf{
class Link;
class JointDynamics
{
public:
JointDynamics() { this->clear(); };
double damping;
double friction;
void clear()
{
damping = 0;
friction = 0;
};
bool initXml(TiXmlElement* config);
};
class JointLimits
{
public:
JointLimits() { this->clear(); };
double lower;
double upper;
double effort;
double velocity;
void clear()
{
lower = 0;
upper = 0;
effort = 0;
velocity = 0;
};
bool initXml(TiXmlElement* config);
};
/// \brief Parameters for Joint Safety Controllers
class JointSafety
{
public:
/// clear variables on construction
JointSafety() { this->clear(); };
///
/// IMPORTANT: The safety controller support is very much PR2 specific, not intended for generic usage.
///
/// Basic safety controller operation is as follows
///
/// current safety controllers will take effect on joints outside the position range below:
///
/// position range: [JointSafety::soft_lower_limit + JointLimits::velocity / JointSafety::k_position,
/// JointSafety::soft_uppper_limit - JointLimits::velocity / JointSafety::k_position]
///
/// if (joint_position is outside of the position range above)
/// velocity_limit_min = -JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_lower_limit)
/// velocity_limit_max = JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_upper_limit)
/// else
/// velocity_limit_min = -JointLimits::velocity
/// velocity_limit_max = JointLimits::velocity
///
/// velocity range: [velocity_limit_min + JointLimits::effort / JointSafety::k_velocity,
/// velocity_limit_max - JointLimits::effort / JointSafety::k_velocity]
///
/// if (joint_velocity is outside of the velocity range above)
/// effort_limit_min = -JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_min)
/// effort_limit_max = JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_max)
/// else
/// effort_limit_min = -JointLimits::effort
/// effort_limit_max = JointLimits::effort
///
/// Final effort command sent to the joint is saturated by [effort_limit_min,effort_limit_max]
///
/// Please see wiki for more details: http://www.ros.org/wiki/pr2_controller_manager/safety_limits
///
double soft_upper_limit;
double soft_lower_limit;
double k_position;
double k_velocity;
void clear()
{
soft_upper_limit = 0;
soft_lower_limit = 0;
k_position = 0;
k_velocity = 0;
};
bool initXml(TiXmlElement* config);
};
class JointCalibration
{
public:
JointCalibration() { this->clear(); };
double reference_position;
boost::shared_ptr<double> rising, falling;
void clear()
{
reference_position = 0;
};
bool initXml(TiXmlElement* config);
};
class JointMimic
{
public:
JointMimic() { this->clear(); };
double offset;
double multiplier;
std::string joint_name;
void clear()
{
offset = 0.0;
multiplier = 0.0;
joint_name.clear();
};
bool initXml(TiXmlElement* config);
};
class Joint
{
public:
Joint() { this->clear(); };
std::string name;
enum
{
UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED
} type;
/// \brief type_ meaning of axis_
/// ------------------------------------------------------
/// UNKNOWN unknown type
/// REVOLUTE rotation axis
/// PRISMATIC translation axis
/// FLOATING N/A
/// PLANAR plane normal axis
/// FIXED N/A
Vector3 axis;
/// child Link element
/// child link frame is the same as the Joint frame
std::string child_link_name;
/// parent Link element
/// origin specifies the transform from Parent Link to Joint Frame
std::string parent_link_name;
/// transform from Parent Link frame to Joint frame
Pose parent_to_joint_origin_transform;
/// Joint Dynamics
boost::shared_ptr<JointDynamics> dynamics;
/// Joint Limits
boost::shared_ptr<JointLimits> limits;
/// Unsupported Hidden Feature
boost::shared_ptr<JointSafety> safety;
/// Unsupported Hidden Feature
boost::shared_ptr<JointCalibration> calibration;
/// Option to Mimic another Joint
boost::shared_ptr<JointMimic> mimic;
bool initXml(TiXmlElement* xml);
void clear()
{
this->axis.clear();
this->child_link_name.clear();
this->parent_link_name.clear();
this->parent_to_joint_origin_transform.clear();
this->dynamics.reset();
this->limits.reset();
this->safety.reset();
this->calibration.reset();
this->type = UNKNOWN;
};
};
}
#endif

260
urdf_interface/include/urdf_interface/link.h
View File

@ -1,260 +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 URDF_INTERFACE_LINK_H
#define URDF_INTERFACE_LINK_H
#include <string>
#include <vector>
#include <tinyxml.h>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include "joint.h"
#include "color.h"
namespace urdf{
class Geometry
{
public:
enum {SPHERE, BOX, CYLINDER, MESH} type;
virtual bool initXml(TiXmlElement *) = 0;
};
class Sphere : public Geometry
{
public:
Sphere() { this->clear(); };
double radius;
void clear()
{
radius = 0;
};
bool initXml(TiXmlElement *);
};
class Box : public Geometry
{
public:
Box() { this->clear(); };
Vector3 dim;
void clear()
{
dim.clear();
};
bool initXml(TiXmlElement *);
};
class Cylinder : public Geometry
{
public:
Cylinder() { this->clear(); };
double length;
double radius;
void clear()
{
length = 0;
radius = 0;
};
bool initXml(TiXmlElement *);
};
class Mesh : public Geometry
{
public:
Mesh() { this->clear(); };
std::string filename;
Vector3 scale;
void clear()
{
filename.clear();
// default scale
scale.x = 1;
scale.y = 1;
scale.z = 1;
};
bool initXml(TiXmlElement *);
bool fileExists(std::string filename);
};
class Material
{
public:
Material() { this->clear(); };
std::string name;
std::string texture_filename;
Color color;
void clear()
{
color.clear();
texture_filename.clear();
name.clear();
};
bool initXml(TiXmlElement* config);
};
class Inertial
{
public:
Inertial() { this->clear(); };
Pose origin;
double mass;
double ixx,ixy,ixz,iyy,iyz,izz;
void clear()
{
origin.clear();
mass = 0;
ixx = ixy = ixz = iyy = iyz = izz = 0;
};
bool initXml(TiXmlElement* config);
};
class Visual
{
public:
Visual() { this->clear(); };
Pose origin;
boost::shared_ptr<Geometry> geometry;
std::string material_name;
boost::shared_ptr<Material> material;
void clear()
{
origin.clear();
material_name.clear();
material.reset();
geometry.reset();
this->group_name.clear();
};
bool initXml(TiXmlElement* config);
std::string group_name;
};
class Collision
{
public:
Collision() { this->clear(); };
Pose origin;
boost::shared_ptr<Geometry> geometry;
void clear()
{
origin.clear();
geometry.reset();
this->group_name.clear();
};
bool initXml(TiXmlElement* config);
std::string group_name;
};
class Link
{
public:
Link() { this->clear(); };
std::string name;
/// inertial element
boost::shared_ptr<Inertial> inertial;
/// visual element
boost::shared_ptr<Visual> visual;
/// collision element
boost::shared_ptr<Collision> collision;
/// a collection of visual elements, keyed by a string tag called "group"
std::map<std::string, boost::shared_ptr<std::vector<boost::shared_ptr<Visual> > > > visual_groups;
/// a collection of collision elements, keyed by a string tag called "group"
std::map<std::string, boost::shared_ptr<std::vector<boost::shared_ptr<Collision> > > > collision_groups;
/// Parent Joint element
/// explicitly stating "parent" because we want directional-ness for tree structure
/// every link can have one parent
boost::shared_ptr<Joint> parent_joint;
std::vector<boost::shared_ptr<Joint> > child_joints;
std::vector<boost::shared_ptr<Link> > child_links;
bool initXml(TiXmlElement* config);
boost::shared_ptr<Link> getParent() const
{return parent_link_.lock();};
void setParent(boost::shared_ptr<Link> parent);
void clear()
{
this->name.clear();
this->inertial.reset();
this->visual.reset();
this->collision.reset();
this->parent_joint.reset();
this->child_joints.clear();
this->child_links.clear();
this->collision_groups.clear();
};
void setParentJoint(boost::shared_ptr<Joint> child);
void addChild(boost::shared_ptr<Link> child);
void addChildJoint(boost::shared_ptr<Joint> child);
void addVisual(std::string group_name, boost::shared_ptr<Visual> visual);
boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > getVisuals(const std::string& group_name) const;
void addCollision(std::string group_name, boost::shared_ptr<Collision> collision);
boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > getCollisions(const std::string& group_name) const;
private:
boost::weak_ptr<Link> parent_link_;
};
}
#endif

224
urdf_interface/include/urdf_interface/model.h
View File

@ -1,224 +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 URDF_INTERFACE_MODEL_H
#define URDF_INTERFACE_MODEL_H
#include <string>
#include <map>
#include <boost/function.hpp>
#include <urdf_interface/link.h>
namespace urdf {
class ModelInterface
{
public:
boost::shared_ptr<const Link> getRoot(void) const{return this->root_link_;};
boost::shared_ptr<const Link> getLink(const std::string& name) const
{
boost::shared_ptr<const Link> ptr;
if (this->links_.find(name) == this->links_.end())
ptr.reset();
else
ptr = this->links_.find(name)->second;
return ptr;
};
boost::shared_ptr<const Joint> getJoint(const std::string& name) const
{
boost::shared_ptr<const Joint> ptr;
if (this->joints_.find(name) == this->joints_.end())
ptr.reset();
else
ptr = this->joints_.find(name)->second;
return ptr;
};
const std::string& getName() const {return name_;};
void getLinks(std::vector<boost::shared_ptr<Link> >& links) const
{
for (std::map<std::string,boost::shared_ptr<Link> >::const_iterator link = this->links_.begin();link != this->links_.end(); link++)
{
links.push_back(link->second);
}
};
void clear()
{
name_.clear();
this->links_.clear();
this->joints_.clear();
this->materials_.clear();
this->root_link_.reset();
};
/// non-const getLink()
void getLink(const std::string& name,boost::shared_ptr<Link> &link) const
{
boost::shared_ptr<Link> ptr;
if (this->links_.find(name) == this->links_.end())
ptr.reset();
else
ptr = this->links_.find(name)->second;
link = ptr;
};
/// non-const getMaterial()
boost::shared_ptr<Material> getMaterial(const std::string& name) const
{
boost::shared_ptr<Material> ptr;
if (this->materials_.find(name) == this->materials_.end())
ptr.reset();
else
ptr = this->materials_.find(name)->second;
return ptr;
};
/// in initXml(), onece all links are loaded,
/// it's time to build a tree
bool initTree(std::map<std::string, std::string> &parent_link_tree)
{
// loop through all joints, for every link, assign children links and children joints
for (std::map<std::string,boost::shared_ptr<Joint> >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
{
std::string parent_link_name = joint->second->parent_link_name;
std::string child_link_name = joint->second->child_link_name;
ROS_DEBUG("build tree: joint: '%s' has parent link '%s' and child link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str());
if (parent_link_name.empty() || child_link_name.empty())
{
ROS_ERROR(" Joint %s is missing a parent and/or child link specification.",(joint->second)->name.c_str());
return false;
}
else
{
// find child and parent links
boost::shared_ptr<Link> child_link, parent_link;
this->getLink(child_link_name, child_link);
if (!child_link)
{
ROS_ERROR(" child link '%s' of joint '%s' not found", child_link_name.c_str(), joint->first.c_str() );
return false;
}
this->getLink(parent_link_name, parent_link);
if (!parent_link)
{
ROS_ERROR(" parent link '%s' of joint '%s' not found. The Boxturtle urdf parser used to automatically add this link for you, but this is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint from your urdf file, or add \"<link name=\"%s\" />\" to your urdf file.", parent_link_name.c_str(), joint->first.c_str(), parent_link_name.c_str() );
return false;
}
//set parent link for child link
child_link->setParent(parent_link);
//set parent joint for child link
child_link->setParentJoint(joint->second);
//set child joint for parent link
parent_link->addChildJoint(joint->second);
//set child link for parent link
parent_link->addChild(child_link);
// fill in child/parent string map
parent_link_tree[child_link->name] = parent_link_name;
ROS_DEBUG(" now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size());
}
}
return true;
};
/// in initXml(), onece tree is built,
/// it's time to find the root Link
bool initRoot(std::map<std::string, std::string> &parent_link_tree)
{
this->root_link_.reset();
// find the links that have no parent in the tree
for (std::map<std::string, boost::shared_ptr<Link> >::iterator l=this->links_.begin(); l!=this->links_.end(); l++)
{
std::map<std::string, std::string >::iterator parent = parent_link_tree.find(l->first);
if (parent == parent_link_tree.end())
{
// store root link
if (!this->root_link_)
{
getLink(l->first, this->root_link_);
}
// we already found a root link
else{
ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), l->first.c_str());
return false;
}
}
}
if (!this->root_link_)
{
ROS_ERROR("No root link found. The robot xml is not a valid tree.");
return false;
}
ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str());
return true;
};
/// \brief complete list of Links
std::map<std::string, boost::shared_ptr<Link> > links_;
/// \brief complete list of Joints
std::map<std::string, boost::shared_ptr<Joint> > joints_;
/// \brief complete list of Materials
std::map<std::string, boost::shared_ptr<Material> > materials_;
std::string name_;
/// ModelInterface is restricted to a tree for now, which means there exists one root link
/// typically, root link is the world(inertial). Where world is a special link
/// or is the root_link_ the link attached to the world by PLANAR/FLOATING joint?
/// hmm...
boost::shared_ptr<Link> root_link_;
};
}
#endif

308
urdf_interface/include/urdf_interface/pose.h
View File

@ -1,308 +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 URDF_INTERFACE_POSE_H
#define URDF_INTERFACE_POSE_H
#include <string>
#include <vector>
#include <math.h>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <tinyxml.h> // FIXME: remove parser from here
#include <ros/console.h>
namespace urdf{
class Vector3
{
public:
Vector3(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;};
Vector3() {this->clear();};
double x;
double y;
double z;
void clear() {this->x=this->y=this->z=0.0;};
bool init(const std::string &vector_str)
{
this->clear();
std::vector<std::string> pieces;
std::vector<double> xyz;
boost::split( pieces, vector_str, boost::is_any_of(" "));
for (unsigned int i = 0; i < pieces.size(); ++i){
if (pieces[i] != ""){
try
{
xyz.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("Vector3 xyz element (%s) is not a valid float",pieces[i].c_str());
return false;
}
}
}
if (xyz.size() != 3) {
ROS_ERROR("Vector contains %i elements instead of 3 elements", (int)xyz.size());
return false;
}
this->x = xyz[0];
this->y = xyz[1];
this->z = xyz[2];
return true;
};
Vector3 operator+(Vector3 vec)
{
return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z);
};
};
class Rotation
{
public:
Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;};
Rotation() {this->clear();};
void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const
{
quat_x = this->x;
quat_y = this->y;
quat_z = this->z;
quat_w = this->w;
};
void getRPY(double &roll,double &pitch,double &yaw) const
{
double sqw;
double sqx;
double sqy;
double sqz;
sqx = this->x * this->x;
sqy = this->y * this->y;
sqz = this->z * this->z;
sqw = this->w * this->w;
roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz);
double sarg = -2 * (this->x*this->z - this->w*this->y);
pitch = sarg <= -1.0 ? -0.5*M_PI : (sarg >= 1.0 ? 0.5*M_PI : asin(sarg));
yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz);
};
void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w)
{
this->x = quat_x;
this->y = quat_y;
this->z = quat_z;
this->w = quat_w;
this->normalize();
};
void setFromRPY(double roll, double pitch, double yaw)
{
double phi, the, psi;
phi = roll / 2.0;
the = pitch / 2.0;
psi = yaw / 2.0;
this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi);
this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi);
this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi);
this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi);
this->normalize();
};
double x,y,z,w;
bool init(const std::string &rotation_str)
{
this->clear();
Vector3 rpy;
if (!rpy.init(rotation_str))
return false;
else
{
this->setFromRPY(rpy.x,rpy.y,rpy.z);
return true;
}
};
void clear() { this->x=this->y=this->z=0.0;this->w=1.0; }
void normalize()
{
double s = sqrt(this->x * this->x +
this->y * this->y +
this->z * this->z +
this->w * this->w);
if (s == 0.0)
{
this->x = 0.0;
this->y = 0.0;
this->z = 0.0;
this->w = 1.0;
}
else
{
this->x /= s;
this->y /= s;
this->z /= s;
this->w /= s;
}
};
// Multiplication operator (copied from gazebo)
Rotation operator*( const Rotation &qt ) const
{
Rotation c;
c.x = this->w * qt.x + this->x * qt.w + this->y * qt.z - this->z * qt.y;
c.y = this->w * qt.y - this->x * qt.z + this->y * qt.w + this->z * qt.x;
c.z = this->w * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.w;
c.w = this->w * qt.w - this->x * qt.x - this->y * qt.y - this->z * qt.z;
return c;
};
/// Rotate a vector using the quaternion
Vector3 operator*(Vector3 vec) const
{
Rotation tmp;
Vector3 result;
tmp.w = 0.0;
tmp.x = vec.x;
tmp.y = vec.y;
tmp.z = vec.z;
tmp = (*this) * (tmp * this->GetInverse());
result.x = tmp.x;
result.y = tmp.y;
result.z = tmp.z;
return result;
};
// Get the inverse of this quaternion
Rotation GetInverse() const
{
Rotation q;
double norm = this->w*this->w+this->x*this->x+this->y*this->y+this->z*this->z;
if (norm > 0.0)
{
q.w = this->w / norm;
q.x = -this->x / norm;
q.y = -this->y / norm;
q.z = -this->z / norm;
}
return q;
};
};
class Pose
{
public:
Pose() { this->clear(); };
Vector3 position;
Rotation rotation;
void clear()
{
this->position.clear();
this->rotation.clear();
};
bool initXml(TiXmlElement* xml)
{
this->clear();
if (!xml)
{
ROS_DEBUG("parsing pose: xml empty");
return false;
}
else
{
const char* xyz_str = xml->Attribute("xyz");
if (xyz_str == NULL)
{
ROS_DEBUG("parsing pose: no xyz, using default values.");
return true;
}
else
{
if (!this->position.init(xyz_str))
{
ROS_ERROR("malformed xyz");
this->position.clear();
return false;
}
}
const char* rpy_str = xml->Attribute("rpy");
if (rpy_str == NULL)
{
ROS_DEBUG("parsing pose: no rpy, using default values.");
return true;
}
else
{
if (!this->rotation.init(rpy_str))
{
ROS_ERROR("malformed rpy");
return false;
this->rotation.clear();
}
}
return true;
}
};
};
}
#endif

8
urdf_interface/manifest.xml
View File

@ -1,17 +1,19 @@
<package> <package>
<description brief="URDF C++ interface."> <description brief="URDF C++ interface.">
This package is DEPRECATED. Please use the urdfdom dependency directly instead of this package.
This package contains URDF C++ interface headers that define the This package contains URDF C++ interface headers that define the
user API to the C++ URDF model. user API to the C++ URDF model.
</description> </description>
<author>Wim Meeussen, John Hsu</author> <author>Wim Meeussen, John Hsu</author>
<license>BSD</license> <license>BSD</license>
<review status="Doc reviewed" notes=""/> <review status="deprecated" notes=" Please use the urdfdom dependency directly."/>
<url>http://ros.org/wiki/urdf_interface</url> <url>http://ros.org/wiki/urdf_interface</url>
<rosdep name="tinyxml" /> <rosdep name="urdfdom" />
<export> <export>
<cpp cflags="-I${prefix}/include"/> <cpp cflags="`pkg-config --cflags urdfdom`"/>
</export> </export>
</package> </package>

38
urdf_parser/CMakeLists.txt
View File

@ -1,38 +0,0 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
set(ROS_BUILD_TYPE Debug)
rosbuild_init()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#rosbuild_genmsg()
rosbuild_gensrv()
#common commands for building c++ executables and libraries
rosbuild_add_library(${PROJECT_NAME} src/link.cpp src/joint.cpp src/urdf_parser.cpp)
target_link_libraries(${PROJECT_NAME} tinyxml)
rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
rosbuild_add_executable(check_urdf src/check_urdf.cpp)
target_link_libraries(check_urdf ${PROJECT_NAME})
rosbuild_add_executable(urdf_to_graphiz src/urdf_to_graphiz.cpp)
target_link_libraries(urdf_to_graphiz ${PROJECT_NAME})
rosbuild_add_executable(mem_test test/memtest.cpp)
target_link_libraries(mem_test ${PROJECT_NAME})

1
urdf_parser/Makefile
View File

@ -1 +0,0 @@
include $(shell rospack find mk)/cmake.mk

53
urdf_parser/include/urdf_parser/urdf_parser.h
View File

@ -1,53 +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 URDF_PARSER_URDF_PARSER_H
#define URDF_PARSER_URDF_PARSER_H
#include <string>
#include <map>
#include <tinyxml.h>
#include <boost/function.hpp>
#include <urdf_interface/model.h>
namespace urdf{
boost::shared_ptr<ModelInterface> parseURDF(const std::string &xml_string);
}
#endif

18
urdf_parser/manifest.xml
View File

@ -1,5 +1,7 @@
<package> <package>
<description brief="URDF Xml robot description parser."> <description brief="URDF Xml robot description parser.">
This package is DEPRECATED. Please use the urdfdom dependency directly instead of this package.
This package contains a C++ parser for the Unified Robot This package contains a C++ parser for the Unified Robot
Description Format (URDF), which is an XML format for representing Description Format (URDF), which is an XML format for representing
a robot model. The parser reads a URDF XML robot description, and a robot model. The parser reads a URDF XML robot description, and
@ -9,21 +11,13 @@
</description> </description>
<author>Wim Meeussen, John Hsu, Rosen Diankov</author> <author>Wim Meeussen, John Hsu, Rosen Diankov</author>
<license>BSD</license> <license>BSD</license>
<review status="Doc reviewed" notes=""/> <review status="deprecated" notes="Please use the urdfdom dependency directly"/>
<url>http://ros.org/wiki/urdf_parser</url> <url>http://ros.org/wiki/urdf_parser</url>
<rosdep name="tinyxml" /> <rosdep name="urdfdom" />
<depend package="roscpp" /> <!-- Only to call ROS_ERROR --> <export>
<depend package="urdf_interface" /> <cpp cflags="`pkg-config --cflags urdfdom`" lflags="`pkg-config --libs urdfdom`"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lurdf_parser"/>
</export> </export>
<platform os="ubuntu" version="9.04"/>
<platform os="ubuntu" version="9.10"/>
<platform os="ubuntu" version="10.04"/>
</package> </package>

103
urdf_parser/src/check_urdf.cpp
View File

@ -1,103 +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 "urdf_parser/urdf_parser.h"
#include <iostream>
#include <fstream>
using namespace urdf;
void printTree(boost::shared_ptr<const Link> link,int level = 0)
{
level+=2;
int count = 0;
for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
{
if (*child)
{
for(int j=0;j<level;j++) std::cout << " "; //indent
std::cout << "child(" << (count++)+1 << "): " << (*child)->name << std::endl;
// first grandchild
printTree(*child,level);
}
else
{
for(int j=0;j<level;j++) std::cout << " "; //indent
std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
}
}
}
int main(int argc, char** argv)
{
if (argc < 2){
std::cerr << "Expect URDF xml file to parse" << std::endl;
return -1;
}
std::string xml_string;
std::fstream xml_file(argv[1], std::fstream::in);
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
boost::shared_ptr<ModelInterface> robot = parseURDF(xml_string);
if (!robot){
std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
return -1;
}
std::cout << "robot name is: " << robot->getName() << std::endl;
// get info from parser
std::cout << "---------- Successfully Parsed XML ---------------" << std::endl;
// get root link
boost::shared_ptr<const Link> root_link=robot->getRoot();
if (!root_link) return -1;
std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl;
// print entire tree
printTree(root_link);
return 0;
}

566
urdf_parser/src/joint.cpp
View File

@ -1,566 +0,0 @@
/*********************************************************************
* Software Ligcense 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: John Hsu */
#include <urdf_interface/joint.h>
#include <ros/console.h>
#include <boost/lexical_cast.hpp>
namespace urdf{
bool JointDynamics::initXml(TiXmlElement* config)
{
this->clear();
// Get joint damping
const char* damping_str = config->Attribute("damping");
if (damping_str == NULL){
ROS_DEBUG("joint dynamics: no damping, defaults to 0");
this->damping = 0;
}
else
{
try
{
this->damping = boost::lexical_cast<double>(damping_str);
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("damping value (%s) is not a float",damping_str);
return false;
}
}
// Get joint friction
const char* friction_str = config->Attribute("friction");
if (friction_str == NULL){
ROS_DEBUG("joint dynamics: no friction, defaults to 0");
this->friction = 0;
}
else
{
try
{
this->friction = boost::lexical_cast<double>(friction_str);
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("friction value (%s) is not a float",friction_str);
return false;
}
}
if (damping_str == NULL && friction_str == NULL)
{
ROS_ERROR("joint dynamics element specified with no damping and no friction");
return false;
}
else{
ROS_DEBUG("joint dynamics: damping %f and friction %f", damping, friction);
return true;
}
}
bool JointLimits::initXml(TiXmlElement* config)
{
this->clear();
// Get lower joint limit
const char* lower_str = config->Attribute("lower");
if (lower_str == NULL){
ROS_DEBUG("joint limit: no lower, defaults to 0");
this->lower = 0;
}
else
{
try
{
this->lower = boost::lexical_cast<double>(lower_str);
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("lower value (%s) is not a float",lower_str);
return false;
}
}
// Get upper joint limit
const char* upper_str = config->Attribute("upper");
if (upper_str == NULL){
ROS_DEBUG("joint limit: no upper, , defaults to 0");
this->upper = 0;
}
else
{
try
{
this->upper = boost::lexical_cast<double>(upper_str);
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("upper value (%s) is not a float",upper_str);
return false;
}
}
// Get joint effort limit
const char* effort_str = config->Attribute("effort");
if (effort_str == NULL){
ROS_ERROR("joint limit: no effort");
return false;
}
else
{
try
{
this->effort = boost::lexical_cast<double>(effort_str);
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("effort value (%s) is not a float",effort_str);
return false;
}
}
// Get joint velocity limit
const char* velocity_str = config->Attribute("velocity");
if (velocity_str == NULL){
ROS_ERROR("joint limit: no velocity");
return false;
}
else
{
try
{
this->velocity = boost::lexical_cast<double>(velocity_str);
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("velocity value (%s) is not a float",velocity_str);
return false;
}
}
return true;
}
bool JointSafety::initXml(TiXmlElement* config)
{
this->clear();
// Get soft_lower_limit joint limit
const char* soft_lower_limit_str = config->Attribute("soft_lower_limit");
if (soft_lower_limit_str == NULL)
{
ROS_DEBUG("joint safety: no soft_lower_limit, using default value");
this->soft_lower_limit = 0;
}
else
{
try
{
this->soft_lower_limit = boost::lexical_cast<double>(soft_lower_limit_str);
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("soft_lower_limit value (%s) is not a float",soft_lower_limit_str);
return false;
}
}
// Get soft_upper_limit joint limit
const char* soft_upper_limit_str = config->Attribute("soft_upper_limit");
if (soft_upper_limit_str == NULL)
{
ROS_DEBUG("joint safety: no soft_upper_limit, using default value");
this->soft_upper_limit = 0;
}
else
{
try
{
this->soft_upper_limit = boost::lexical_cast<double>(soft_upper_limit_str);
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("soft_upper_limit value (%s) is not a float",soft_upper_limit_str);
return false;
}
}
// Get k_position_ safety "position" gain - not exactly position gain
const char* k_position_str = config->Attribute("k_position");
if (k_position_str == NULL)
{
ROS_DEBUG("joint safety: no k_position, using default value");
this->k_position = 0;
}
else
{
try
{
this->k_position = boost::lexical_cast<double>(k_position_str);
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("k_position value (%s) is not a float",k_position_str);
return false;
}
}
// Get k_velocity_ safety velocity gain
const char* k_velocity_str = config->Attribute("k_velocity");
if (k_velocity_str == NULL)
{
ROS_ERROR("joint safety: no k_velocity");
return false;
}
else
{
try
{
this->k_velocity = boost::lexical_cast<double>(k_velocity_str);
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("k_velocity value (%s) is not a float",k_velocity_str);
return false;
}
}
return true;
}
bool JointCalibration::initXml(TiXmlElement* config)
{
this->clear();
// Get rising edge position
const char* rising_position_str = config->Attribute("rising");
if (rising_position_str == NULL)
{
ROS_DEBUG("joint calibration: no rising, using default value");
this->rising.reset();
}
else
{
try
{
this->rising.reset(new double(boost::lexical_cast<double>(rising_position_str)));
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("risingvalue (%s) is not a float",rising_position_str);
return false;
}
}
// Get falling edge position
const char* falling_position_str = config->Attribute("falling");
if (falling_position_str == NULL)
{
ROS_DEBUG("joint calibration: no falling, using default value");
this->falling.reset();
}
else
{
try
{
this->falling.reset(new double(boost::lexical_cast<double>(falling_position_str)));
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("fallingvalue (%s) is not a float",falling_position_str);
return false;
}
}
return true;
}
bool JointMimic::initXml(TiXmlElement* config)
{
this->clear();
// Get name of joint to mimic
const char* joint_name_str = config->Attribute("joint");
if (joint_name_str == NULL)
{
ROS_ERROR("joint mimic: no mimic joint specified");
//return false;
}
else
this->joint_name = joint_name_str;
// Get mimic multiplier
const char* multiplier_str = config->Attribute("multiplier");
if (multiplier_str == NULL)
{
ROS_DEBUG("joint mimic: no multiplier, using default value of 1");
this->multiplier = 1;
}
else
{
try
{
this->multiplier = boost::lexical_cast<double>(multiplier_str);
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("multiplier value (%s) is not a float",multiplier_str);
return false;
}
}
// Get mimic offset
const char* offset_str = config->Attribute("offset");
if (offset_str == NULL)
{
ROS_DEBUG("joint mimic: no offset, using default value of 0");
this->offset = 0;
}
else
{
try
{
this->offset = boost::lexical_cast<double>(offset_str);
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("offset value (%s) is not a float",offset_str);
return false;
}
}
return true;
}
bool Joint::initXml(TiXmlElement* config)
{
this->clear();
// Get Joint Name
const char *name = config->Attribute("name");
if (!name)
{
ROS_ERROR("unnamed joint found");
return false;
}
this->name = name;
// Get transform from Parent Link to Joint Frame
TiXmlElement *origin_xml = config->FirstChildElement("origin");
if (!origin_xml)
{
ROS_DEBUG("Joint '%s' missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", this->name.c_str());
this->parent_to_joint_origin_transform.clear();
}
else
{
if (!this->parent_to_joint_origin_transform.initXml(origin_xml))
{
ROS_ERROR("Malformed parent origin element for joint '%s'", this->name.c_str());
this->parent_to_joint_origin_transform.clear();
return false;
}
}
// Get Parent Link
TiXmlElement *parent_xml = config->FirstChildElement("parent");
if (parent_xml)
{
const char *pname = parent_xml->Attribute("link");
if (!pname)
ROS_INFO("no parent link name specified for Joint link '%s'. this might be the root?", this->name.c_str());
else
{
this->parent_link_name = std::string(pname);
}
}
// Get Child Link
TiXmlElement *child_xml = config->FirstChildElement("child");
if (child_xml)
{
const char *pname = child_xml->Attribute("link");
if (!pname)
ROS_INFO("no child link name specified for Joint link '%s'.", this->name.c_str());
else
{
this->child_link_name = std::string(pname);
}
}
// Get Joint type
const char* type_char = config->Attribute("type");
if (!type_char)
{
ROS_ERROR("joint '%s' has no type, check to see if it's a reference.", this->name.c_str());
return false;
}
std::string type_str = type_char;
if (type_str == "planar")
{
type = PLANAR;
ROS_WARN("Planar joints are deprecated in the URDF!\n");
}
else if (type_str == "floating")
{
type = FLOATING;
ROS_WARN("Floating joints are deprecated in the URDF!\n");
}
else if (type_str == "revolute")
type = REVOLUTE;
else if (type_str == "continuous")
type = CONTINUOUS;
else if (type_str == "prismatic")
type = PRISMATIC;
else if (type_str == "fixed")
type = FIXED;
else
{
ROS_ERROR("Joint '%s' has no known type '%s'", this->name.c_str(), type_str.c_str());
return false;
}
// Get Joint Axis
if (this->type != FLOATING && this->type != FIXED)
{
// axis
TiXmlElement *axis_xml = config->FirstChildElement("axis");
if (!axis_xml){
ROS_DEBUG("no axis elemement for Joint link '%s', defaulting to (1,0,0) axis", this->name.c_str());
this->axis = Vector3(1.0, 0.0, 0.0);
}
else{
if (!axis_xml->Attribute("xyz")){
ROS_ERROR("no xyz attribute for axis element for Joint link '%s'", this->name.c_str());
}
else {
if (!this->axis.init(axis_xml->Attribute("xyz")))
{
ROS_ERROR("Malformed axis element for joint '%s'", this->name.c_str());
this->axis.clear();
return false;
}
}
}
}
// Get limit
TiXmlElement *limit_xml = config->FirstChildElement("limit");
if (limit_xml)
{
limits.reset(new JointLimits);
if (!limits->initXml(limit_xml))
{
ROS_ERROR("Could not parse limit element for joint '%s'", this->name.c_str());
limits.reset();
return false;
}
}
else if (this->type == REVOLUTE)
{
ROS_ERROR("Joint '%s' is of type REVOLUTE but it does not specify limits", this->name.c_str());
return false;
}
else if (this->type == PRISMATIC)
{
ROS_INFO("Joint '%s' is of type PRISMATIC without limits", this->name.c_str());
limits.reset();
}
// Get safety
TiXmlElement *safety_xml = config->FirstChildElement("safety_controller");
if (safety_xml)
{
safety.reset(new JointSafety);
if (!safety->initXml(safety_xml))
{
ROS_ERROR("Could not parse safety element for joint '%s'", this->name.c_str());
safety.reset();
return false;
}
}
// Get calibration
TiXmlElement *calibration_xml = config->FirstChildElement("calibration");
if (calibration_xml)
{
calibration.reset(new JointCalibration);
if (!calibration->initXml(calibration_xml))
{
ROS_ERROR("Could not parse calibration element for joint '%s'", this->name.c_str());
calibration.reset();
return false;
}
}
// Get Joint Mimic
TiXmlElement *mimic_xml = config->FirstChildElement("mimic");
if (mimic_xml)
{
mimic.reset(new JointMimic);
if (!mimic->initXml(mimic_xml))
{
ROS_ERROR("Could not parse mimic element for joint '%s'", this->name.c_str());
mimic.reset();
return false;
}
}
// Get Dynamics
TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
if (prop_xml)
{
dynamics.reset(new JointDynamics);
if (!dynamics->initXml(prop_xml))
{
ROS_ERROR("Could not parse joint_dynamics element for joint '%s'", this->name.c_str());
dynamics.reset();
return false;
}
}
return true;
}
}

648
urdf_parser/src/link.cpp
View File

@ -1,648 +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 <urdf_interface/link.h>
#include <ros/console.h>
#include <ros/package.h>
#include <fstream>
#include <boost/lexical_cast.hpp>
#include <algorithm>
namespace urdf{
boost::shared_ptr<Geometry> parseGeometry(TiXmlElement *g)
{
boost::shared_ptr<Geometry> geom;
if (!g) return geom;
TiXmlElement *shape = g->FirstChildElement();
if (!shape)
{
ROS_ERROR("Geometry tag contains no child element.");
return geom;
}
std::string type_name = shape->ValueStr();
if (type_name == "sphere")
geom.reset(new Sphere);
else if (type_name == "box")
geom.reset(new Box);
else if (type_name == "cylinder")
geom.reset(new Cylinder);
else if (type_name == "mesh")
geom.reset(new Mesh);
else
{
ROS_ERROR("Unknown geometry type '%s'", type_name.c_str());
return geom;
}
// clear geom object when fails to initialize
if (!geom->initXml(shape)){
ROS_ERROR("Geometry failed to parse");
geom.reset();
}
return geom;
}
bool Material::initXml(TiXmlElement *config)
{
bool has_rgb = false;
bool has_filename = false;
this->clear();
if (!config->Attribute("name"))
{
ROS_ERROR("Material must contain a name attribute");
return false;
}
this->name = config->Attribute("name");
// texture
TiXmlElement *t = config->FirstChildElement("texture");
if (t)
{
if (t->Attribute("filename"))
{
this->texture_filename = t->Attribute("filename");
has_filename = true;
}
else
{
ROS_ERROR("texture has no filename for Material %s",this->name.c_str());
}
}
// color
TiXmlElement *c = config->FirstChildElement("color");
if (c)
{
if (c->Attribute("rgba"))
{
if (!this->color.init(c->Attribute("rgba")))
{
ROS_ERROR("Material %s has malformed color rgba values.",this->name.c_str());
this->color.clear();
return false;
}
else
has_rgb = true;
}
else
{
ROS_ERROR("Material %s color has no rgba",this->name.c_str());
}
}
return (has_rgb || has_filename);
}
bool Inertial::initXml(TiXmlElement *config)
{
this->clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (!o)
{
ROS_DEBUG("Origin tag not present for inertial element, using default (Identity)");
this->origin.clear();
}
else
{
if (!this->origin.initXml(o))
{
ROS_ERROR("Inertial has a malformed origin tag");
this->origin.clear();
return false;
}
}
TiXmlElement *mass_xml = config->FirstChildElement("mass");
if (!mass_xml)
{
ROS_ERROR("Inertial element must have mass element");
return false;
}
if (!mass_xml->Attribute("value"))
{
ROS_ERROR("Inertial: mass element must have value attributes");
return false;
}
try
{
mass = boost::lexical_cast<double>(mass_xml->Attribute("value"));
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("mass (%s) is not a float",mass_xml->Attribute("value"));
return false;
}
TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
if (!inertia_xml)
{
ROS_ERROR("Inertial element must have inertia element");
return false;
}
if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
inertia_xml->Attribute("izz")))
{
ROS_ERROR("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
return false;
}
try
{
ixx = boost::lexical_cast<double>(inertia_xml->Attribute("ixx"));
ixy = boost::lexical_cast<double>(inertia_xml->Attribute("ixy"));
ixz = boost::lexical_cast<double>(inertia_xml->Attribute("ixz"));
iyy = boost::lexical_cast<double>(inertia_xml->Attribute("iyy"));
iyz = boost::lexical_cast<double>(inertia_xml->Attribute("iyz"));
izz = boost::lexical_cast<double>(inertia_xml->Attribute("izz"));
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("one of the inertia elements: ixx (%s) ixy (%s) ixz (%s) iyy (%s) iyz (%s) izz (%s) is not a valid double",
inertia_xml->Attribute("ixx"),
inertia_xml->Attribute("ixy"),
inertia_xml->Attribute("ixz"),
inertia_xml->Attribute("iyy"),
inertia_xml->Attribute("iyz"),
inertia_xml->Attribute("izz"));
return false;
}
return true;
}
bool Visual::initXml(TiXmlElement *config)
{
this->clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (!o)
{
ROS_DEBUG("Origin tag not present for visual element, using default (Identity)");
this->origin.clear();
}
else if (!this->origin.initXml(o))
{
ROS_ERROR("Visual has a malformed origin tag");
this->origin.clear();
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
geometry = parseGeometry(geom);
if (!geometry)
{
ROS_ERROR("Malformed geometry for Visual element");
return false;
}
// Material
TiXmlElement *mat = config->FirstChildElement("material");
if (!mat)
{
ROS_DEBUG("visual element has no material tag.");
}
else
{
// get material name
if (!mat->Attribute("name"))
{
ROS_ERROR("Visual material must contain a name attribute");
return false;
}
this->material_name = mat->Attribute("name");
// try to parse material element in place
this->material.reset(new Material);
if (!this->material->initXml(mat))
{
ROS_DEBUG("Could not parse material element in Visual block, maybe defined outside.");
this->material.reset();
}
else
{
ROS_DEBUG("Parsed material element in Visual block.");
}
}
// Group Tag (optional)
// collision blocks without a group tag are designated to the "default" group
const char *group_name_char = config->Attribute("group");
if (!group_name_char)
group_name = std::string("default");
else
group_name = std::string(group_name_char);
return true;
}
bool Collision::initXml(TiXmlElement* config)
{
this->clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (!o)
{
ROS_DEBUG("Origin tag not present for collision element, using default (Identity)");
this->origin.clear();
}
else if (!this->origin.initXml(o))
{
ROS_ERROR("Collision has a malformed origin tag");
this->origin.clear();
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
geometry = parseGeometry(geom);
if (!geometry)
{
ROS_ERROR("Malformed geometry for Collision element");
return false;
}
// Group Tag (optional)
// collision blocks without a group tag are designated to the "default" group
const char *group_name_char = config->Attribute("group");
if (!group_name_char)
group_name = std::string("default");
else
group_name = std::string(group_name_char);
return true;
}
bool Sphere::initXml(TiXmlElement *c)
{
this->clear();
this->type = SPHERE;
if (!c->Attribute("radius"))
{
ROS_ERROR("Sphere shape must have a radius attribute");
return false;
}
try
{
radius = boost::lexical_cast<double>(c->Attribute("radius"));
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("radius (%s) is not a valid float",c->Attribute("radius"));
return false;
}
return true;
}
bool Box::initXml(TiXmlElement *c)
{
this->clear();
this->type = BOX;
if (!c->Attribute("size"))
{
ROS_ERROR("Box shape has no size attribute");
return false;
}
if (!dim.init(c->Attribute("size")))
{
ROS_ERROR("Box shape has malformed size attribute");
dim.clear();
return false;
}
return true;
}
bool Cylinder::initXml(TiXmlElement *c)
{
this->clear();
this->type = CYLINDER;
if (!c->Attribute("length") ||
!c->Attribute("radius"))
{
ROS_ERROR("Cylinder shape must have both length and radius attributes");
return false;
}
try
{
length = boost::lexical_cast<double>(c->Attribute("length"));
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("length (%s) is not a valid float",c->Attribute("length"));
return false;
}
try
{
radius = boost::lexical_cast<double>(c->Attribute("radius"));
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("radius (%s) is not a valid float",c->Attribute("radius"));
return false;
}
return true;
}
bool Mesh::initXml(TiXmlElement *c)
{
this->clear();
this->type = MESH;
if (!c->Attribute("filename"))
{
ROS_ERROR("Mesh must contain a filename attribute");
return false;
}
filename = c->Attribute("filename");
if (c->Attribute("scale"))
{
if (!this->scale.init(c->Attribute("scale")))
{
ROS_ERROR("Mesh scale was specified, but could not be parsed");
this->scale.clear();
return false;
}
}
else
ROS_DEBUG("Mesh scale was not specified, default to (1,1,1)");
return true;
}
bool Link::initXml(TiXmlElement* config)
{
this->clear();
const char *name_char = config->Attribute("name");
if (!name_char)
{
ROS_ERROR("No name given for the link.");
return false;
}
name = std::string(name_char);
// Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial");
if (i)
{
inertial.reset(new Inertial);
if (!inertial->initXml(i))
{
ROS_ERROR("Could not parse inertial element for Link '%s'", this->name.c_str());
return false;
}
}
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
boost::shared_ptr<Visual> vis;
vis.reset(new Visual);
if (vis->initXml(vis_xml))
{
boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > viss = this->getVisuals(vis->group_name);
if (!viss)
{
// group does not exist, create one and add to map
viss.reset(new std::vector<boost::shared_ptr<Visual > >);
// new group name, create vector, add vector to map and add Visual to the vector
this->visual_groups.insert(make_pair(vis->group_name,viss));
ROS_DEBUG("successfully added a new visual group name '%s'",vis->group_name.c_str());
}
// group exists, add Visual to the vector in the map
viss->push_back(vis);
ROS_DEBUG("successfully added a new visual under group name '%s'",vis->group_name.c_str());
}
else
{
ROS_ERROR("Could not parse visual element for Link '%s'", this->name.c_str());
vis.reset();
return false;
}
}
// Visual (optional)
// Assign one single default visual pointer from the visual_groups map
this->visual.reset();
boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > default_visual = this->getVisuals("default");
if (!default_visual)
{
ROS_DEBUG("No 'default' visual group for Link '%s'", this->name.c_str());
}
else if (default_visual->empty())
{
ROS_DEBUG("'default' visual group is empty for Link '%s'", this->name.c_str());
}
else
{
if (default_visual->size() > 1)
ROS_WARN("'default' visual group has %d visuals for Link '%s', taking the first one as default",(int)default_visual->size(), this->name.c_str());
this->visual = (*default_visual->begin());
}
// Multiple Collisions (optional)
for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
{
boost::shared_ptr<Collision> col;
col.reset(new Collision);
if (col->initXml(col_xml))
{
boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > cols = this->getCollisions(col->group_name);
if (!cols)
{
// group does not exist, create one and add to map
cols.reset(new std::vector<boost::shared_ptr<Collision > >);
// new group name, create vector, add vector to map and add Collision to the vector
this->collision_groups.insert(make_pair(col->group_name,cols));
ROS_DEBUG("successfully added a new collision group name '%s'",col->group_name.c_str());
}
// group exists, add Collision to the vector in the map
cols->push_back(col);
ROS_DEBUG("successfully added a new collision under group name '%s'",col->group_name.c_str());
}
else
{
ROS_ERROR("Could not parse collision element for Link '%s'", this->name.c_str());
col.reset();
return false;
}
}
// Collision (optional)
// Assign one single default collision pointer from the collision_groups map
this->collision.reset();
boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > default_collision = this->getCollisions("default");
if (!default_collision)
{
ROS_DEBUG("No 'default' collision group for Link '%s'", this->name.c_str());
}
else if (default_collision->empty())
{
ROS_DEBUG("'default' collision group is empty for Link '%s'", this->name.c_str());
}
else
{
if (default_collision->size() > 1)
ROS_WARN("'default' collision group has %d collisions for Link '%s', taking the first one as default",(int)default_collision->size(), this->name.c_str());
this->collision = (*default_collision->begin());
}
return true;
}
void Link::addVisual(std::string group_name, boost::shared_ptr<Visual> visual)
{
boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > viss = this->getVisuals(group_name);
if (!viss)
{
// group does not exist, create one and add to map
viss.reset(new std::vector<boost::shared_ptr<Visual > >);
// new group name, create vector, add vector to map and add Visual to the vector
this->visual_groups.insert(make_pair(group_name,viss));
ROS_DEBUG("successfully added a new visual group name '%s'",group_name.c_str());
}
// group exists, add Visual to the vector in the map
std::vector<boost::shared_ptr<Visual > >::iterator vis_it = find(viss->begin(),viss->end(),visual);
if (vis_it != viss->end())
ROS_WARN("attempted to add a visual that already exists under group name '%s', skipping.",group_name.c_str());
else
viss->push_back(visual);
ROS_DEBUG("successfully added a new visual under group name '%s'",group_name.c_str());
}
boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > Link::getVisuals(const std::string& group_name) const
{
boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > ptr;
if (this->visual_groups.find(group_name) == this->visual_groups.end())
ptr.reset();
else
ptr = this->visual_groups.find(group_name)->second;
return ptr;
}
void Link::addCollision(std::string group_name, boost::shared_ptr<Collision> collision)
{
boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > viss = this->getCollisions(group_name);
if (!viss)
{
// group does not exist, create one and add to map
viss.reset(new std::vector<boost::shared_ptr<Collision > >);
// new group name, create vector, add vector to map and add Collision to the vector
this->collision_groups.insert(make_pair(group_name,viss));
ROS_DEBUG("successfully added a new collision group name '%s'",group_name.c_str());
}
// group exists, add Collision to the vector in the map
std::vector<boost::shared_ptr<Collision > >::iterator vis_it = find(viss->begin(),viss->end(),collision);
if (vis_it != viss->end())
ROS_WARN("attempted to add a collision that already exists under group name '%s', skipping.",group_name.c_str());
else
viss->push_back(collision);
ROS_DEBUG("successfully added a new collision under group name '%s'",group_name.c_str());
}
boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > Link::getCollisions(const std::string& group_name) const
{
boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > ptr;
if (this->collision_groups.find(group_name) == this->collision_groups.end())
ptr.reset();
else
ptr = this->collision_groups.find(group_name)->second;
return ptr;
}
void Link::setParent(boost::shared_ptr<Link> parent)
{
this->parent_link_ = parent;
ROS_DEBUG("set parent Link '%s' for Link '%s'", parent->name.c_str(), this->name.c_str());
}
void Link::setParentJoint(boost::shared_ptr<Joint> parent)
{
this->parent_joint = parent;
ROS_DEBUG("set parent joint '%s' to Link '%s'", parent->name.c_str(), this->name.c_str());
}
void Link::addChild(boost::shared_ptr<Link> child)
{
this->child_links.push_back(child);
ROS_DEBUG("added child Link '%s' to Link '%s'", child->name.c_str(), this->name.c_str());
}
void Link::addChildJoint(boost::shared_ptr<Joint> child)
{
this->child_joints.push_back(child);
ROS_DEBUG("added child Joint '%s' to Link '%s'", child->name.c_str(), this->name.c_str());
}
}

217
urdf_parser/src/urdf_parser.cpp
View File

@ -1,217 +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 <ros/console.h>
#include <vector>
#include "urdf_parser/urdf_parser.h"
namespace urdf{
boost::shared_ptr<ModelInterface> parseURDF(const std::string &xml_string)
{
boost::shared_ptr<ModelInterface> model(new ModelInterface);
model->clear();
TiXmlDocument xml_doc;
xml_doc.Parse(xml_string.c_str());
TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
if (!robot_xml)
{
ROS_ERROR("Could not find the 'robot' element in the xml file");
model.reset();
return model;
}
// Get robot name
const char *name = robot_xml->Attribute("name");
if (!name)
{
ROS_ERROR("No name given for the robot.");
model.reset();
return model;
}
model->name_ = std::string(name);
// Get all Material elements
for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
{
boost::shared_ptr<Material> material;
material.reset(new Material);
if (material->initXml(material_xml))
{
if (model->getMaterial(material->name))
{
ROS_ERROR("material '%s' is not unique.", material->name.c_str());
material.reset();
model.reset();
return model;
}
else
{
model->materials_.insert(make_pair(material->name,material));
ROS_DEBUG("successfully added a new material '%s'", material->name.c_str());
}
}
else
{
ROS_ERROR("material xml is not initialized correctly");
material.reset();
model.reset();
return model;
}
}
// Get all Link elements
for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{
boost::shared_ptr<Link> link;
link.reset(new Link);
if (link->initXml(link_xml))
{
if (model->getLink(link->name))
{
ROS_ERROR("link '%s' is not unique.", link->name.c_str());
model.reset();
return model;
}
else
{
// set link visual material
ROS_DEBUG("setting link '%s' material", link->name.c_str());
if (link->visual)
{
if (!link->visual->material_name.empty())
{
if (model->getMaterial(link->visual->material_name))
{
ROS_DEBUG("setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str());
link->visual->material = model->getMaterial( link->visual->material_name.c_str() );
}
else
{
if (link->visual->material)
{
ROS_DEBUG("link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str());
model->materials_.insert(make_pair(link->visual->material->name,link->visual->material));
}
else
{
ROS_ERROR("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str());
model.reset();
return model;
}
}
}
}
model->links_.insert(make_pair(link->name,link));
ROS_DEBUG("successfully added a new link '%s'", link->name.c_str());
}
}
else
{
ROS_ERROR("link xml is not initialized correctly");
model.reset();
return model;
}
}
if (model->links_.empty()){
ROS_ERROR("No link elements found in urdf file");
model.reset();
return model;
}
// Get all Joint elements
for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{
boost::shared_ptr<Joint> joint;
joint.reset(new Joint);
if (joint->initXml(joint_xml))
{
if (model->getJoint(joint->name))
{
ROS_ERROR("joint '%s' is not unique.", joint->name.c_str());
model.reset();
return model;
}
else
{
model->joints_.insert(make_pair(joint->name,joint));
ROS_DEBUG("successfully added a new joint '%s'", joint->name.c_str());
}
}
else
{
ROS_ERROR("joint xml is not initialized correctly");
model.reset();
return model;
}
}
// every link has children links and joints, but no parents, so we create a
// local convenience data structure for keeping child->parent relations
std::map<std::string, std::string> parent_link_tree;
parent_link_tree.clear();
// building tree: name mapping
if (!model->initTree(parent_link_tree))
{
ROS_ERROR("failed to build tree");
model.reset();
return model;
}
// find the root link
if (!model->initRoot(parent_link_tree))
{
ROS_ERROR("failed to find root link");
model.reset();
return model;
}
return model;
}
}

120
urdf_parser/src/urdf_to_graphiz.cpp
View File

@ -1,120 +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:
*
* * Redstributions 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 "urdf_parser/urdf_parser.h"
#include <iostream>
#include <fstream>
using namespace urdf;
using namespace std;
void addChildLinkNames(boost::shared_ptr<const Link> link, ofstream& os)
{
os << "\"" << link->name << "\" [label=\"" << link->name << "\"];" << endl;
for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
addChildLinkNames(*child, os);
}
void addChildJointNames(boost::shared_ptr<const Link> link, ofstream& os)
{
double r, p, y;
for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){
(*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y);
os << "\"" << link->name << "\" -> \"" << (*child)->parent_joint->name
<< "\" [label=\"xyz: "
<< (*child)->parent_joint->parent_to_joint_origin_transform.position.x << " "
<< (*child)->parent_joint->parent_to_joint_origin_transform.position.y << " "
<< (*child)->parent_joint->parent_to_joint_origin_transform.position.z << " "
<< "\\nrpy: " << r << " " << p << " " << y << "\"]" << endl;
os << "\"" << (*child)->parent_joint->name << "\" -> \"" << (*child)->name << "\"" << endl;
addChildJointNames(*child, os);
}
}
void printTree(boost::shared_ptr<const Link> link, string file)
{
std::ofstream os;
os.open(file.c_str());
os << "digraph G {" << endl;
os << "node [shape=box];" << endl;
addChildLinkNames(link, os);
os << "node [shape=ellipse, color=blue, fontcolor=blue];" << endl;
addChildJointNames(link, os);
os << "}" << endl;
os.close();
}
int main(int argc, char** argv)
{
if (argc != 2){
std::cerr << "Usage: urdf_to_graphiz input.xml" << std::endl;
return -1;
}
// get the entire file
std::string xml_string;
std::fstream xml_file(argv[1], std::fstream::in);
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
boost::shared_ptr<ModelInterface> robot = parseURDF(xml_string);
if (!robot){
std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
return -1;
}
string output = robot->getName();
// print entire tree to file
printTree(robot->getRoot(), output+".gv");
cout << "Created file " << output << ".gv" << endl;
string command = "dot -Tpdf "+output+".gv -o "+output+".pdf";
system(command.c_str());
cout << "Created file " << output << ".pdf" << endl;
return 0;
}

22
urdf_parser/test/memtest.cpp
View File

@ -1,22 +0,0 @@
#include "urdf_parser/urdf_parser.h"
#include <ros/ros.h>
#include <fstream>
#include <iostream>
int main(int argc, char** argv){
ros::init(argc, argv, "memtest");
while (ros::ok()){
std::string xml_string;
std::fstream xml_file(argv[1], std::fstream::in);
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
urdf::parseURDF(xml_string);
}
}