规范模块目录和第三方依赖结构

third_party/kdl_parser/include/kdl_parser/kdl_parser.hpp

@@ -0,0 +1,104 @@
+/*********************************************************************
+* 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 KDL_PARSER__KDL_PARSER_HPP_
+#define KDL_PARSER__KDL_PARSER_HPP_
+
+#include <kdl/tree.hpp>
+#include <string>
+#include <urdf_model/model.h>
+#include <tinyxml2.h>
+#include <tinyxml.h>  // NOLINT
+
+#include "kdl_parser/visibility_control.hpp"
+
+namespace kdl_parser
+{
+
+/** Constructs a KDL tree from a file, given the file name
+ * \param file The filename from where to read the xml
+ * \param tree The resulting KDL Tree
+ * returns true on success, false on failure
+ */
+	KDL_PARSER_PUBLIC
+		bool treeFromFile(const std::string& file, KDL::Tree& tree);
+
+	KDL_PARSER_PUBLIC
+		bool treeFromFileDocument(const std::string& file, KDL::Tree& tree);
+
+	/** Constructs a KDL tree from the parameter server, given the parameter name
+ * \param param the name of the parameter on the parameter server
+ * \param tree The resulting KDL Tree
+ * returns true on success, false on failure or if built without ROS
+ */
+KDL_PARSER_PUBLIC
+bool treeFromParam(const std::string & param, KDL::Tree & tree);
+
+/** Constructs a KDL tree from a string containing xml
+ * \param xml A string containing the xml description of the robot
+ * \param tree The resulting KDL Tree
+ * returns true on success, false on failure
+ */
+KDL_PARSER_PUBLIC
+bool treeFromString(const std::string & xml, KDL::Tree & tree);
+
+/** Constructs a KDL tree from a TinyXML2 document
+ * \param[in] xml_doc The document containing the xml description of the robot
+ * \param[out] tree The resulting KDL Tree
+ * \return true on success, false on failure
+ */
+KDL_PARSER_PUBLIC
+bool treeFromXml(const tinyxml2::XMLDocument * xml_doc, KDL::Tree & tree);
+
+/** Constructs a KDL tree from a TinyXML document
+ * \param[in] xml_doc The document containing the xml description of the robot
+ * \param[out] tree The resulting KDL Tree
+ * returns true on success, false on failure
+ */
+KDL_PARSER_PUBLIC
+KDL_PARSER_DEPRECATED("TinyXML API is deprecated, use the TinyXML2 version instead")
+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
+ */
+KDL_PARSER_PUBLIC
+bool treeFromUrdfModel(const urdf::ModelInterface & robot_model, KDL::Tree & tree);
+}  // namespace kdl_parser
+
+#endif  // KDL_PARSER__KDL_PARSER_HPP_

third_party/kdl_parser/include/kdl_parser/visibility_control.hpp

@@ -0,0 +1,79 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2017, Open Source Robotics Foundation, 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 copyright holder 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.
+*********************************************************************/
+
+/* This header must be included by all kdl_parser headers which declare symbols
+ * which are defined in the kdl_parser library. When not building the kdl_parser
+ * library, i.e. when using the headers in other package's code, the contents
+ * of this header change the visibility of certain symbols which the kdl_parser
+ * library cannot have, but the consuming code must have inorder to link.
+ */
+
+#ifndef KDL_PARSER__VISIBILITY_CONTROL_HPP_
+#define KDL_PARSER__VISIBILITY_CONTROL_HPP_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define KDL_PARSER_EXPORT __attribute__ ((dllexport))
+    #define KDL_PARSER_IMPORT __attribute__ ((dllimport))
+    #define KDL_PARSER_DEPRECATED(msg) __attribute__((deprecated(msg)))
+  #else
+    #define KDL_PARSER_EXPORT __declspec(dllexport)
+    #define KDL_PARSER_IMPORT __declspec(dllimport)
+    #define KDL_PARSER_DEPRECATED(msg) __declspec(deprecated(msg))
+  #endif
+  #ifdef KDL_PARSER_BUILDING_DLL
+    #define KDL_PARSER_PUBLIC KDL_PARSER_EXPORT
+  #else
+    #define KDL_PARSER_PUBLIC KDL_PARSER_IMPORT
+  #endif
+  #define KDL_PARSER_PUBLIC_TYPE KDL_PARSER_PUBLIC
+  #define KDL_PARSER_LOCAL
+#else
+  #define KDL_PARSER_EXPORT __attribute__ ((visibility("default")))
+  #define KDL_PARSER_IMPORT
+  #if __GNUC__ >= 4
+    #define KDL_PARSER_PUBLIC __attribute__ ((visibility("default")))
+    #define KDL_PARSER_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define KDL_PARSER_PUBLIC
+    #define KDL_PARSER_LOCAL
+  #endif
+  #define KDL_PARSER_PUBLIC_TYPE
+  #define KDL_PARSER_DEPRECATED(msg) __attribute__((deprecated(msg)))
+#endif
+
+#endif  // KDL_PARSER__VISIBILITY_CONTROL_HPP_

third_party/kdl_parser/include/tinystr.h

@@ -0,0 +1,305 @@
+/*
+www.sourceforge.net/projects/tinyxml
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any
+damages arising from the use of this software.
+
+Permission is granted to anyone to use this software for any
+purpose, including commercial applications, and to alter it and
+redistribute it freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must
+not claim that you wrote the original software. If you use this
+software in a product, an acknowledgment in the product documentation
+would be appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and
+must not be misrepresented as being the original software.
+
+3. This notice may not be removed or altered from any source
+distribution.
+*/
+
+
+#ifndef TIXML_USE_STL
+
+#ifndef TIXML_STRING_INCLUDED
+#define TIXML_STRING_INCLUDED
+
+#include <assert.h>
+#include <string.h>
+
+/*	The support for explicit isn't that universal, and it isn't really
+	required - it is used to check that the TiXmlString class isn't incorrectly
+	used. Be nice to old compilers and macro it here:
+*/
+#if defined(_MSC_VER) && (_MSC_VER >= 1200 )
+	// Microsoft visual studio, version 6 and higher.
+	#define TIXML_EXPLICIT explicit
+#elif defined(__GNUC__) && (__GNUC__ >= 3 )
+	// GCC version 3 and higher.s
+	#define TIXML_EXPLICIT explicit
+#else
+	#define TIXML_EXPLICIT
+#endif
+
+
+/*
+   TiXmlString is an emulation of a subset of the std::string template.
+   Its purpose is to allow compiling TinyXML on compilers with no or poor STL support.
+   Only the member functions relevant to the TinyXML project have been implemented.
+   The buffer allocation is made by a simplistic power of 2 like mechanism : if we increase
+   a string and there's no more room, we allocate a buffer twice as big as we need.
+*/
+class TiXmlString
+{
+  public :
+	// The size type used
+  	typedef size_t size_type;
+
+	// Error value for find primitive
+	static const size_type npos; // = -1;
+
+
+	// TiXmlString empty constructor
+	TiXmlString () : rep_(&nullrep_)
+	{
+	}
+
+	// TiXmlString copy constructor
+	TiXmlString ( const TiXmlString & copy) : rep_(0)
+	{
+		init(copy.length());
+		memcpy(start(), copy.data(), length());
+	}
+
+	// TiXmlString constructor, based on a string
+	TIXML_EXPLICIT TiXmlString ( const char * copy) : rep_(0)
+	{
+		init( static_cast<size_type>( strlen(copy) ));
+		memcpy(start(), copy, length());
+	}
+
+	// TiXmlString constructor, based on a string
+	TIXML_EXPLICIT TiXmlString ( const char * str, size_type len) : rep_(0)
+	{
+		init(len);
+		memcpy(start(), str, len);
+	}
+
+	// TiXmlString destructor
+	~TiXmlString ()
+	{
+		quit();
+	}
+
+	TiXmlString& operator = (const char * copy)
+	{
+		return assign( copy, (size_type)strlen(copy));
+	}
+
+	TiXmlString& operator = (const TiXmlString & copy)
+	{
+		return assign(copy.start(), copy.length());
+	}
+
+
+	// += operator. Maps to append
+	TiXmlString& operator += (const char * suffix)
+	{
+		return append(suffix, static_cast<size_type>( strlen(suffix) ));
+	}
+
+	// += operator. Maps to append
+	TiXmlString& operator += (char single)
+	{
+		return append(&single, 1);
+	}
+
+	// += operator. Maps to append
+	TiXmlString& operator += (const TiXmlString & suffix)
+	{
+		return append(suffix.data(), suffix.length());
+	}
+
+
+	// Convert a TiXmlString into a null-terminated char *
+	const char * c_str () const { return rep_->str; }
+
+	// Convert a TiXmlString into a char * (need not be null terminated).
+	const char * data () const { return rep_->str; }
+
+	// Return the length of a TiXmlString
+	size_type length () const { return rep_->size; }
+
+	// Alias for length()
+	size_type size () const { return rep_->size; }
+
+	// Checks if a TiXmlString is empty
+	bool empty () const { return rep_->size == 0; }
+
+	// Return capacity of string
+	size_type capacity () const { return rep_->capacity; }
+
+
+	// single char extraction
+	const char& at (size_type index) const
+	{
+		assert( index < length() );
+		return rep_->str[ index ];
+	}
+
+	// [] operator
+	char& operator [] (size_type index) const
+	{
+		assert( index < length() );
+		return rep_->str[ index ];
+	}
+
+	// find a char in a string. Return TiXmlString::npos if not found
+	size_type find (char lookup) const
+	{
+		return find(lookup, 0);
+	}
+
+	// find a char in a string from an offset. Return TiXmlString::npos if not found
+	size_type find (char tofind, size_type offset) const
+	{
+		if (offset >= length()) return npos;
+
+		for (const char* p = c_str() + offset; *p != '\0'; ++p)
+		{
+		   if (*p == tofind) return static_cast< size_type >( p - c_str() );
+		}
+		return npos;
+	}
+
+	void clear ()
+	{
+		//Lee:
+		//The original was just too strange, though correct:
+		//	TiXmlString().swap(*this);
+		//Instead use the quit & re-init:
+		quit();
+		init(0,0);
+	}
+
+	/*	Function to reserve a big amount of data when we know we'll need it. Be aware that this
+		function DOES NOT clear the content of the TiXmlString if any exists.
+	*/
+	void reserve (size_type cap);
+
+	TiXmlString& assign (const char* str, size_type len);
+
+	TiXmlString& append (const char* str, size_type len);
+
+	void swap (TiXmlString& other)
+	{
+		Rep* r = rep_;
+		rep_ = other.rep_;
+		other.rep_ = r;
+	}
+
+  private:
+
+	void init(size_type sz) { init(sz, sz); }
+	void set_size(size_type sz) { rep_->str[ rep_->size = sz ] = '\0'; }
+	char* start() const { return rep_->str; }
+	char* finish() const { return rep_->str + rep_->size; }
+
+	struct Rep
+	{
+		size_type size, capacity;
+		char str[1];
+	};
+
+	void init(size_type sz, size_type cap)
+	{
+		if (cap)
+		{
+			// Lee: the original form:
+			//	rep_ = static_cast<Rep*>(operator new(sizeof(Rep) + cap));
+			// doesn't work in some cases of new being overloaded. Switching
+			// to the normal allocation, although use an 'int' for systems
+			// that are overly picky about structure alignment.
+			const size_type bytesNeeded = sizeof(Rep) + cap;
+			const size_type intsNeeded = ( bytesNeeded + sizeof(int) - 1 ) / sizeof( int ); 
+			rep_ = reinterpret_cast<Rep*>( new int[ intsNeeded ] );
+
+			rep_->str[ rep_->size = sz ] = '\0';
+			rep_->capacity = cap;
+		}
+		else
+		{
+			rep_ = &nullrep_;
+		}
+	}
+
+	void quit()
+	{
+		if (rep_ != &nullrep_)
+		{
+			// The rep_ is really an array of ints. (see the allocator, above).
+			// Cast it back before delete, so the compiler won't incorrectly call destructors.
+			delete [] ( reinterpret_cast<int*>( rep_ ) );
+		}
+	}
+
+	Rep * rep_;
+	static Rep nullrep_;
+
+} ;
+
+
+inline bool operator == (const TiXmlString & a, const TiXmlString & b)
+{
+	return    ( a.length() == b.length() )				// optimization on some platforms
+	       && ( strcmp(a.c_str(), b.c_str()) == 0 );	// actual compare
+}
+inline bool operator < (const TiXmlString & a, const TiXmlString & b)
+{
+	return strcmp(a.c_str(), b.c_str()) < 0;
+}
+
+inline bool operator != (const TiXmlString & a, const TiXmlString & b) { return !(a == b); }
+inline bool operator >  (const TiXmlString & a, const TiXmlString & b) { return b < a; }
+inline bool operator <= (const TiXmlString & a, const TiXmlString & b) { return !(b < a); }
+inline bool operator >= (const TiXmlString & a, const TiXmlString & b) { return !(a < b); }
+
+inline bool operator == (const TiXmlString & a, const char* b) { return strcmp(a.c_str(), b) == 0; }
+inline bool operator == (const char* a, const TiXmlString & b) { return b == a; }
+inline bool operator != (const TiXmlString & a, const char* b) { return !(a == b); }
+inline bool operator != (const char* a, const TiXmlString & b) { return !(b == a); }
+
+TiXmlString operator + (const TiXmlString & a, const TiXmlString & b);
+TiXmlString operator + (const TiXmlString & a, const char* b);
+TiXmlString operator + (const char* a, const TiXmlString & b);
+
+
+/*
+   TiXmlOutStream is an emulation of std::ostream. It is based on TiXmlString.
+   Only the operators that we need for TinyXML have been developped.
+*/
+class TiXmlOutStream : public TiXmlString
+{
+public :
+
+	// TiXmlOutStream << operator.
+	TiXmlOutStream & operator << (const TiXmlString & in)
+	{
+		*this += in;
+		return *this;
+	}
+
+	// TiXmlOutStream << operator.
+	TiXmlOutStream & operator << (const char * in)
+	{
+		*this += in;
+		return *this;
+	}
+
+} ;
+
+#endif	// TIXML_STRING_INCLUDED
+#endif	// TIXML_USE_STL

third_party/kdl_parser/include/urdf_exception/exception.h

@@ -0,0 +1,53 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+// URDF exceptions
+#ifndef URDF_INTERFACE_EXCEPTION_H_
+#define URDF_INTERFACE_EXCEPTION_H_
+
+#include <string>
+#include <stdexcept>
+
+namespace urdf
+{
+
+class ParseError: public std::runtime_error
+{
+public:
+  ParseError(const std::string &error_msg) : std::runtime_error(error_msg) {};
+};
+
+}
+
+#endif

third_party/kdl_parser/include/urdf_model/color.h

@@ -0,0 +1,105 @@
+/*********************************************************************
+* 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 <stdexcept>
+#include <string>
+#include <vector>
+#include <math.h>
+
+#include <urdf_model/utils.h>
+#include <urdf_exception/exception.h>
+
+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;
+    urdf::split_string( pieces, vector_str, " ");
+    for (unsigned int i = 0; i < pieces.size(); ++i)
+    {
+      if (!pieces[i].empty())
+      {
+        try
+        {
+          double piece = strToDouble(pieces[i].c_str());
+          if ((piece < 0) || (piece > 1))
+            throw ParseError("Component [" + pieces[i] + "] is outside the valid range for colors [0, 1]");
+          rgba.push_back(static_cast<float>(piece));
+        }
+        catch (std::runtime_error &/*e*/) {
+          throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a color value)");
+        }
+      }
+    }
+
+    if (rgba.size() != 4)
+    {
+      return false;
+    }
+
+    this->r = rgba[0];
+    this->g = rgba[1];
+    this->b = rgba[2];
+    this->a = rgba[3];
+
+    return true;
+  };
+};
+
+}
+
+#endif
+

third_party/kdl_parser/include/urdf_model/joint.h

@@ -0,0 +1,230 @@
+/*********************************************************************
+* 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 "urdf_model/pose.h"
+#include "urdf_model/types.h"
+
+
+namespace urdf{
+
+class Link;
+
+class JointDynamics
+{
+public:
+  JointDynamics() { this->clear(); };
+  double damping;
+  double friction;
+
+  void clear()
+  {
+    damping = 0;
+    friction = 0;
+  };
+};
+
+class JointLimits
+{
+public:
+  JointLimits() { this->clear(); };
+  double lower;
+  double upper;
+  double effort;
+  double velocity;
+
+  void clear()
+  {
+    lower = 0;
+    upper = 0;
+    effort = 0;
+    velocity = 0;
+  };
+};
+
+/// \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;
+  };
+};
+
+
+class JointCalibration
+{
+public:
+  JointCalibration() { this->clear(); };
+  double reference_position;
+  DoubleSharedPtr rising, falling;
+
+  void clear()
+  {
+    reference_position = 0;
+  };
+};
+
+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();
+  };
+};
+
+
+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
+  JointDynamicsSharedPtr dynamics;
+
+  /// Joint Limits
+  JointLimitsSharedPtr limits;
+
+  /// Unsupported Hidden Feature
+  JointSafetySharedPtr safety;
+
+  /// Unsupported Hidden Feature
+  JointCalibrationSharedPtr calibration;
+
+  /// Option to Mimic another Joint
+  JointMimicSharedPtr mimic;
+
+  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->mimic.reset();
+    this->type = UNKNOWN;
+  };
+};
+
+}
+
+#endif

third_party/kdl_parser/include/urdf_model/link.h

@@ -0,0 +1,247 @@
+/*********************************************************************
+* 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 <map>
+
+#include "joint.h"
+#include "color.h"
+#include "types.h"
+
+namespace urdf{
+
+class Geometry
+{
+public:
+  enum {SPHERE, BOX, CYLINDER, MESH} type;
+
+  virtual ~Geometry(void)
+  {
+  }  
+};
+
+class Sphere : public Geometry
+{
+public:
+  Sphere() { this->clear(); type = SPHERE; };
+  double radius;
+
+  void clear()
+  {
+    radius = 0;
+  };
+};
+
+class Box : public Geometry
+{
+public:
+  Box() { this->clear(); type = BOX; };
+  Vector3 dim;
+
+  void clear()
+  {
+    this->dim.clear();
+  };
+};
+
+class Cylinder : public Geometry
+{
+public:
+  Cylinder() { this->clear(); type = CYLINDER; };
+  double length;
+  double radius;
+
+  void clear()
+  {
+    length = 0;
+    radius = 0;
+  };
+};
+
+class Mesh : public Geometry
+{
+public:
+  Mesh() { this->clear(); type = MESH; };
+  std::string filename;
+  Vector3 scale;
+
+  void clear()
+  {
+    filename.clear();
+    // default scale
+    scale.x = 1;
+    scale.y = 1;
+    scale.z = 1;
+  };
+};
+
+class Material
+{
+public:
+  Material() { this->clear(); };
+  std::string name;
+  std::string texture_filename;
+  Color color;
+
+  void clear()
+  {
+    color.clear();
+    texture_filename.clear();
+    name.clear();
+  };
+};
+
+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;
+  };
+};
+
+class Visual
+{
+public:
+  Visual() { this->clear(); };
+  Pose origin;
+  GeometrySharedPtr geometry;
+
+  std::string material_name;
+  MaterialSharedPtr material;
+
+  void clear()
+  {
+    origin.clear();
+    material_name.clear();
+    material.reset();
+    geometry.reset();
+    name.clear();
+  };
+
+  std::string name;
+};
+
+class Collision
+{
+public:
+  Collision() { this->clear(); };
+  Pose origin;
+  GeometrySharedPtr geometry;
+
+  void clear()
+  {
+    origin.clear();
+    geometry.reset();
+    name.clear();
+  };
+
+  std::string name;
+
+};
+
+
+class Link
+{
+public:
+  Link() { this->clear(); };
+
+  std::string name;
+
+  /// inertial element
+  InertialSharedPtr inertial;
+
+  /// visual element
+  VisualSharedPtr visual;
+
+  /// collision element
+  CollisionSharedPtr collision;
+
+  /// if more than one collision element is specified, all collision elements are placed in this array (the collision member points to the first element of the array)
+  std::vector<CollisionSharedPtr> collision_array;
+
+  /// if more than one visual element is specified, all visual elements are placed in this array (the visual member points to the first element of the array)
+  std::vector<VisualSharedPtr> visual_array;
+
+  /// Parent Joint element
+  ///   explicitly stating "parent" because we want directional-ness for tree structure
+  ///   every link can have one parent
+  JointSharedPtr parent_joint;
+
+  std::vector<JointSharedPtr> child_joints;
+  std::vector<LinkSharedPtr> child_links;
+
+  LinkSharedPtr getParent() const
+  {return parent_link_.lock();};
+
+  void setParent(const LinkSharedPtr &parent)
+  { parent_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_array.clear();
+    this->visual_array.clear();
+  };
+
+private:
+  LinkWeakPtr parent_link_;
+
+};
+
+
+
+
+}
+
+#endif

third_party/kdl_parser/include/urdf_model/model.h

@@ -0,0 +1,206 @@
+/*********************************************************************
+* 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 <urdf_model/link.h>
+#include <urdf_model/types.h>
+#include <urdf_exception/exception.h>
+
+namespace urdf {
+
+class ModelInterface
+{
+public:
+  LinkConstSharedPtr getRoot(void) const{return this->root_link_;};
+  LinkConstSharedPtr getLink(const std::string& name) const
+  {
+    LinkConstSharedPtr ptr;
+    if (this->links_.find(name) == this->links_.end())
+      ptr.reset();
+    else
+      ptr = this->links_.find(name)->second;
+    return ptr;
+  };
+  
+  JointConstSharedPtr getJoint(const std::string& name) const
+  {
+    JointConstSharedPtr 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<LinkSharedPtr >& links) const
+  {
+    for (std::map<std::string,LinkSharedPtr>::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, LinkSharedPtr &link) const
+  {
+    LinkSharedPtr ptr;
+    if (this->links_.find(name) == this->links_.end())
+      ptr.reset();
+    else
+      ptr = this->links_.find(name)->second;
+    link = ptr;
+  };
+  
+  /// non-const getMaterial()
+  MaterialSharedPtr getMaterial(const std::string& name) const
+  {
+    MaterialSharedPtr ptr;
+    if (this->materials_.find(name) == this->materials_.end())
+      ptr.reset();
+    else
+      ptr = this->materials_.find(name)->second;
+    return ptr;
+  };
+  
+  void 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, JointSharedPtr>::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;
+      
+      if (parent_link_name.empty() || child_link_name.empty())
+      {
+        throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification.");
+      }
+      else
+      {
+        // find child and parent links
+        LinkSharedPtr child_link, parent_link;
+        this->getLink(child_link_name, child_link);
+        if (!child_link)
+        {
+          throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found");
+        }
+        this->getLink(parent_link_name, parent_link);
+        if (!parent_link)
+        {
+          throw ParseError("parent link [" + parent_link_name + "] of joint [" + joint->first + "] not found.  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 [" + joint->first + "] from your urdf file, or add \"<link name=\"" + parent_link_name + "\" />\" to your urdf file.");
+        }
+        
+        //set parent link for child link
+        child_link->setParent(parent_link);
+
+        //set parent joint for child link        
+        child_link->parent_joint = joint->second;
+        
+        //set child joint for parent link
+        parent_link->child_joints.push_back(joint->second);
+
+        //set child link for parent link
+        parent_link->child_links.push_back(child_link);
+
+        // fill in child/parent string map
+        parent_link_tree[child_link->name] = parent_link_name;
+      }
+    }
+  }
+  
+  void initRoot(const 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, LinkSharedPtr>::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++)
+    {
+      std::map<std::string, std::string >::const_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
+        {
+          throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]");
+        }
+      }
+    }
+    if (!this->root_link_)
+    {
+      throw ParseError("No root link found. The robot xml is not a valid tree.");
+    }
+  }
+  
+  
+  /// \brief complete list of Links
+  std::map<std::string, LinkSharedPtr> links_;
+  /// \brief complete list of Joints
+  std::map<std::string, JointSharedPtr> joints_;
+  /// \brief complete list of Materials
+  std::map<std::string, MaterialSharedPtr> materials_;
+
+  /// \brief The name of the robot model
+  std::string name_;
+
+  /// \brief The root is always a link (the parent of the tree describing the robot)
+  LinkSharedPtr root_link_;
+
+
+
+};
+
+}
+
+#endif

third_party/kdl_parser/include/urdf_model/pose.h

@@ -0,0 +1,260 @@
+/*********************************************************************
+* 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 <cmath>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include <urdf_exception/exception.h>
+#include <urdf_model/utils.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;};
+  void init(const std::string &vector_str)
+  {
+    this->clear();
+    std::vector<std::string> pieces;
+    std::vector<double> xyz;
+    urdf::split_string( pieces, vector_str, " ");
+    for (unsigned int i = 0; i < pieces.size(); ++i){
+      if (pieces[i] != ""){
+        try {
+          xyz.push_back(strToDouble(pieces[i].c_str()));
+        } catch(std::runtime_error &) {
+          throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)");
+        }
+      }
+    }
+
+    if (xyz.size() != 3)
+      throw ParseError("Parser found " + std::to_string(xyz.size())  + " elements but 3 expected while parsing vector [" + vector_str + "]");
+
+    this->x = xyz[0];
+    this->y = xyz[1];
+    this->z = xyz[2];
+  }
+
+  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;
+
+    // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
+    double sarg = -2 * (this->x*this->z - this->w*this->y);
+    const double pi_2 = 1.57079632679489661923;
+    if (sarg <= -0.99999) {
+      pitch = -pi_2;
+      roll  = 0;
+      yaw   = 2 * atan2(this->x, -this->y);
+    } else if (sarg >= 0.99999) {
+      pitch = pi_2;
+      roll  = 0;
+      yaw   = 2 * atan2(-this->x, this->y);
+    } else {
+      pitch = asin(sarg);
+      roll  = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz);
+      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;
+
+  void init(const std::string &rotation_str)
+  {
+    this->clear();
+    Vector3 rpy;
+    rpy.init(rotation_str);
+    setFromRPY(rpy.x, rpy.y, rpy.z);
+  }
+
+  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();
+  };
+};
+
+}
+
+#endif

third_party/kdl_parser/include/urdf_model/twist.h

@@ -0,0 +1,68 @@
+/*********************************************************************
+* 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: John Hsu */
+
+#ifndef URDF_TWIST_H
+#define URDF_TWIST_H
+
+#include <string>
+#include <sstream>
+#include <vector>
+#include <math.h>
+#include <urdf_model/pose.h>
+
+namespace urdf{
+
+
+class Twist
+{
+public:
+  Twist() { this->clear(); };
+
+  Vector3  linear;
+  // Angular velocity represented by Euler angles
+  Vector3  angular;
+
+  void clear()
+  {
+    this->linear.clear();
+    this->angular.clear();
+  };
+};
+
+}
+
+#endif
+

third_party/kdl_parser/include/urdf_model/types.h

@@ -0,0 +1,91 @@
+/*********************************************************************
+* 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: Steve Peters */
+
+#ifndef URDF_MODEL_TYPES_H
+#define URDF_MODEL_TYPES_H
+
+#include <memory>
+
+#define URDF_TYPEDEF_CLASS_POINTER(Class) \
+class Class; \
+typedef std::shared_ptr<Class> Class##SharedPtr; \
+typedef std::shared_ptr<const Class> Class##ConstSharedPtr; \
+typedef std::weak_ptr<Class> Class##WeakPtr
+
+namespace urdf{
+
+// shared pointer used in joint.h
+typedef std::shared_ptr<double> DoubleSharedPtr;
+
+URDF_TYPEDEF_CLASS_POINTER(Box);
+URDF_TYPEDEF_CLASS_POINTER(Collision);
+URDF_TYPEDEF_CLASS_POINTER(Cylinder);
+URDF_TYPEDEF_CLASS_POINTER(Geometry);
+URDF_TYPEDEF_CLASS_POINTER(Inertial);
+URDF_TYPEDEF_CLASS_POINTER(Joint);
+URDF_TYPEDEF_CLASS_POINTER(JointCalibration);
+URDF_TYPEDEF_CLASS_POINTER(JointDynamics);
+URDF_TYPEDEF_CLASS_POINTER(JointLimits);
+URDF_TYPEDEF_CLASS_POINTER(JointMimic);
+URDF_TYPEDEF_CLASS_POINTER(JointSafety);
+URDF_TYPEDEF_CLASS_POINTER(Link);
+URDF_TYPEDEF_CLASS_POINTER(Material);
+URDF_TYPEDEF_CLASS_POINTER(Mesh);
+URDF_TYPEDEF_CLASS_POINTER(Sphere);
+URDF_TYPEDEF_CLASS_POINTER(Visual);
+
+// create *_pointer_cast functions in urdf namespace
+template<class T, class U>
+std::shared_ptr<T> const_pointer_cast(std::shared_ptr<U> const & r)
+{
+  return std::const_pointer_cast<T>(r);
+}
+
+template<class T, class U>
+std::shared_ptr<T> dynamic_pointer_cast(std::shared_ptr<U> const & r)
+{
+  return std::dynamic_pointer_cast<T>(r);
+}
+
+template<class T, class U>
+std::shared_ptr<T> static_pointer_cast(std::shared_ptr<U> const & r)
+{
+  return std::static_pointer_cast<T>(r);
+}
+
+}
+
+#endif

third_party/kdl_parser/include/urdf_model/utils.h

@@ -0,0 +1,92 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2016, Open Source Robotics Foundation (OSRF)
+*  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 OSRF 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: Steve Peters */
+
+#ifndef URDF_INTERFACE_UTILS_H
+#define URDF_INTERFACE_UTILS_H
+
+#include <locale>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+namespace urdf {
+
+// Replacement for boost::split( ... , ... , boost::is_any_of(" "))
+inline
+void split_string(std::vector<std::string> &result,
+                  const std::string &input,
+                  const std::string &isAnyOf)
+{
+  std::string::size_type start = 0;
+  std::string::size_type end = input.find_first_of(isAnyOf, start);
+  while (end != std::string::npos)
+  {
+    result.push_back(input.substr(start, end-start));
+    start = end + 1;
+    end = input.find_first_of(isAnyOf, start);
+  }
+  if (start < input.length())
+  {
+    result.push_back(input.substr(start));
+  }
+}
+
+// This is a locale-safe version of string-to-double, which is suprisingly
+// difficult to do correctly.  This function ensures that the C locale is used
+// for parsing, as that matches up with what the XSD for double specifies.
+// On success, the double is returned; on failure, a std::runtime_error is
+// thrown.
+static inline double strToDouble(const char *in)
+{
+  std::stringstream ss;
+  ss.imbue(std::locale::classic());
+
+  ss << in;
+
+  double out;
+  ss >> out;
+
+  if (ss.fail() || !ss.eof()) {
+    throw std::runtime_error("Failed converting string to double");
+  }
+
+  return out;
+}
+
+}
+
+#endif

third_party/kdl_parser/include/urdf_model_state/model_state.h

@@ -0,0 +1,151 @@
+/*********************************************************************
+* 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: John Hsu */
+
+#ifndef URDF_MODEL_STATE_H
+#define URDF_MODEL_STATE_H
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include "urdf_model/pose.h"
+#include <urdf_model/twist.h>
+#include "urdf_model_state/types.h"
+
+
+namespace urdf{
+
+//round is not defined in C++98
+//So in Visual Studio <= 2012 is necessary to define it
+#ifdef _MSC_VER
+#if (_MSC_VER <= 1700)
+double round(double value)
+{
+    return (value >= 0.0f)?(floor(value + 0.5f)):(ceil(value - 0.5f));
+}
+#endif 
+#endif
+
+class Time
+{
+public:
+  Time() { this->clear(); };
+
+  void set(double _seconds)
+  {
+    this->sec = (int32_t)(floor(_seconds));
+    this->nsec = (int32_t)(round((_seconds - this->sec) * 1e9));
+    this->Correct();
+  };
+
+  operator double ()
+  {
+    return (static_cast<double>(this->sec) +
+            static_cast<double>(this->nsec)*1e-9);
+  };
+
+  int32_t sec;
+  int32_t nsec;
+
+  void clear()
+  {
+    this->sec = 0;
+    this->nsec = 0;
+  };
+private:
+  void Correct()
+  {
+    // Make any corrections
+    if (this->nsec >= 1e9)
+    {
+      this->sec++;
+      this->nsec = (int32_t)(this->nsec - 1e9);
+    }
+    else if (this->nsec < 0)
+    {
+      this->sec--;
+      this->nsec = (int32_t)(this->nsec + 1e9);
+    }
+  };
+};
+
+
+class JointState
+{
+public:
+  JointState() { this->clear(); };
+
+  /// joint name
+  std::string joint;
+
+  std::vector<double> position;
+  std::vector<double> velocity;
+  std::vector<double> effort;
+
+  void clear()
+  {
+    this->joint.clear();
+    this->position.clear();
+    this->velocity.clear();
+    this->effort.clear();
+  }
+};
+
+class ModelState
+{
+public:
+  ModelState() { this->clear(); };
+
+  /// state name must be unique
+  std::string name;
+
+  Time time_stamp;
+
+  void clear()
+  {
+    this->name.clear();
+    this->time_stamp.set(0);
+    this->joint_states.clear();
+  };
+
+  std::vector<JointStateSharedPtr> joint_states;
+
+};
+
+}
+
+#endif
+

third_party/kdl_parser/include/urdf_model_state/twist.h

@@ -0,0 +1,42 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+#ifndef URDF_MODEL_STATE_TWIST_
+#define URDF_MODEL_STATE_TWIST_
+
+#warning "Please Use #include <urdf_model/twist.h>"
+
+#include <urdf_model/twist.h>
+
+#endif

third_party/kdl_parser/include/urdf_model_state/types.h

@@ -0,0 +1,52 @@
+/*********************************************************************
+* 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: Steve Peters */
+
+#ifndef URDF_MODEL_STATE_TYPES_H
+#define URDF_MODEL_STATE_TYPES_H
+
+#include <memory>
+
+
+namespace urdf{
+
+class JointState;
+
+// typedef shared pointers
+typedef std::shared_ptr<JointState> JointStateSharedPtr;
+
+}
+
+#endif

third_party/kdl_parser/include/urdf_parser/exportdecl.h

@@ -0,0 +1,84 @@
+/*********************************************************************
+* 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: Thomas Moulard */
+
+#ifndef URDFDOM_EXPORTDECL_H
+# define URDFDOM_EXPORTDECL_H
+
+// Handle portable symbol export.
+// Defining manually which symbol should be exported is required
+// under Windows whether MinGW or MSVC is used.
+//
+// The headers then have to be able to work in two different modes:
+// - dllexport when one is building the library,
+// - dllimport for clients using the library.
+//
+// On Linux, set the visibility accordingly. If C++ symbol visibility
+// is handled by the compiler, see: http://gcc.gnu.org/wiki/Visibility
+# if defined _WIN32 || defined __CYGWIN__
+// On Microsoft Windows, use dllimport and dllexport to tag symbols.
+#  define URDFDOM_DLLIMPORT __declspec(dllimport)
+#  define URDFDOM_DLLEXPORT __declspec(dllexport)
+#  define URDFDOM_DLLLOCAL
+# else
+// On Linux, for GCC >= 4, tag symbols using GCC extension.
+#  if __GNUC__ >= 4
+#   define URDFDOM_DLLIMPORT __attribute__ ((visibility("default")))
+#   define URDFDOM_DLLEXPORT __attribute__ ((visibility("default")))
+#   define URDFDOM_DLLLOCAL  __attribute__ ((visibility("hidden")))
+#  else
+// Otherwise (GCC < 4 or another compiler is used), export everything.
+#   define URDFDOM_DLLIMPORT
+#   define URDFDOM_DLLEXPORT
+#   define URDFDOM_DLLLOCAL
+#  endif // __GNUC__ >= 4
+# endif // defined _WIN32 || defined __CYGWIN__
+
+# ifdef URDFDOM_STATIC
+// If one is using the library statically, get rid of
+// extra information.
+#  define URDFDOM_DLLAPI
+#  define URDFDOM_LOCAL
+# else
+// Depending on whether one is building or using the
+// library define DLLAPI to import or export.
+#  ifdef URDFDOM_EXPORTS
+#   define URDFDOM_DLLAPI URDFDOM_DLLEXPORT
+#  else
+#   define URDFDOM_DLLAPI URDFDOM_DLLIMPORT
+#  endif // URDFDOM_EXPORTS
+#  define URDFDOM_LOCAL URDFDOM_DLLLOCAL
+# endif // URDFDOM_STATIC
+#endif //! URDFDOM_EXPORTDECL_H

third_party/kdl_parser/include/urdf_parser/urdf_parser.h

@@ -0,0 +1,150 @@
+/*********************************************************************
+* 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 <stdexcept>
+#include <string>
+#include <vector>
+
+#include <tinyxml.h>
+#include <urdf_model/model.h>
+#include <urdf_model/color.h>
+#include <urdf_world/types.h>
+#include <urdf_model/utils.h>
+
+#include "exportdecl.h"
+
+namespace urdf_export_helpers {
+
+URDFDOM_DLLAPI std::string values2str(unsigned int count, const double *values, double (*conv)(double) = NULL);
+URDFDOM_DLLAPI std::string values2str(urdf::Vector3 vec);
+URDFDOM_DLLAPI std::string values2str(urdf::Rotation rot);
+URDFDOM_DLLAPI std::string values2str(urdf::Color c);
+URDFDOM_DLLAPI std::string values2str(double d);
+
+// This lives here (rather than in model.cpp) so we can run tests on it.
+class URDFVersion final
+{
+public:
+  explicit URDFVersion(const char *attr)
+  {
+    // If the passed in attribute is NULL, it means it wasn't specified in the
+    // XML, so we just assume version 1.0.
+    if (attr == nullptr)
+    {
+      major_ = 1;
+      minor_ = 0;
+      return;
+    }
+
+    // We only accept version strings of the type <major>.<minor>
+    std::vector<std::string> split;
+    urdf::split_string(split, std::string(attr), ".");
+    if (split.size() == 2)
+    {
+      major_ = strToUnsigned(split[0].c_str());
+      minor_ = strToUnsigned(split[1].c_str());
+    }
+    else
+    {
+      throw std::runtime_error("The version attribute should be in the form 'x.y'");
+    }
+  }
+
+  bool equal(uint32_t maj, uint32_t min)
+  {
+    return this->major_ == maj && this->minor_ == min;
+  }
+
+  uint32_t getMajor() const
+  {
+    return major_;
+  }
+
+  uint32_t getMinor() const
+  {
+    return minor_;
+  }
+
+private:
+  uint32_t strToUnsigned(const char *str)
+  {
+    if (str[0] == '\0')
+    {
+      // This would get caught below, but we can make a nicer error message
+      throw std::runtime_error("One of the fields of the version attribute is blank");
+    }
+    char *end = const_cast<char *>(str);
+    long value = strtol(str, &end, 10);
+    if (end == str)
+    {
+      // If the pointer didn't move at all, then we couldn't convert any of
+      // the string to an integer.
+      throw std::runtime_error("Version attribute is not an integer");
+    }
+    if (*end != '\0')
+    {
+      // Here, we didn't go all the way to the end of the string, which
+      // means there was junk at the end
+      throw std::runtime_error("Extra characters after the version number");
+    }
+    if (value < 0)
+    {
+      throw std::runtime_error("Version number must be positive");
+    }
+
+    return value;
+  }
+
+  uint32_t major_;
+  uint32_t minor_;
+};
+
+}
+
+namespace urdf{
+
+  URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDF(const std::string &xml_string);
+  URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFile(const std::string& path);
+  URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFileDocument(const std::string &path);
+  URDFDOM_DLLAPI TiXmlDocument*  exportURDF(ModelInterfaceSharedPtr &model);
+  URDFDOM_DLLAPI TiXmlDocument*  exportURDF(const ModelInterface &model);
+  URDFDOM_DLLAPI bool parsePose(Pose&, TiXmlElement*);
+}
+
+#endif

third_party/kdl_parser/include/urdf_sensor/sensor.h

@@ -0,0 +1,176 @@
+/*********************************************************************
+* 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: John Hsu */
+
+/* example
+
+ <sensor name="my_camera_sensor" update_rate="20">
+   <origin xyz="0 0 0" rpy="0 0 0"/>
+   <camera>
+     <horizontal_hov>1.5708</horizontal_hov>
+     <image width="640" height="480" format="R8G8B8"/>
+     <clip near="0.01" far="50.0"/>
+   </camera>
+ </sensor>
+ <sensor name="my_ray_sensor" update_rate="20">
+   <origin xyz="0 0 0" rpy="0 0 0"/>
+   <ray>
+     <scan>
+       <horizontal samples="100" resolution="1" min_angle="-1.5708" max_angle="1.5708"/>
+       <vertical samples="1" resolution="1" min_angle="0" max_angle="0"/>
+     </scan>
+   </ray>
+ </sensor>
+
+*/
+
+
+
+#ifndef URDF_SENSOR_H
+#define URDF_SENSOR_H
+
+#include <string>
+#include <vector>
+#include <map>
+#include "urdf_model/pose.h"
+#include "urdf_model/joint.h"
+#include "urdf_model/link.h"
+#include "urdf_model/types.h"
+#include "urdf_sensor/types.h"
+
+namespace urdf{
+
+class VisualSensor
+{
+public:
+  enum {CAMERA, RAY} type;
+  virtual ~VisualSensor(void)
+  {
+  }
+};
+
+class Camera : public VisualSensor
+{
+public:
+  Camera() { this->clear(); };
+  unsigned int width, height;
+  /// format is optional: defaults to R8G8B8), but can be
+  /// (L8|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8)
+  std::string format;
+  double hfov;
+  double near;
+  double far;
+
+  void clear()
+  {
+    hfov = 0;
+    width = 0;
+    height = 0;
+    format.clear();
+    near = 0;
+    far = 0;
+  };
+};
+
+class Ray : public VisualSensor
+{
+public:
+  Ray() { this->clear(); };
+  unsigned int horizontal_samples;
+  double horizontal_resolution;
+  double horizontal_min_angle;
+  double horizontal_max_angle;
+  unsigned int vertical_samples;
+  double vertical_resolution;
+  double vertical_min_angle;
+  double vertical_max_angle;
+
+  void clear()
+  {
+    // set defaults
+    horizontal_samples = 1;
+    horizontal_resolution = 1;
+    horizontal_min_angle = 0;
+    horizontal_max_angle = 0;
+    vertical_samples = 1;
+    vertical_resolution = 1;
+    vertical_min_angle = 0;
+    vertical_max_angle = 0;
+  };
+};
+
+
+class Sensor
+{
+public:
+  Sensor() { this->clear(); };
+
+  /// sensor name must be unique
+  std::string name;
+
+  /// update rate in Hz
+  double update_rate;
+
+  /// transform from parent frame to optical center
+  ///   with z-forward and x-right, y-down
+  Pose origin;
+
+  /// sensor
+  VisualSensorSharedPtr sensor;
+
+
+  /// Parent link element name.  A pointer is stored in parent_link_.
+  std::string parent_link_name;
+
+  LinkSharedPtr getParent() const
+  {return parent_link_.lock();};
+
+  void setParent(LinkSharedPtr parent)
+  {  this->parent_link_ = parent; }
+  
+  void clear()
+  {
+    this->name.clear();
+    this->sensor.reset();
+    this->parent_link_name.clear();
+    this->parent_link_.reset();
+  };
+
+private:
+  LinkWeakPtr parent_link_;
+
+};
+}
+#endif

third_party/kdl_parser/include/urdf_sensor/types.h

@@ -0,0 +1,52 @@
+/*********************************************************************
+* 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: Steve Peters */
+
+#ifndef URDF_SENSOR_TYPES_H
+#define URDF_SENSOR_TYPES_H
+
+#include <memory>
+
+
+namespace urdf{
+
+class VisualSensor;
+
+// typedef shared pointers
+typedef std::shared_ptr<VisualSensor> VisualSensorSharedPtr;
+
+}
+
+#endif

third_party/kdl_parser/include/urdf_world/types.h

@@ -0,0 +1,52 @@
+/*********************************************************************
+* 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: Steve Peters */
+
+#ifndef URDF_WORLD_TYPES_H
+#define URDF_WORLD_TYPES_H
+
+#include <memory>
+
+
+namespace urdf{
+
+class ModelInterface;
+
+// typedef shared pointers
+typedef std::shared_ptr<ModelInterface> ModelInterfaceSharedPtr;
+
+}
+
+#endif

third_party/kdl_parser/include/urdf_world/world.h

@@ -0,0 +1,110 @@
+/*********************************************************************
+* 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: John Hsu */
+
+/* encapsulates components in a world
+   see http://ros.org/wiki/usdf/XML/urdf_world
+   for details
+*/
+/* example world XML
+
+<world name="pr2_with_table">
+  <!-- include the models by including
+       either the complete urdf or
+       referencing the file name.  -->
+  <model name="pr2">
+    ...
+  </model>
+  <include filename="table.urdf" model_name="table_model"/>
+
+  <!-- models in the world -->
+  <entity model="pr2" name="prj">
+    <origin xyz="0 1 0" rpy="0 0 0"/>
+    <twist linear="0 0 0" angular="0 0 0"/>
+  </entity>
+  <entity model="pr2" name="prk">
+    <origin xyz="0 2 0" rpy="0 0 0"/>
+    <twist linear="0 0 0" angular="0 0 0"/>
+  </entity>
+  <entity model="table_model">
+    <origin xyz="0 3 0" rpy="0 0 0"/>
+    <twist linear="0 0 0" angular="0 0 0"/>
+  </entity>
+
+</world>
+
+*/
+
+#ifndef URDF_WORLD_H
+#define URDF_WORLD_H
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include "urdf_model/model.h"
+#include "urdf_model/pose.h"
+#include "urdf_model/twist.h"
+#include "urdf_world/types.h"
+
+namespace urdf{
+
+class Entity
+{
+public:
+  ModelInterfaceSharedPtr model;
+  Pose origin;
+  Twist twist;
+};
+
+class World
+{
+public:
+  World() { this->clear(); };
+
+  /// world name must be unique
+  std::string name;
+
+  std::vector<Entity> models;
+
+  void clear()
+  {
+    this->name.clear();
+  };
+};
+}
+
+#endif
+