add tinyxml, urdfdom and urdfdom_headers from https://github.com/ros/urdfdom

(removed BOOST dependency and make it compile on Windows)

btgui/tinyxml/tinystr.cpp

@@ -0,0 +1,111 @@
+/*
+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
+
+#include "tinystr.h"
+
+// Error value for find primitive
+const TiXmlString::size_type TiXmlString::npos = static_cast< TiXmlString::size_type >(-1);
+
+
+// Null rep.
+TiXmlString::Rep TiXmlString::nullrep_ = { 0, 0, { '\0' } };
+
+
+void TiXmlString::reserve (size_type cap)
+{
+	if (cap > capacity())
+	{
+		TiXmlString tmp;
+		tmp.init(length(), cap);
+		memcpy(tmp.start(), data(), length());
+		swap(tmp);
+	}
+}
+
+
+TiXmlString& TiXmlString::assign(const char* str, size_type len)
+{
+	size_type cap = capacity();
+	if (len > cap || cap > 3*(len + 8))
+	{
+		TiXmlString tmp;
+		tmp.init(len);
+		memcpy(tmp.start(), str, len);
+		swap(tmp);
+	}
+	else
+	{
+		memmove(start(), str, len);
+		set_size(len);
+	}
+	return *this;
+}
+
+
+TiXmlString& TiXmlString::append(const char* str, size_type len)
+{
+	size_type newsize = length() + len;
+	if (newsize > capacity())
+	{
+		reserve (newsize + capacity());
+	}
+	memmove(finish(), str, len);
+	set_size(newsize);
+	return *this;
+}
+
+
+TiXmlString operator + (const TiXmlString & a, const TiXmlString & b)
+{
+	TiXmlString tmp;
+	tmp.reserve(a.length() + b.length());
+	tmp += a;
+	tmp += b;
+	return tmp;
+}
+
+TiXmlString operator + (const TiXmlString & a, const char* b)
+{
+	TiXmlString tmp;
+	TiXmlString::size_type b_len = static_cast<TiXmlString::size_type>( strlen(b) );
+	tmp.reserve(a.length() + b_len);
+	tmp += a;
+	tmp.append(b, b_len);
+	return tmp;
+}
+
+TiXmlString operator + (const char* a, const TiXmlString & b)
+{
+	TiXmlString tmp;
+	TiXmlString::size_type a_len = static_cast<TiXmlString::size_type>( strlen(a) );
+	tmp.reserve(a_len + b.length());
+	tmp.append(a, a_len);
+	tmp += b;
+	return tmp;
+}
+
+
+#endif	// TIXML_USE_STL

btgui/tinyxml/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

btgui/tinyxml/tinyxmlerror.cpp

@@ -0,0 +1,52 @@
+/*
+www.sourceforge.net/projects/tinyxml
+Original code (2.0 and earlier )copyright (c) 2000-2006 Lee Thomason (www.grinninglizard.com)
+
+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.
+*/
+
+#include "tinyxml.h"
+
+// The goal of the seperate error file is to make the first
+// step towards localization. tinyxml (currently) only supports
+// english error messages, but the could now be translated.
+//
+// It also cleans up the code a bit.
+//
+
+const char* TiXmlBase::errorString[ TiXmlBase::TIXML_ERROR_STRING_COUNT ] =
+{
+	"No error",
+	"Error",
+	"Failed to open file",
+	"Error parsing Element.",
+	"Failed to read Element name",
+	"Error reading Element value.",
+	"Error reading Attributes.",
+	"Error: empty tag.",
+	"Error reading end tag.",
+	"Error parsing Unknown.",
+	"Error parsing Comment.",
+	"Error parsing Declaration.",
+	"Error document empty.",
+	"Error null (0) or unexpected EOF found in input stream.",
+	"Error parsing CDATA.",
+	"Error when TiXmlDocument added to document, because TiXmlDocument can only be at the root.",
+};

btgui/urdf/boost_replacement/lexical_cast.h

@@ -0,0 +1,27 @@
+#ifndef BOOST_REPLACEMENT_LEXICAL_CAST_H
+#define BOOST_REPLACEMENT_LEXICAL_CAST_H
+
+#include <stdlib.h>
+
+namespace boost
+{
+
+	template <typename T> T lexical_cast(const char* txt)
+	{
+		double result = atof(txt);
+		return result;
+	};
+
+	struct bad_lexical_cast
+	{
+		const char* what()
+		{
+		return ("bad lexical cast\n");
+		}
+		
+	};
+	
+} //namespace boost
+
+#endif
+

btgui/urdf/boost_replacement/printf_console.cpp

@@ -0,0 +1,28 @@
+#include "printf_console.h"
+#include <stdio.h>
+
+
+void logError(const char* msg, const char* arg0, const char* arg1, const char* arg2)
+{
+	printf("%s %s %s %s\n", msg,arg0,arg1,arg2);
+
+}
+	
+void logDebug(const char* msg, float v0, float v1)
+{
+	printf("%s %f %f\n", msg, v0, v1);
+};
+void logDebug(const char* msg, const char* msg1, const char* arg1)
+{
+	printf("%s %s %s\n", msg, msg1, arg1);
+
+}
+
+void logInform(const char* msg, const char* arg0)
+{
+	printf("%s %s\n", msg, arg0);
+}
+void logWarn(const char* msg,int id, const char* arg0)
+{
+	printf("%s %d %s\n", msg,id,arg0);
+}

btgui/urdf/boost_replacement/printf_console.h

@@ -0,0 +1,13 @@
+#ifndef PRINTF_CONSOLE_H
+#define PRINTF_CONSOLE_H
+
+
+void logError(const char* msg="", const char* arg0="", const char* arg1="", const char* arg2="");
+void logDebug(const char* msg, float v0, float v1);
+void logDebug(const char* msg, const char* msg1="", const char* arg1="");
+void logInform(const char* msg, const char* arg0="");
+void logWarn(const char* msg,int id, const char* arg0="");
+
+#endif
+
+

btgui/urdf/boost_replacement/shared_ptr.h

@@ -0,0 +1,213 @@
+/*
+Bullet Continuous Collision Detection and Physics Library Maya Plugin
+Copyright (c) 2008 Walt Disney Studios
+ 
+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.
+ 
+Written by: Nicola Candussi <nicola@fluidinteractive.com>
+
+Modified by Francisco Gochez
+Dec 2011 - Added deferencing operator
+*/
+
+//my_shared_ptr
+
+#ifndef DYN_SHARED_PTR_H
+#define DYN_SHARED_PTR_H
+
+#define DYN_SHARED_PTR_THREAD_SAFE
+
+
+#include <assert.h>
+
+#ifdef _WIN32
+#include <windows.h>
+
+namespace boost
+{
+class my_shared_count {
+public:
+    my_shared_count(): m_count(1) {   }
+    ~my_shared_count() {    }
+
+    long increment()
+    {
+#ifdef DYN_SHARED_PTR_THREAD_SAFE
+        return InterlockedIncrement(&m_count);
+#else
+        return ++m_count;
+#endif
+    }
+
+    long decrement() {
+#ifdef DYN_SHARED_PTR_THREAD_SAFE
+        return InterlockedDecrement(&m_count);
+#else
+        return ++m_count;
+#endif
+    }
+
+    long use_count() { return m_count;  }
+
+private:
+    long m_count;
+};
+};
+#else //ifdef WIN32
+
+
+#include <pthread.h>
+
+class my_shared_count {
+public:
+    my_shared_count(): m_count(1) {
+#ifdef DYN_SHARED_PTR_THREAD_SAFE
+        pthread_mutex_init(&m_mutex, 0);
+#endif
+    }
+    ~my_shared_count() {
+#ifdef DYN_SHARED_PTR_THREAD_SAFE
+        pthread_mutex_destroy(&m_mutex);
+#endif
+    }
+
+    long increment()
+    {
+#ifdef DYN_SHARED_PTR_THREAD_SAFE
+        pthread_mutex_lock(&m_mutex);
+#endif
+        long c = ++m_count;  
+#ifdef DYN_SHARED_PTR_THREAD_SAFE
+        pthread_mutex_unlock(&m_mutex);
+#endif
+        return c;
+     }
+
+    long decrement() {
+#ifdef DYN_SHARED_PTR_THREAD_SAFE
+        pthread_mutex_lock(&m_mutex);
+#endif
+        long c = --m_count;  
+#ifdef DYN_SHARED_PTR_THREAD_SAFE
+        pthread_mutex_unlock(&m_mutex);
+#endif
+        return c;
+    }
+
+    long use_count() { return m_count;  }
+
+private:
+    long m_count;
+    mutable pthread_mutex_t m_mutex;
+};
+
+#endif
+
+
+template<typename T>
+class my_shared_ptr
+{
+public:
+    my_shared_ptr(): m_ptr(NULL), m_count(NULL) { }
+    my_shared_ptr(my_shared_ptr<T> const& other):
+            m_ptr(other.m_ptr),
+            m_count(other.m_count)
+    {
+        if(other.m_count != NULL) other.m_count->increment();
+    }
+
+    template<typename U>
+    my_shared_ptr(my_shared_ptr<U> const& other):
+            m_ptr(other.m_ptr),
+            m_count(other.m_count)
+    {
+        if(other.m_count != NULL)  other.m_count->increment();
+    }
+
+    my_shared_ptr(T const* other): m_ptr(const_cast<T*>(other)), m_count(NULL)
+    {
+        if(other != NULL) m_count = new my_shared_count;
+    }
+
+    ~my_shared_ptr() 
+    {
+        giveup_ownership();
+    }
+
+    void reset(T const* other) 
+    {
+        if(m_ptr == other) return;
+        giveup_ownership();
+        m_ptr = const_cast<T*>(other);
+        if(other != NULL) m_count = new my_shared_count;
+        else m_count = NULL;
+    }
+
+    T* get() { return m_ptr; }
+    T const* get() const { return m_ptr; }
+    T* operator->() {   return m_ptr; }
+    T const* operator->() const { return m_ptr; }
+    operator bool() const { return m_ptr != NULL; }
+	T& operator*() const
+	{ 
+		assert(m_ptr != 0);
+		return *m_ptr;
+	}
+
+    bool operator<(my_shared_ptr<T> const& rhs) const { return m_ptr < rhs.m_ptr; }
+
+    my_shared_ptr<T>& operator=(my_shared_ptr<T> const& other) {
+        if(m_ptr == other.m_ptr) return *this;
+        giveup_ownership();
+        m_ptr = other.m_ptr;
+        m_count = other.m_count;
+        if(other.m_count != NULL) m_count->increment();
+        return *this;
+    }
+
+    template<typename U> 
+    my_shared_ptr<T>& operator=(my_shared_ptr<U>& other) {
+        if(m_ptr == other.m_ptr) return *this;
+        giveup_ownership();
+        m_ptr = other.m_ptr;
+        m_count = other.m_count;
+        if(other.m_count != NULL) m_count->increment();
+        return *this;
+    }
+
+protected:
+
+    template<typename U> friend class my_shared_ptr;
+    void giveup_ownership()
+    {
+        if(m_count != NULL) {
+            if( m_count->decrement() == 0) {
+                delete m_ptr;
+                m_ptr = NULL;
+                delete m_count;
+                m_count = NULL;
+            }
+        }
+    }
+
+protected:
+    T               *m_ptr;
+    my_shared_count    *m_count;
+
+};
+
+
+#endif

btgui/urdf/boost_replacement/string_split.cpp

@@ -0,0 +1,253 @@
+
+
+#include <assert.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "string_split.h"
+
+namespace boost
+{
+        void split( std::vector<std::string>&pieces, const std::string& vector_str, std::vector<std::string> separators)
+	{
+		assert(separators.size()==1);
+		if (separators.size()==1)
+		{
+			char** strArray = str_split(vector_str.c_str(),separators[0].c_str());
+			int numSubStr = str_array_len(strArray);
+			for (int i=0;i<numSubStr;i++)
+				pieces.push_back(std::string(strArray[i]));
+			str_array_free(strArray);
+		}
+	}
+        std::vector<std::string> is_any_of(const char* seps)
+	{
+		std::vector<std::string> strArray;
+
+		int numSeps = strlen(seps);
+		for (int i=0;i<numSeps;i++)
+		{
+			char sep2[2] = {0,0};
+			
+			sep2[0] = seps[i];
+			strArray.push_back(sep2);
+		}
+		return strArray;
+		
+	}
+};
+
+
+
+/* Append an item to a dynamically allocated array of strings. On failure,
+ return NULL, in which case the original array is intact. The item
+ string is dynamically copied. If the array is NULL, allocate a new
+ array. Otherwise, extend the array. Make sure the array is always
+ NULL-terminated. Input string might not be '\0'-terminated. */
+char **str_array_append(char **array, size_t nitems, const char *item,
+                        size_t itemlen)
+{
+    /* Make a dynamic copy of the item. */
+    char *copy;
+    if (item == NULL)
+        copy = NULL;
+    else {
+        copy = (char*)malloc(itemlen + 1);
+        if (copy == NULL)
+            return NULL;
+        memcpy(copy, item, itemlen);
+        copy[itemlen] = '\0';
+    }
+	
+    /* Extend array with one element. Except extend it by two elements,
+	 in case it did not yet exist. This might mean it is a teeny bit
+	 too big, but we don't care. */
+    array = (char**)realloc(array, (nitems + 2) * sizeof(array[0]));
+    if (array == NULL) {
+        free(copy);
+        return NULL;
+    }
+	
+    /* Add copy of item to array, and return it. */
+    array[nitems] = copy;
+    array[nitems+1] = NULL;
+    return array;
+}
+
+
+/* Free a dynamic array of dynamic strings. */
+void str_array_free(char **array)
+{
+    if (array == NULL)
+        return;
+    for (size_t i = 0; array[i] != NULL; ++i)
+        free(array[i]);
+    free(array);
+}
+
+
+/* Split a string into substrings. Return dynamic array of dynamically
+ allocated substrings, or NULL if there was an error. Caller is
+ expected to free the memory, for example with str_array_free. */
+char **str_split(const char *input, const char *sep)
+{
+    size_t nitems = 0;
+    char **array = NULL;
+    const char *start = input;
+    const char *next = strstr(start, sep);
+    size_t seplen = strlen(sep);
+    const char *item;
+    size_t itemlen;
+	
+    for (;;) {
+        next = strstr(start, sep);
+        if (next == NULL) {
+            /* Add the remaining string (or empty string, if input ends with
+			 separator. */
+            char **newstr = str_array_append(array, nitems, start, strlen(start));
+            if (newstr == NULL) {
+                str_array_free(array);
+                return NULL;
+            }
+            array = newstr;
+            ++nitems;
+            break;
+        } else if (next == input) {
+            /* Input starts with separator. */
+            item = "";
+            itemlen = 0;
+        } else {
+            item = start;
+            itemlen = next - item;
+        }
+        char **newstr = str_array_append(array, nitems, item, itemlen);
+        if (newstr == NULL) {
+            str_array_free(array);
+            return NULL;
+        }
+        array = newstr;
+        ++nitems;
+        start = next + seplen;
+    }
+	
+    if (nitems == 0) {
+        /* Input does not contain separator at all. */
+        assert(array == NULL);
+        array = str_array_append(array, nitems, input, strlen(input));
+    }
+	
+    return array;
+}
+
+
+/* Return length of a NULL-delimited array of strings. */
+size_t str_array_len(char **array)
+{
+    size_t len;
+	
+    for (len = 0; array[len] != NULL; ++len)
+        continue;
+    return len;
+}
+
+#ifdef UNIT_TEST_STRING
+
+#define MAX_OUTPUT 20
+
+
+int main(void)
+{
+    struct {
+        const char *input;
+        const char *sep;
+        char *output[MAX_OUTPUT];
+    } tab[] = {
+        /* Input is empty string. Output should be a list with an empty
+		 string. */
+        {
+            "",
+            "and",
+            {
+                "",
+                NULL,
+            },
+        },
+        /* Input is exactly the separator. Output should be two empty
+		 strings. */
+        {
+            "and",
+            "and",
+            {
+                "",
+                "",
+                NULL,
+            },
+        },
+        /* Input is non-empty, but does not have separator. Output should
+		 be the same string. */
+        {
+            "foo",
+            "and",
+            {
+                "foo",
+                NULL,
+            },
+        },
+        /* Input is non-empty, and does have separator. */
+        {
+            "foo bar 1 and foo bar 2",
+            " and ",
+            {
+                "foo bar 1",
+                "foo bar 2",
+                NULL,
+            },
+        },
+    };
+    const int tab_len = sizeof(tab) / sizeof(tab[0]);
+    bool errors;
+	
+    errors = false;
+	
+    for (int i = 0; i < tab_len; ++i) {
+        printf("test %d\n", i);
+		
+        char **output = str_split(tab[i].input, tab[i].sep);
+        if (output == NULL) {
+            fprintf(stderr, "output is NULL\n");
+            errors = true;
+            break;
+        }
+        size_t num_output = str_array_len(output);
+        printf("num_output %lu\n", (unsigned long) num_output);
+		
+        size_t num_correct = str_array_len(tab[i].output);
+        if (num_output != num_correct) {
+            fprintf(stderr, "wrong number of outputs (%lu, not %lu)\n",
+                    (unsigned long) num_output, (unsigned long) num_correct);
+            errors = true;
+        } else {
+            for (size_t j = 0; j < num_output; ++j) {
+                if (strcmp(tab[i].output[j], output[j]) != 0) {
+                    fprintf(stderr, "output[%lu] is '%s' not '%s'\n",
+                            (unsigned long) j, output[j], tab[i].output[j]);
+                    errors = true;
+                    break;
+                }
+            }
+        }
+		
+        str_array_free(output);
+        printf("\n");
+    }
+	
+    if (errors)
+        return EXIT_FAILURE;
+    return 0;
+}
+
+#endif//
+
+

btgui/urdf/boost_replacement/string_split.h

@@ -0,0 +1,31 @@
+
+#ifndef STRING_SPLIT_H
+#define STRING_SPLIT_H
+
+#include <cstring>
+#include <vector>
+#include <string>
+
+namespace boost
+{
+	void split( std::vector<std::string>&pieces, const std::string& vector_str, std::vector<std::string> separators);
+	std::vector<std::string> is_any_of(const char* seps);
+};
+
+///The string split C code is by Lars Wirzenius
+///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char
+
+
+/* Split a string into substrings. Return dynamic array of dynamically
+ allocated substrings, or NULL if there was an error. Caller is
+ expected to free the memory, for example with str_array_free. */
+char**	str_split(const char* input, const char* sep);
+
+/* Free a dynamic array of dynamic strings. */
+void str_array_free(char** array);
+
+/* Return length of a NULL-delimited array of strings. */
+size_t str_array_len(char** array);
+
+#endif //STRING_SPLIT_H
+

btgui/urdf/premake4.lua

@@ -0,0 +1,50 @@
+
+	
+		project "urdf_test"
+
+		flags {"FloatStrict"}
+		
+		language "C++"
+				
+		kind "ConsoleApp"
+		targetdir "../../bin"
+
+--		links {
+--		}
+
+		includedirs {
+		".",
+		"boost_replacement",
+		"../tinyxml",
+		"urdfdom/urdf_parser/include",
+		"urdfdom_headers/urdf_exception/include",
+		"urdfdom_headers/urdf_model/include",
+
+		}
+
+	
+		files {
+			"urdfdom/urdf_parser/src/check_urdf.cpp",	
+			"urdfdom/urdf_parser/src/pose.cpp",
+			"urdfdom/urdf_parser/src/model.cpp",
+			"urdfdom/urdf_parser/src/link.cpp",
+			"urdfdom/urdf_parser/src/joint.cpp",
+			"urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h",
+			"urdfdom_headers/urdf_exception/include/urdf_exception/exception.h",	
+                	"urdfdom_headers/urdf_model/include/urdf_model/pose.h",
+			"urdfdom_headers/urdf_model/include/urdf_model/model.h",
+			"urdfdom_headers/urdf_model/include/urdf_model/link.h",
+			"urdfdom_headers/urdf_model/include/urdf_model/joint.h",
+			"../tinyxml/tinystr.cpp",
+			"../tinyxml/tinyxml.cpp",
+			"../tinyxml/tinyxmlerror.cpp",
+			"../tinyxml/tinyxmlparser.cpp",
+			"boost_replacement/lexical_cast.h",
+			"boost_replacement/shared_ptr.h",
+			"boost_replacement/printf_console.cpp",
+			"boost_replacement/printf_console.h",
+			"boost_replacement/string_split.cpp",
+			"boost_replacement/string_split.h",
+			
+
+		}

btgui/urdf/urdfdom/LICENSE

@@ -0,0 +1,15 @@
+Software License Agreement (Apache License)
+
+Copyright 2011 John Hsu
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.

btgui/urdf/urdfdom/README.txt

@@ -0,0 +1,7 @@
+The URDF (U-Robot Description Format) library
+  provides core data structures and a simple XML parsers
+  for populating the class data structures from an URDF file.
+
+For now, the details of the URDF specifications reside on
+  http://ros.org/wiki/urdf
+

btgui/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h

@@ -0,0 +1,63 @@
+/*********************************************************************
+* 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>
+
+#ifndef M_PI
+#define M_PI 3.1415925438
+#endif //M_PI
+
+
+#include <urdf_model/model.h>
+
+
+
+
+namespace urdf{
+
+  my_shared_ptr<ModelInterface> parseURDF(const std::string &xml_string);
+
+}
+
+#endif

btgui/urdf/urdfdom/urdf_parser/src/check_urdf.cpp

@@ -0,0 +1,137 @@
+/*********************************************************************
+* 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(my_shared_ptr<const Link> link,int level = 0)
+{
+  level+=2;
+  int count = 0;
+  for (std::vector<my_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;
+    }
+  }
+
+}
+
+
+#define MSTRINGIFY(A) #A
+
+
+const char* urdf_char = MSTRINGIFY(
+<robot name="test_robot">
+<link name="link1" />
+<link name="link2" />
+<link name="link3" />
+<link name="link4" />
+
+<joint name="joint1" type="continuous">
+<parent link="link1"/>
+<child link="link2"/>
+</joint>
+
+<joint name="joint2" type="continuous">
+<parent link="link1"/>
+<child link="link3"/>
+</joint>
+
+<joint name="joint3" type="continuous">
+<parent link="link3"/>
+<child link="link4"/>
+</joint>
+</robot>);
+
+
+int main(int argc, char** argv)
+{
+    
+  std::string xml_string;
+    
+  if (argc < 2){
+    std::cerr << "No URDF file name provided, using a dummy test URDF" << std::endl;
+
+      xml_string = std::string(urdf_char);
+      
+  } else
+  {
+
+      
+      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();
+  }
+    
+  my_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
+  my_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;
+}
+

btgui/urdf/urdfdom/urdf_parser/src/joint.cpp

@@ -0,0 +1,579 @@
+/*********************************************************************
+* 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 <sstream>
+#include <urdf_model/joint.h>
+#ifdef URDF_USE_BOOST
+#include <boost/lexical_cast.hpp>
+#else
+#include <lexical_cast.h>
+#endif
+#include <urdf_model/pose.h>
+
+#ifdef URDF_USE_CONSOLE_BRIDGE
+	#include <console_bridge/console.h>
+#else
+	#include "printf_console.h"
+#endif
+
+#include <tinyxml.h>
+#include <urdf_parser/urdf_parser.h>
+
+namespace urdf{
+
+bool parsePose(Pose &pose, TiXmlElement* xml);
+
+bool parseJointDynamics(JointDynamics &jd, TiXmlElement* config)
+{
+  jd.clear();
+
+  // Get joint damping
+  const char* damping_str = config->Attribute("damping");
+  if (damping_str == NULL){
+    logDebug("urdfdom.joint_dynamics: no damping, defaults to 0");
+    jd.damping = 0;
+  }
+  else
+  {
+    try
+    {
+      jd.damping = boost::lexical_cast<double>(damping_str);
+    }
+    catch (boost::bad_lexical_cast &e)
+    {
+      logError("damping value (%s) is not a float: %s",damping_str, e.what());
+      return false;
+    }
+  }
+
+  // Get joint friction
+  const char* friction_str = config->Attribute("friction");
+  if (friction_str == NULL){
+    logDebug("urdfdom.joint_dynamics: no friction, defaults to 0");
+    jd.friction = 0;
+  }
+  else
+  {
+    try
+    {
+      jd.friction = boost::lexical_cast<double>(friction_str);
+    }
+    catch (boost::bad_lexical_cast &e)
+    {
+      logError("friction value (%s) is not a float: %s",friction_str, e.what());
+      return false;
+    }
+  }
+
+  if (damping_str == NULL && friction_str == NULL)
+  {
+    logError("joint dynamics element specified with no damping and no friction");
+    return false;
+  }
+  else{
+    logDebug("urdfdom.joint_dynamics: damping %f and friction %f", jd.damping, jd.friction);
+    return true;
+  }
+}
+
+bool parseJointLimits(JointLimits &jl, TiXmlElement* config)
+{
+  jl.clear();
+
+  // Get lower joint limit
+  const char* lower_str = config->Attribute("lower");
+  if (lower_str == NULL){
+    logDebug("urdfdom.joint_limit: no lower, defaults to 0");
+    jl.lower = 0;
+  }
+  else
+  {
+    try
+    {
+      jl.lower = boost::lexical_cast<double>(lower_str);
+    }
+    catch (boost::bad_lexical_cast &e)
+    {
+      logError("lower value (%s) is not a float: %s", lower_str, e.what());
+      return false;
+    }
+  }
+
+  // Get upper joint limit
+  const char* upper_str = config->Attribute("upper");
+  if (upper_str == NULL){
+    logDebug("urdfdom.joint_limit: no upper, , defaults to 0");
+    jl.upper = 0;
+  }
+  else
+  {
+    try
+    {
+      jl.upper = boost::lexical_cast<double>(upper_str);
+    }
+    catch (boost::bad_lexical_cast &e)
+    {
+      logError("upper value (%s) is not a float: %s",upper_str, e.what());
+      return false;
+    }
+  }
+
+  // Get joint effort limit
+  const char* effort_str = config->Attribute("effort");
+  if (effort_str == NULL){
+    logError("joint limit: no effort");
+    return false;
+  }
+  else
+  {
+    try
+    {
+      jl.effort = boost::lexical_cast<double>(effort_str);
+    }
+    catch (boost::bad_lexical_cast &e)
+    {
+      logError("effort value (%s) is not a float: %s",effort_str, e.what());
+      return false;
+    }
+  }
+
+  // Get joint velocity limit
+  const char* velocity_str = config->Attribute("velocity");
+  if (velocity_str == NULL){
+    logError("joint limit: no velocity");
+    return false;
+  }
+  else
+  {
+    try
+    {
+      jl.velocity = boost::lexical_cast<double>(velocity_str);
+    }
+    catch (boost::bad_lexical_cast &e)
+    {
+      logError("velocity value (%s) is not a float: %s",velocity_str, e.what());
+      return false;
+    }
+  }
+
+  return true;
+}
+
+bool parseJointSafety(JointSafety &js, TiXmlElement* config)
+{
+  js.clear();
+
+  // Get soft_lower_limit joint limit
+  const char* soft_lower_limit_str = config->Attribute("soft_lower_limit");
+  if (soft_lower_limit_str == NULL)
+  {
+    logDebug("urdfdom.joint_safety: no soft_lower_limit, using default value");
+    js.soft_lower_limit = 0;
+  }
+  else
+  {
+    try
+    {
+      js.soft_lower_limit = boost::lexical_cast<double>(soft_lower_limit_str);
+    }
+    catch (boost::bad_lexical_cast &e)
+    {
+      logError("soft_lower_limit value (%s) is not a float: %s",soft_lower_limit_str, e.what());
+      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)
+  {
+    logDebug("urdfdom.joint_safety: no soft_upper_limit, using default value");
+    js.soft_upper_limit = 0;
+  }
+  else
+  {
+    try
+    {
+      js.soft_upper_limit = boost::lexical_cast<double>(soft_upper_limit_str);
+    }
+    catch (boost::bad_lexical_cast &e)
+    {
+      logError("soft_upper_limit value (%s) is not a float: %s",soft_upper_limit_str, e.what());
+      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)
+  {
+    logDebug("urdfdom.joint_safety: no k_position, using default value");
+    js.k_position = 0;
+  }
+  else
+  {
+    try
+    {
+      js.k_position = boost::lexical_cast<double>(k_position_str);
+    }
+    catch (boost::bad_lexical_cast &e)
+    {
+      logError("k_position value (%s) is not a float: %s",k_position_str, e.what());
+      return false;
+    }
+  }
+  // Get k_velocity_ safety velocity gain
+  const char* k_velocity_str = config->Attribute("k_velocity");
+  if (k_velocity_str == NULL)
+  {
+    logError("joint safety: no k_velocity");
+    return false;
+  }
+  else
+  {
+    try
+    {
+      js.k_velocity = boost::lexical_cast<double>(k_velocity_str);
+    }
+    catch (boost::bad_lexical_cast &e)
+    {
+      logError("k_velocity value (%s) is not a float: %s",k_velocity_str, e.what());
+      return false;
+    }
+  }
+
+  return true;
+}
+
+bool parseJointCalibration(JointCalibration &jc, TiXmlElement* config)
+{
+  jc.clear();
+
+  // Get rising edge position
+  const char* rising_position_str = config->Attribute("rising");
+  if (rising_position_str == NULL)
+  {
+    logDebug("urdfdom.joint_calibration: no rising, using default value");
+    jc.rising.reset(0);
+  }
+  else
+  {
+    try
+    {
+      jc.rising.reset(new double(boost::lexical_cast<double>(rising_position_str)));
+    }
+    catch (boost::bad_lexical_cast &e)
+    {
+      logError("risingvalue (%s) is not a float: %s",rising_position_str, e.what());
+      return false;
+    }
+  }
+
+  // Get falling edge position
+  const char* falling_position_str = config->Attribute("falling");
+  if (falling_position_str == NULL)
+  {
+    logDebug("urdfdom.joint_calibration: no falling, using default value");
+    jc.falling.reset(0);
+  }
+  else
+  {
+    try
+    {
+      jc.falling.reset(new double(boost::lexical_cast<double>(falling_position_str)));
+    }
+    catch (boost::bad_lexical_cast &e)
+    {
+      logError("fallingvalue (%s) is not a float: %s",falling_position_str, e.what());
+      return false;
+    }
+  }
+
+  return true;
+}
+
+bool parseJointMimic(JointMimic &jm, TiXmlElement* config)
+{
+  jm.clear();
+
+  // Get name of joint to mimic
+  const char* joint_name_str = config->Attribute("joint");
+
+  if (joint_name_str == NULL)
+  {
+    logError("joint mimic: no mimic joint specified");
+    return false;
+  }
+  else
+    jm.joint_name = joint_name_str;
+  
+  // Get mimic multiplier
+  const char* multiplier_str = config->Attribute("multiplier");
+
+  if (multiplier_str == NULL)
+  {
+    logDebug("urdfdom.joint_mimic: no multiplier, using default value of 1");
+    jm.multiplier = 1;    
+  }
+  else
+  {
+    try
+    {
+      jm.multiplier = boost::lexical_cast<double>(multiplier_str);
+    }
+    catch (boost::bad_lexical_cast &e)
+    {
+      logError("multiplier value (%s) is not a float: %s",multiplier_str, e.what());
+      return false;
+    }
+  }
+
+  
+  // Get mimic offset
+  const char* offset_str = config->Attribute("offset");
+  if (offset_str == NULL)
+  {
+    logDebug("urdfdom.joint_mimic: no offset, using default value of 0");
+    jm.offset = 0;
+  }
+  else
+  {
+    try
+    {
+      jm.offset = boost::lexical_cast<double>(offset_str);
+    }
+    catch (boost::bad_lexical_cast &e)
+    {
+      logError("offset value (%s) is not a float: %s",offset_str, e.what());
+      return false;
+    }
+  }
+
+  return true;
+}
+
+bool parseJoint(Joint &joint, TiXmlElement* config)
+{
+  joint.clear();
+
+  // Get Joint Name
+  const char *name = config->Attribute("name");
+  if (!name)
+  {
+    logError("unnamed joint found");
+    return false;
+  }
+  joint.name = name;
+
+  // Get transform from Parent Link to Joint Frame
+  TiXmlElement *origin_xml = config->FirstChildElement("origin");
+  if (!origin_xml)
+  {
+    logDebug("urdfdom: Joint [%s] missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", joint.name.c_str());
+    joint.parent_to_joint_origin_transform.clear();
+  }
+  else
+  {
+    if (!parsePose(joint.parent_to_joint_origin_transform, origin_xml))
+    {
+      joint.parent_to_joint_origin_transform.clear();
+      logError("Malformed parent origin element for joint [%s]", joint.name.c_str());
+      return false;
+    }
+  }
+
+  // Get Parent Link
+  TiXmlElement *parent_xml = config->FirstChildElement("parent");
+  if (parent_xml)
+  {
+    const char *pname = parent_xml->Attribute("link");
+    if (!pname)
+    {
+      logInform("no parent link name specified for Joint link [%s]. this might be the root?", joint.name.c_str());
+    }
+    else
+    {
+      joint.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)
+    {
+      logInform("no child link name specified for Joint link [%s].", joint.name.c_str());
+    }
+    else
+    {
+      joint.child_link_name = std::string(pname);
+    }
+  }
+
+  // Get Joint type
+  const char* type_char = config->Attribute("type");
+  if (!type_char)
+  {
+    logError("joint [%s] has no type, check to see if it's a reference.", joint.name.c_str());
+    return false;
+  }
+  
+  std::string type_str = type_char;
+  if (type_str == "planar")
+    joint.type = Joint::PLANAR;
+  else if (type_str == "floating")
+    joint.type = Joint::FLOATING;
+  else if (type_str == "revolute")
+    joint.type = Joint::REVOLUTE;
+  else if (type_str == "continuous")
+    joint.type = Joint::CONTINUOUS;
+  else if (type_str == "prismatic")
+    joint.type = Joint::PRISMATIC;
+  else if (type_str == "fixed")
+    joint.type = Joint::FIXED;
+  else
+  {
+    logError("Joint [%s] has no known type [%s]", joint.name.c_str(), type_str.c_str());
+    return false;
+  }
+
+  // Get Joint Axis
+  if (joint.type != Joint::FLOATING && joint.type != Joint::FIXED)
+  {
+    // axis
+    TiXmlElement *axis_xml = config->FirstChildElement("axis");
+    if (!axis_xml){
+      logDebug("urdfdom: no axis elemement for Joint link [%s], defaulting to (1,0,0) axis", joint.name.c_str());
+      joint.axis = Vector3(1.0, 0.0, 0.0);
+    }
+    else{
+      if (axis_xml->Attribute("xyz")){
+        try {
+          joint.axis.init(axis_xml->Attribute("xyz"));
+        }
+        catch (ParseError &e) {
+          joint.axis.clear();
+          logError("Malformed axis element for joint [%s]: %s", joint.name.c_str(), e.what());
+          return false;
+        }
+      }
+    }
+  }
+
+  // Get limit
+  TiXmlElement *limit_xml = config->FirstChildElement("limit");
+  if (limit_xml)
+  {
+    joint.limits.reset(new JointLimits());
+    if (!parseJointLimits(*joint.limits, limit_xml))
+    {
+      logError("Could not parse limit element for joint [%s]", joint.name.c_str());
+      joint.limits.reset(0);
+      return false;
+    }
+  }
+  else if (joint.type == Joint::REVOLUTE)
+  {
+    logError("Joint [%s] is of type REVOLUTE but it does not specify limits", joint.name.c_str());
+    return false;
+  }
+  else if (joint.type == Joint::PRISMATIC)
+  {
+    logError("Joint [%s] is of type PRISMATIC without limits", joint.name.c_str()); 
+    return false;
+  }
+
+  // Get safety
+  TiXmlElement *safety_xml = config->FirstChildElement("safety_controller");
+  if (safety_xml)
+  {
+    joint.safety.reset(new JointSafety());
+    if (!parseJointSafety(*joint.safety, safety_xml))
+    {
+      logError("Could not parse safety element for joint [%s]", joint.name.c_str());
+      joint.safety.reset(0);
+      return false;
+    }
+  }
+
+  // Get calibration
+  TiXmlElement *calibration_xml = config->FirstChildElement("calibration");
+  if (calibration_xml)
+  {
+    joint.calibration.reset(new JointCalibration());
+    if (!parseJointCalibration(*joint.calibration, calibration_xml))
+    {
+      logError("Could not parse calibration element for joint  [%s]", joint.name.c_str());
+      joint.calibration.reset(0);
+      return false;
+    }
+  }
+
+  // Get Joint Mimic
+  TiXmlElement *mimic_xml = config->FirstChildElement("mimic");
+  if (mimic_xml)
+  {
+    joint.mimic.reset(new JointMimic());
+    if (!parseJointMimic(*joint.mimic, mimic_xml))
+    {
+      logError("Could not parse mimic element for joint  [%s]", joint.name.c_str());
+      joint.mimic.reset(0);
+      return false;
+    }
+  }
+
+  // Get Dynamics
+  TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
+  if (prop_xml)
+  {
+    joint.dynamics.reset(new JointDynamics());
+    if (!parseJointDynamics(*joint.dynamics, prop_xml))
+    {
+      logError("Could not parse joint_dynamics element for joint [%s]", joint.name.c_str());
+      joint.dynamics.reset(0);
+      return false;
+    }
+  }
+
+  return true;
+}
+
+
+
+
+}

btgui/urdf/urdfdom/urdf_parser/src/link.cpp

@@ -0,0 +1,505 @@
+/*********************************************************************
+* 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 <urdf_model/link.h>
+//#include <fstream>
+//#include <sstream>
+#ifdef URDF_USE_BOOST
+#include <boost/lexical_cast.hpp>
+#else
+#include <lexical_cast.h>
+#endif
+
+#include <algorithm>
+#include <tinyxml.h>
+#ifdef URDF_USE_CONSOLE_BRIDGE
+#include <console_bridge/console.h>
+#else
+#include "printf_console.h"
+#endif
+
+
+
+namespace urdf{
+
+bool parsePose(Pose &pose, TiXmlElement* xml);
+
+bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok)
+{
+  bool has_rgb = false;
+  bool has_filename = false;
+
+  material.clear();
+
+  if (!config->Attribute("name"))
+  {
+    logError("Material must contain a name attribute");
+    return false;
+  }
+  
+  material.name = config->Attribute("name");
+
+  // texture
+  TiXmlElement *t = config->FirstChildElement("texture");
+  if (t)
+  {
+    if (t->Attribute("filename"))
+    {
+      material.texture_filename = t->Attribute("filename");
+      has_filename = true;
+    }
+  }
+
+  // color
+  TiXmlElement *c = config->FirstChildElement("color");
+  if (c)
+  {
+    if (c->Attribute("rgba")) {
+
+      try {
+        material.color.init(c->Attribute("rgba"));
+        has_rgb = true;
+      }
+      catch (ParseError &e) {  
+        material.color.clear();
+        logError(std::string("Material [" + material.name + "] has malformed color rgba values: " + e.what()).c_str());
+      }
+    }
+  }
+
+  if (!has_rgb && !has_filename) {
+    if (!only_name_is_ok) // no need for an error if only name is ok
+    {
+      if (!has_rgb) logError(std::string("Material ["+material.name+"] color has no rgba").c_str());
+      if (!has_filename) logError(std::string("Material ["+material.name+"] not defined in file").c_str());
+    }
+    return false;
+  }
+  return true;
+}
+
+
+bool parseSphere(Sphere &s, TiXmlElement *c)
+{
+  s.clear();
+
+  s.type = Geometry::SPHERE;
+  if (!c->Attribute("radius"))
+  {
+    logError("Sphere shape must have a radius attribute");
+    return false;
+  }
+
+  try
+  {
+    s.radius = boost::lexical_cast<double>(c->Attribute("radius"));
+  }
+  catch (boost::bad_lexical_cast &e)
+  {
+   // std::stringstream stm;
+   // stm << "radius [" << c->Attribute("radius") << "] is not a valid float: " << e.what();
+   // logError(stm.str().c_str());
+      logError("radius issue");
+    return false;
+  }
+  
+  return true;
+}
+
+bool parseBox(Box &b, TiXmlElement *c)
+{
+  b.clear();
+  
+  b.type = Geometry::BOX;
+  if (!c->Attribute("size"))
+  {
+    logError("Box shape has no size attribute");
+    return false;
+  }
+  try
+  {
+    b.dim.init(c->Attribute("size"));
+  }
+  catch (ParseError &e)
+  {
+    b.dim.clear();
+    logError(e.what());
+    return false;
+  }
+  return true;
+}
+
+bool parseCylinder(Cylinder &y, TiXmlElement *c)
+{
+  y.clear();
+
+  y.type = Geometry::CYLINDER;
+  if (!c->Attribute("length") ||
+      !c->Attribute("radius"))
+  {
+    logError("Cylinder shape must have both length and radius attributes");
+    return false;
+  }
+
+  try
+  {
+    y.length = boost::lexical_cast<double>(c->Attribute("length"));
+  }
+  catch (boost::bad_lexical_cast &e)
+  {
+  //  std::stringstream stm;
+   // stm << "length [" << c->Attribute("length") << "] is not a valid float";
+    //logError(stm.str().c_str());
+      logError("length");
+      return false;
+  }
+
+  try
+  {
+    y.radius = boost::lexical_cast<double>(c->Attribute("radius"));
+  }
+  catch (boost::bad_lexical_cast &e)
+  {
+ //   std::stringstream stm;
+   // stm << "radius [" << c->Attribute("radius") << "] is not a valid float";
+    //logError(stm.str().c_str());
+      logError("radius");
+      return false;
+  }
+  return true;
+}
+
+
+bool parseMesh(Mesh &m, TiXmlElement *c)
+{
+  m.clear();
+
+  m.type = Geometry::MESH;
+  if (!c->Attribute("filename")) {
+    logError("Mesh must contain a filename attribute");
+    return false;
+  }
+
+  m.filename = c->Attribute("filename");
+
+  if (c->Attribute("scale")) {
+    try {
+      m.scale.init(c->Attribute("scale"));
+    }
+    catch (ParseError &e) {
+      m.scale.clear();
+      logError("Mesh scale was specified, but could not be parsed: %s", e.what());
+      return false;
+    }
+  }
+  else
+  {
+    m.scale.x = m.scale.y = m.scale.z = 1;
+  }
+  return true;
+}
+
+my_shared_ptr<Geometry> parseGeometry(TiXmlElement *g)
+{
+  my_shared_ptr<Geometry> geom;
+  if (!g) return geom;
+
+  TiXmlElement *shape = g->FirstChildElement();
+  if (!shape)
+  {
+    logError("Geometry tag contains no child element.");
+    return geom;
+  }
+
+  const std::string type_name = shape->ValueTStr().c_str();
+  if (type_name == "sphere")
+  {
+    Sphere *s = new Sphere();
+    geom.reset(s);
+    if (parseSphere(*s, shape))
+      return geom;
+  }
+  else if (type_name == "box")
+  {
+    Box *b = new Box();
+    geom.reset(b);
+    if (parseBox(*b, shape))
+      return geom;
+  }
+  else if (type_name == "cylinder")
+  {
+    Cylinder *c = new Cylinder();
+    geom.reset(c);
+    if (parseCylinder(*c, shape))
+      return geom;
+  }
+  else if (type_name == "mesh")
+  {
+    Mesh *m = new Mesh();
+    geom.reset(m);
+    if (parseMesh(*m, shape))
+      return geom;    
+  }
+  else
+  {
+    logError("Unknown geometry type '%s'", type_name.c_str());
+    return geom;
+  }
+  
+  return my_shared_ptr<Geometry>();
+}
+
+bool parseInertial(Inertial &i, TiXmlElement *config)
+{
+  i.clear();
+
+  // Origin
+  TiXmlElement *o = config->FirstChildElement("origin");
+  if (o)
+  {
+    if (!parsePose(i.origin, o))
+      return false;
+  }
+
+  TiXmlElement *mass_xml = config->FirstChildElement("mass");
+  if (!mass_xml)
+  {
+    logError("Inertial element must have a mass element");
+    return false;
+  }
+  if (!mass_xml->Attribute("value"))
+  {
+    logError("Inertial: mass element must have value attribute");
+    return false;
+  }
+
+  try
+  {
+    i.mass = boost::lexical_cast<double>(mass_xml->Attribute("value"));
+  }
+  catch (boost::bad_lexical_cast &e)
+  {
+  //  std::stringstream stm;
+   // stm << "Inertial: mass [" << mass_xml->Attribute("value")
+    //    << "] is not a float";
+    //logError(stm.str().c_str());
+      logError("Inertial mass issue");
+      return false;
+  }
+
+  TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
+  if (!inertia_xml)
+  {
+    logError("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")))
+  {
+    logError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
+    return false;
+  }
+  try
+  {
+    i.ixx  = boost::lexical_cast<double>(inertia_xml->Attribute("ixx"));
+    i.ixy  = boost::lexical_cast<double>(inertia_xml->Attribute("ixy"));
+    i.ixz  = boost::lexical_cast<double>(inertia_xml->Attribute("ixz"));
+    i.iyy  = boost::lexical_cast<double>(inertia_xml->Attribute("iyy"));
+    i.iyz  = boost::lexical_cast<double>(inertia_xml->Attribute("iyz"));
+    i.izz  = boost::lexical_cast<double>(inertia_xml->Attribute("izz"));
+  }
+  catch (boost::bad_lexical_cast &e)
+  {
+   /* std::stringstream stm;
+    stm << "Inertial: one of the inertia elements is not a valid double:"
+        << " ixx [" << inertia_xml->Attribute("ixx") << "]"
+        << " ixy [" << inertia_xml->Attribute("ixy") << "]"
+        << " ixz [" << inertia_xml->Attribute("ixz") << "]"
+        << " iyy [" << inertia_xml->Attribute("iyy") << "]"
+        << " iyz [" << inertia_xml->Attribute("iyz") << "]"
+        << " izz [" << inertia_xml->Attribute("izz") << "]";
+    logError(stm.str().c_str());
+    */
+      logError("Inertia error");
+      
+    return false;
+  }
+  return true;
+}
+
+bool parseVisual(Visual &vis, TiXmlElement *config)
+{
+  vis.clear();
+
+  // Origin
+  TiXmlElement *o = config->FirstChildElement("origin");
+  if (o) {
+    if (!parsePose(vis.origin, o))
+      return false;
+  }
+
+  // Geometry
+  TiXmlElement *geom = config->FirstChildElement("geometry");
+  vis.geometry = parseGeometry(geom);
+  if (!vis.geometry)
+    return false;
+
+  const char *name_char = config->Attribute("name");
+  if (name_char)
+    vis.name = name_char;
+
+  // Material
+  TiXmlElement *mat = config->FirstChildElement("material");
+  if (mat) {
+    // get material name
+    if (!mat->Attribute("name")) {
+      logError("Visual material must contain a name attribute");
+      return false;
+    }
+    vis.material_name = mat->Attribute("name");
+    
+    // try to parse material element in place
+    vis.material.reset(new Material());
+    if (!parseMaterial(*vis.material, mat, true))
+    {
+      logDebug("urdfdom: material has only name, actual material definition may be in the model");
+    }
+  }
+  
+  return true;
+}
+
+bool parseCollision(Collision &col, TiXmlElement* config)
+{  
+  col.clear();
+
+  // Origin
+  TiXmlElement *o = config->FirstChildElement("origin");
+  if (o) {
+    if (!parsePose(col.origin, o))
+      return false;
+  }
+  
+  // Geometry
+  TiXmlElement *geom = config->FirstChildElement("geometry");
+  col.geometry = parseGeometry(geom);
+  if (!col.geometry)
+    return false;
+
+  const char *name_char = config->Attribute("name");
+  if (name_char)
+    col.name = name_char;
+
+  return true;
+}
+
+bool parseLink(Link &link, TiXmlElement* config)
+{
+  
+  link.clear();
+
+  const char *name_char = config->Attribute("name");
+  if (!name_char)
+  {
+    logError("No name given for the link.");
+    return false;
+  }
+  link.name = std::string(name_char);
+
+  // Inertial (optional)
+  TiXmlElement *i = config->FirstChildElement("inertial");
+  if (i)
+  {
+    link.inertial.reset(new Inertial());
+    if (!parseInertial(*link.inertial, i))
+    {
+      logError("Could not parse inertial element for Link [%s]", link.name.c_str());
+      return false;
+    }
+  }
+
+  // Multiple Visuals (optional)
+  for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
+  {
+
+    my_shared_ptr<Visual> vis;
+    vis.reset(new Visual());
+    if (parseVisual(*vis, vis_xml))
+    {
+      link.visual_array.push_back(vis);
+    }
+    else
+    {
+      vis.reset(0);
+      logError("Could not parse visual element for Link [%s]", link.name.c_str());
+      return false;
+    }
+  }
+
+  // Visual (optional)
+  // Assign the first visual to the .visual ptr, if it exists
+  if (!link.visual_array.empty())
+    link.visual = link.visual_array[0];
+  
+  // Multiple Collisions (optional)
+  for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
+  {
+    my_shared_ptr<Collision> col;
+    col.reset(new Collision());
+    if (parseCollision(*col, col_xml))
+    {      
+      link.collision_array.push_back(col);
+    }
+    else
+    {
+      col.reset(0);
+      logError("Could not parse collision element for Link [%s]",  link.name.c_str());
+      return false;
+    }
+  }
+  
+  // Collision (optional)  
+  // Assign the first collision to the .collision ptr, if it exists
+  if (!link.collision_array.empty())
+    link.collision = link.collision_array[0];
+  return true;
+
+}
+
+}

btgui/urdf/urdfdom/urdf_parser/src/model.cpp

@@ -0,0 +1,238 @@
+/*********************************************************************
+* 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 <vector>
+#include "urdf_parser/urdf_parser.h"
+#ifdef URDF_USE_CONSOLE_BRIDGE
+	#include <console_bridge/console.h>
+#else
+	#include "printf_console.h"
+#endif
+
+namespace urdf{
+
+bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok);
+bool parseLink(Link &link, TiXmlElement *config);
+bool parseJoint(Joint &joint, TiXmlElement *config);
+
+
+my_shared_ptr<ModelInterface>  parseURDF(const std::string &xml_string)
+{
+  my_shared_ptr<ModelInterface> model(new ModelInterface);
+  model->clear();
+
+  TiXmlDocument xml_doc;
+  xml_doc.Parse(xml_string.c_str());
+  if (xml_doc.Error())
+  {
+    logError(xml_doc.ErrorDesc());
+    xml_doc.ClearError();
+    model.reset(0);
+    return model;
+  }
+
+  TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
+  if (!robot_xml)
+  {
+    logError("Could not find the 'robot' element in the xml file");
+    model.reset(0);
+    return model;
+  }
+
+  // Get robot name
+  const char *name = robot_xml->Attribute("name");
+  if (!name)
+  {
+    logError("No name given for the robot.");
+    model.reset(0);
+    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"))
+  {
+    my_shared_ptr<Material> material;
+    material.reset(new Material);
+
+    try {
+      parseMaterial(*material, material_xml, false); // material needs to be fully defined here
+      if (model->getMaterial(material->name))
+      {
+        logError("material '%s' is not unique.", material->name.c_str());
+        material.reset(0);
+        model.reset(0);
+        return model;
+      }
+      else
+      {
+        model->materials_.insert(make_pair(material->name,material));
+        logDebug("urdfdom: successfully added a new material '%s'", material->name.c_str());
+      }
+    }
+    catch (ParseError &e) {
+      logError("material xml is not initialized correctly");
+      material.reset(0);
+      model.reset(0);
+      return model;
+    }
+  }
+
+  // Get all Link elements
+  for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
+  {
+    my_shared_ptr<Link> link;
+    link.reset(new Link);
+
+    try {
+      parseLink(*link, link_xml);
+      if (model->getLink(link->name))
+      {
+        logError("link '%s' is not unique.", link->name.c_str());
+        model.reset(0);
+        return model;
+      }
+      else
+      {
+        // set link visual material
+        logDebug("urdfdom: setting link '%s' material", link->name.c_str());
+        if (link->visual)
+        {
+          if (!link->visual->material_name.empty())
+          {
+            if (model->getMaterial(link->visual->material_name))
+            {
+              logDebug("urdfdom: 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)
+              {
+                logDebug("urdfdom: 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
+              {
+                logError("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str());
+                model.reset(0);
+                return model;
+              }
+            }
+          }
+        }
+
+        model->links_.insert(make_pair(link->name,link));
+        logDebug("urdfdom: successfully added a new link '%s'", link->name.c_str());
+      }
+    }
+    catch (ParseError &e) {
+      logError("link xml is not initialized correctly");
+      model.reset(0);
+      return model;
+    }
+  }
+  if (model->links_.empty()){
+    logError("No link elements found in urdf file");
+    model.reset(0);
+    return model;
+  }
+
+  // Get all Joint elements
+  for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
+  {
+    my_shared_ptr<Joint> joint;
+    joint.reset(new Joint);
+
+    if (parseJoint(*joint, joint_xml))
+    {
+      if (model->getJoint(joint->name))
+      {
+        logError("joint '%s' is not unique.", joint->name.c_str());
+        model.reset(0);
+        return model;
+      }
+      else
+      {
+        model->joints_.insert(make_pair(joint->name,joint));
+        logDebug("urdfdom: successfully added a new joint '%s'", joint->name.c_str());
+      }
+    }
+    else
+    {
+      logError("joint xml is not initialized correctly");
+      model.reset(0);
+      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
+  try 
+  {
+    model->initTree(parent_link_tree);
+  }
+  catch(ParseError &e)
+  {
+    logError("Failed to build tree: %s", e.what());
+    model.reset(0);
+    return model;
+  }
+
+  // find the root link
+  try
+  {
+    model->initRoot(parent_link_tree);
+  }
+  catch(ParseError &e)
+  {
+    logError("Failed to find root link: %s", e.what());
+    model.reset(0);
+    return model;
+  }
+  
+  return model;
+}
+
+
+
+}
+

btgui/urdf/urdfdom/urdf_parser/src/pose.cpp

@@ -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: Wim Meeussen, John Hsu */
+
+
+#include <urdf_model/pose.h>
+#include <fstream>
+#include <sstream>
+//#include <boost/lexical_cast.hpp>
+#include <algorithm>
+
+#ifdef URDF_USE_CONSOLE_BRIDGE
+#include <console_bridge/console.h>
+#else
+#include "printf_console.h"
+#endif
+
+#include <tinyxml.h>
+#include <urdf_exception/exception.h>
+
+
+namespace urdf{
+
+bool parsePose(Pose &pose, TiXmlElement* xml)
+{
+  pose.clear();
+  if (xml)
+  {
+    const char* xyz_str = xml->Attribute("xyz");
+    if (xyz_str != NULL)
+    {
+      try {
+        pose.position.init(xyz_str);
+      }
+      catch (ParseError &e) {
+        logError(e.what());
+        return false;
+      }
+    }
+
+    const char* rpy_str = xml->Attribute("rpy");
+    if (rpy_str != NULL)
+    {
+      try {
+        pose.rotation.init(rpy_str);
+      }
+      catch (ParseError &e) {
+        logError(e.what());
+        return false;
+      }
+    }
+  }
+  return true;
+}
+
+
+}
+
+

btgui/urdf/urdfdom/urdf_parser/src/twist.cpp

@@ -0,0 +1,85 @@
+/*********************************************************************
+* 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 */
+
+
+#include <urdf_model/twist.h>
+#include <fstream>
+#include <sstream>
+#include <boost/lexical_cast.hpp>
+#include <algorithm>
+#include <tinyxml.h>
+#include <console_bridge/console.h>
+
+namespace urdf{
+
+bool parseTwist(Twist &twist, TiXmlElement* xml)
+{
+  twist.clear();
+  if (xml)
+  {
+    const char* linear_char = xml->Attribute("linear");
+    if (linear_char != NULL)
+    {
+      try {
+        twist.linear.init(linear_char);
+      }
+      catch (ParseError &e) {
+        twist.linear.clear();
+        logError("Malformed linear string [%s]: %s", linear_char, e.what());
+        return false;
+      }
+    }
+
+    const char* angular_char = xml->Attribute("angular");
+    if (angular_char != NULL)
+    {
+      try {
+        twist.angular.init(angular_char);
+      }
+      catch (ParseError &e) {
+        twist.angular.clear();
+        logError("Malformed angular [%s]: %s", angular_char, e.what());
+        return false;
+      }
+    }
+  }
+  return true;
+}
+
+}
+
+
+

btgui/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp

@@ -0,0 +1,154 @@
+/*********************************************************************
+* 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 */
+
+
+#include <urdf_model_state/model_state.h>
+#include <fstream>
+#include <sstream>
+#include <boost/lexical_cast.hpp>
+#include <algorithm>
+#include <tinyxml.h>
+#include <console_bridge/console.h>
+
+namespace urdf{
+
+bool parseModelState(ModelState &ms, TiXmlElement* config)
+{
+  ms.clear();
+
+  const char *name_char = config->Attribute("name");
+  if (!name_char)
+  {
+    logError("No name given for the model_state.");
+    return false;
+  }
+  ms.name = std::string(name_char);
+
+  const char *time_stamp_char = config->Attribute("time_stamp");
+  if (time_stamp_char)
+  {
+    try {
+      double sec = boost::lexical_cast<double>(time_stamp_char);
+      ms.time_stamp.set(sec);
+    }
+    catch (boost::bad_lexical_cast &e) {
+      logError("Parsing time stamp [%s] failed: %s", time_stamp_char, e.what());
+      return false;
+    }
+  }
+
+  TiXmlElement *joint_state_elem = config->FirstChildElement("joint_state");
+  if (joint_state_elem)
+  {
+    boost::shared_ptr<JointState> joint_state;
+    joint_state.reset(new JointState());
+
+    const char *joint_char = joint_state_elem->Attribute("joint");
+    if (joint_char)
+      joint_state->joint = std::string(joint_char);
+    else
+    {
+      logError("No joint name given for the model_state.");
+      return false;
+    }
+    
+    // parse position
+    const char *position_char = joint_state_elem->Attribute("position");
+    if (position_char)
+    {
+
+      std::vector<std::string> pieces;
+      boost::split( pieces, position_char, boost::is_any_of(" "));
+      for (unsigned int i = 0; i < pieces.size(); ++i){
+        if (pieces[i] != ""){
+          try {
+            joint_state->position.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
+          }
+          catch (boost::bad_lexical_cast &e) {
+            throw ParseError("position element ("+ pieces[i] +") is not a valid float");
+          }
+        }
+      }
+    }
+
+    // parse velocity
+    const char *velocity_char = joint_state_elem->Attribute("velocity");
+    if (velocity_char)
+    {
+
+      std::vector<std::string> pieces;
+      boost::split( pieces, velocity_char, boost::is_any_of(" "));
+      for (unsigned int i = 0; i < pieces.size(); ++i){
+        if (pieces[i] != ""){
+          try {
+            joint_state->velocity.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
+          }
+          catch (boost::bad_lexical_cast &e) {
+            throw ParseError("velocity element ("+ pieces[i] +") is not a valid float");
+          }
+        }
+      }
+    }
+
+    // parse effort
+    const char *effort_char = joint_state_elem->Attribute("effort");
+    if (effort_char)
+    {
+
+      std::vector<std::string> pieces;
+      boost::split( pieces, effort_char, boost::is_any_of(" "));
+      for (unsigned int i = 0; i < pieces.size(); ++i){
+        if (pieces[i] != ""){
+          try {
+            joint_state->effort.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
+          }
+          catch (boost::bad_lexical_cast &e) {
+            throw ParseError("effort element ("+ pieces[i] +") is not a valid float");
+          }
+        }
+      }
+    }
+
+    // add to vector
+    ms.joint_states.push_back(joint_state);
+  }
+};
+
+
+
+}
+
+

btgui/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp

@@ -0,0 +1,364 @@
+/*********************************************************************
+* 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 */
+
+
+#include <urdf_sensor/sensor.h>
+#include <fstream>
+#include <sstream>
+#include <boost/lexical_cast.hpp>
+#include <algorithm>
+#include <tinyxml.h>
+#include <console_bridge/console.h>
+
+namespace urdf{
+
+bool parsePose(Pose &pose, TiXmlElement* xml);
+
+bool parseCamera(Camera &camera, TiXmlElement* config)
+{
+  camera.clear();
+  camera.type = VisualSensor::CAMERA;
+
+  TiXmlElement *image = config->FirstChildElement("image");
+  if (image)
+  {
+    const char* width_char = image->Attribute("width");
+    if (width_char)
+    {
+      try
+      {
+        camera.width = boost::lexical_cast<unsigned int>(width_char);
+      }
+      catch (boost::bad_lexical_cast &e)
+      {
+        logError("Camera image width [%s] is not a valid int: %s", width_char, e.what());
+        return false;
+      }
+    }
+    else
+    {
+      logError("Camera sensor needs an image width attribute");
+      return false;
+    }
+
+    const char* height_char = image->Attribute("height");
+    if (height_char)
+    {
+      try
+      {
+        camera.height = boost::lexical_cast<unsigned int>(height_char);
+      }
+      catch (boost::bad_lexical_cast &e)
+      {
+        logError("Camera image height [%s] is not a valid int: %s", height_char, e.what());
+        return false;
+      }
+    }
+    else
+    {
+      logError("Camera sensor needs an image height attribute");
+      return false;
+    }
+
+    const char* format_char = image->Attribute("format");
+    if (format_char)
+      camera.format = std::string(format_char);
+    else
+    {
+      logError("Camera sensor needs an image format attribute");
+      return false;
+    }    
+
+    const char* hfov_char = image->Attribute("hfov");
+    if (hfov_char)
+    {
+      try
+      {
+        camera.hfov = boost::lexical_cast<double>(hfov_char);
+      }
+      catch (boost::bad_lexical_cast &e)
+      {
+        logError("Camera image hfov [%s] is not a valid float: %s", hfov_char, e.what());
+        return false;
+      }
+    }
+    else
+    {
+      logError("Camera sensor needs an image hfov attribute");
+      return false;
+    }
+
+    const char* near_char = image->Attribute("near");
+    if (near_char)
+    {
+      try
+      {
+        camera.near = boost::lexical_cast<double>(near_char);
+      }
+      catch (boost::bad_lexical_cast &e)
+      {
+        logError("Camera image near [%s] is not a valid float: %s", near_char, e.what());
+        return false;
+      }
+    }
+    else
+    {
+      logError("Camera sensor needs an image near attribute");
+      return false;
+    }
+
+    const char* far_char = image->Attribute("far");
+    if (far_char)
+    {
+      try
+      {
+        camera.far = boost::lexical_cast<double>(far_char);
+      }
+      catch (boost::bad_lexical_cast &e)
+      {
+        logError("Camera image far [%s] is not a valid float: %s", far_char, e.what());
+        return false;
+      }
+    }
+    else
+    {
+      logError("Camera sensor needs an image far attribute");
+      return false;
+    }
+    
+  }
+  else
+  {
+    logError("Camera sensor has no <image> element");
+    return false;
+  }
+  return true;
+}
+
+bool parseRay(Ray &ray, TiXmlElement* config)
+{
+  ray.clear();
+  ray.type = VisualSensor::RAY;
+
+  TiXmlElement *horizontal = config->FirstChildElement("horizontal");
+  if (horizontal)
+  {
+    const char* samples_char = horizontal->Attribute("samples");
+    if (samples_char)
+    {
+      try
+      {
+        ray.horizontal_samples = boost::lexical_cast<unsigned int>(samples_char);
+      }
+      catch (boost::bad_lexical_cast &e)
+      {
+        logError("Ray horizontal samples [%s] is not a valid float: %s", samples_char, e.what());
+        return false;
+      }
+    }
+
+    const char* resolution_char = horizontal->Attribute("resolution");
+    if (resolution_char)
+    {
+      try
+      {
+        ray.horizontal_resolution = boost::lexical_cast<double>(resolution_char);
+      }
+      catch (boost::bad_lexical_cast &e)
+      {
+        logError("Ray horizontal resolution [%s] is not a valid float: %s", resolution_char, e.what());
+        return false;
+      }
+    }   
+    
+    const char* min_angle_char = horizontal->Attribute("min_angle");
+    if (min_angle_char)
+    {
+      try
+      {
+        ray.horizontal_min_angle = boost::lexical_cast<double>(min_angle_char);
+      }
+      catch (boost::bad_lexical_cast &e)
+      {
+        logError("Ray horizontal min_angle [%s] is not a valid float: %s", min_angle_char, e.what());
+        return false;
+      }
+    }
+
+    const char* max_angle_char = horizontal->Attribute("max_angle");
+    if (max_angle_char)
+    {
+      try
+      {
+        ray.horizontal_max_angle = boost::lexical_cast<double>(max_angle_char);
+      }
+      catch (boost::bad_lexical_cast &e)
+      {
+        logError("Ray horizontal max_angle [%s] is not a valid float: %s", max_angle_char, e.what());
+        return false;
+      }
+    }
+  }
+  
+  TiXmlElement *vertical = config->FirstChildElement("vertical");
+  if (vertical)
+  {
+    const char* samples_char = vertical->Attribute("samples");
+    if (samples_char)
+    {
+      try
+      {
+        ray.vertical_samples = boost::lexical_cast<unsigned int>(samples_char);
+      }
+      catch (boost::bad_lexical_cast &e)
+      {
+        logError("Ray vertical samples [%s] is not a valid float: %s", samples_char, e.what());
+        return false;
+      }
+    }
+
+    const char* resolution_char = vertical->Attribute("resolution");
+    if (resolution_char)
+    {
+      try
+      {
+        ray.vertical_resolution = boost::lexical_cast<double>(resolution_char);
+      }
+      catch (boost::bad_lexical_cast &e)
+      {
+        logError("Ray vertical resolution [%s] is not a valid float: %s", resolution_char, e.what());
+        return false;
+      }
+    }   
+    
+    const char* min_angle_char = vertical->Attribute("min_angle");
+    if (min_angle_char)
+    {
+      try
+      {
+        ray.vertical_min_angle = boost::lexical_cast<double>(min_angle_char);
+      }
+      catch (boost::bad_lexical_cast &e)
+      {
+        logError("Ray vertical min_angle [%s] is not a valid float: %s", min_angle_char, e.what());
+        return false;
+      }
+    }
+
+    const char* max_angle_char = vertical->Attribute("max_angle");
+    if (max_angle_char)
+    {
+      try
+      {
+        ray.vertical_max_angle = boost::lexical_cast<double>(max_angle_char);
+      }
+      catch (boost::bad_lexical_cast &e)
+      {
+        logError("Ray vertical max_angle [%s] is not a valid float: %s", max_angle_char, e.what());
+        return false;
+      }
+    }
+  }
+}
+
+boost::shared_ptr<VisualSensor> parseVisualSensor(TiXmlElement *g)
+{
+  boost::shared_ptr<VisualSensor> visual_sensor;
+
+  // get sensor type
+  TiXmlElement *sensor_xml;
+  if (g->FirstChildElement("camera"))
+  {
+    Camera *camera = new Camera();
+    visual_sensor.reset(camera);
+    sensor_xml = g->FirstChildElement("camera");
+    if (!parseCamera(*camera, sensor_xml))
+      visual_sensor.reset();
+  }
+  else if (g->FirstChildElement("ray"))
+  {
+    Ray *ray = new Ray();
+    visual_sensor.reset(ray);
+    sensor_xml = g->FirstChildElement("ray");
+    if (!parseRay(*ray, sensor_xml))
+      visual_sensor.reset();
+  }
+  else
+  {
+    logError("No know sensor types [camera|ray] defined in <sensor> block");
+  }
+  return visual_sensor;
+}
+
+
+bool parseSensor(Sensor &sensor, TiXmlElement* config)
+{
+  sensor.clear();
+
+  const char *name_char = config->Attribute("name");
+  if (!name_char)
+  {
+    logError("No name given for the sensor.");
+    return false;
+  }
+  sensor.name = std::string(name_char);
+
+  // parse parent_link_name
+  const char *parent_link_name_char = config->Attribute("parent_link_name");
+  if (!parent_link_name_char)
+  {
+    logError("No parent_link_name given for the sensor.");
+    return false;
+  }
+  sensor.parent_link_name = std::string(parent_link_name_char);
+
+  // parse origin
+  TiXmlElement *o = config->FirstChildElement("origin");
+  if (o)
+  {
+    if (!parsePose(sensor.origin, o))
+      return false;
+  }
+
+  // parse sensor
+  sensor.sensor = parseVisualSensor(config);
+  return true;
+}
+
+
+}
+
+

btgui/urdf/urdfdom/urdf_parser/src/world.cpp

@@ -0,0 +1,71 @@
+/*********************************************************************
+* 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_world/world.h>
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
+#include <fstream>
+#include <sstream>
+#include <boost/lexical_cast.hpp>
+#include <algorithm>
+#include <tinyxml.h>
+#include <console_bridge/console.h>
+
+namespace urdf{
+
+bool parseWorld(World &world, TiXmlElement* config)
+{
+
+  // to be implemented
+
+  return true;
+}
+
+bool exportWorld(World &world, TiXmlElement* xml)
+{
+  TiXmlElement * world_xml = new TiXmlElement("world");
+  world_xml->SetAttribute("name", world.name);
+
+  // to be implemented
+  // exportModels(*world.models, world_xml);
+
+  xml->LinkEndChild(world_xml);
+
+  return true;
+}
+
+}

btgui/urdf/urdfdom/urdf_parser/test/memtest.cpp

@@ -0,0 +1,20 @@
+#include "urdf_parser/urdf_parser.h"
+#include <fstream>
+#include <iostream>
+
+int main(int argc, char** argv){
+  while (true){
+    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);
+  }
+}

btgui/urdf/urdfdom_headers/LICENSE

@@ -0,0 +1,15 @@
+Software License Agreement (Apache License)
+
+Copyright 2011 John Hsu
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.

btgui/urdf/urdfdom_headers/README.txt

@@ -0,0 +1,6 @@
+The URDF (U-Robot Description Format) headers
+  provides core data structure headers for URDF.
+
+For now, the details of the URDF specifications reside on
+  http://ros.org/wiki/urdf
+

btgui/urdf/urdfdom_headers/urdf_exception/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

btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h

@@ -0,0 +1,102 @@
+/*********************************************************************
+* 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 <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;
+#if 0
+    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)
+        {
+          return false;
+        }
+      }
+    }
+
+    if (rgba.size() != 4)
+    {
+      return false;
+    }
+#endif
+    this->r = rgba[0];
+    this->g = rgba[1];
+    this->b = rgba[2];
+    this->a = rgba[3];
+
+    return true;
+  };
+};
+
+}
+
+#endif
+

btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h

@@ -0,0 +1,234 @@
+/*********************************************************************
+* 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>
+#ifdef URDF_USE_BOOST
+#include <boost/shared_ptr.hpp>
+#define my_shared_ptr my_shared_ptr
+#else
+#include <shared_ptr.h>
+#endif
+
+#include "urdf_model/pose.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;
+  my_shared_ptr<double> 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
+  my_shared_ptr<JointDynamics> dynamics;
+
+  /// Joint Limits
+  my_shared_ptr<JointLimits> limits;
+
+  /// Unsupported Hidden Feature
+  my_shared_ptr<JointSafety> safety;
+
+  /// Unsupported Hidden Feature
+  my_shared_ptr<JointCalibration> calibration;
+
+  /// Option to Mimic another Joint
+  my_shared_ptr<JointMimic> 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(0);
+    this->limits.reset(0);
+    this->safety.reset(0);
+    this->calibration.reset(0);
+    this->type = UNKNOWN;
+  };
+};
+
+}
+
+#endif

btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h

@@ -0,0 +1,258 @@
+/*********************************************************************
+* 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>
+
+
+#ifdef URDF_USE_BOOST
+	#include <boost/shared_ptr.hpp>
+	#include <boost/weak_ptr.hpp>
+#else
+	#include <shared_ptr.h>
+#endif
+
+#include "joint.h"
+#include "color.h"
+#include "pose.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;
+  my_shared_ptr<Geometry> geometry;
+
+  std::string material_name;
+  my_shared_ptr<Material> material;
+
+  void clear()
+  {
+    origin.clear();
+    material_name.clear();
+    material.reset(0);
+    geometry.reset(0);
+    name.clear();
+  };
+
+  std::string name;
+};
+
+class Collision
+{
+public:
+  Collision() { this->clear(); };
+  Pose origin;
+  my_shared_ptr<Geometry> geometry;
+
+  void clear()
+  {
+    origin.clear();
+    geometry.reset(0);
+    name.clear();
+  };
+
+  std::string name;
+
+};
+
+
+class Link
+{
+public:
+  Link() { this->clear(); };
+
+  std::string name;
+
+  /// inertial element
+  my_shared_ptr<Inertial> inertial;
+
+  /// visual element
+  my_shared_ptr<Visual> visual;
+
+  /// collision element
+  my_shared_ptr<Collision> 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<my_shared_ptr<Collision> > 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<my_shared_ptr<Visual> > visual_array;
+
+  /// Parent Joint element
+  ///   explicitly stating "parent" because we want directional-ness for tree structure
+  ///   every link can have one parent
+  my_shared_ptr<Joint> parent_joint;
+
+  std::vector<my_shared_ptr<Joint> > child_joints;
+  std::vector<my_shared_ptr<Link> > child_links;
+
+  const Link* getParent() const
+  {return parent_link_;}
+
+  void setParent(const my_shared_ptr<Link> &parent)
+  {
+	  parent_link_ = parent.get();
+  }
+  
+  void clear()
+  {
+    this->name.clear();
+    this->inertial.reset(0);
+    this->visual.reset(0);
+    this->collision.reset(0);
+    this->parent_joint.reset(0);
+    this->child_joints.clear();
+    this->child_links.clear();
+    this->collision_array.clear();
+    this->visual_array.clear();
+  };
+
+private:
+//  boost::weak_ptr<Link> parent_link_;
+	const Link* parent_link_;
+
+};
+
+
+
+
+}
+
+#endif

btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h

@@ -0,0 +1,215 @@
+/*********************************************************************
+* 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_model/link.h>
+
+#include <urdf_exception/exception.h>
+
+namespace urdf {
+
+class ModelInterface
+{
+public:
+  my_shared_ptr<const Link> getRoot(void) const{return this->root_link_;};
+  my_shared_ptr<const Link> getLink(const std::string& name) const
+  {
+    my_shared_ptr<const Link> ptr;
+    if (this->links_.find(name) == this->links_.end())
+      ptr.reset(0);
+    else
+      ptr = this->links_.find(name)->second;
+    return ptr;
+  };
+  
+  my_shared_ptr<const Joint> getJoint(const std::string& name) const
+  {
+    my_shared_ptr<const Joint> ptr;
+    if (this->joints_.find(name) == this->joints_.end())
+      ptr.reset(0);
+    else
+      ptr = this->joints_.find(name)->second;
+    return ptr;
+  };
+  
+  
+  const std::string& getName() const {return name_;};
+  void getLinks(std::vector<my_shared_ptr<Link> >& links) const
+  {
+    for (std::map<std::string,my_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(0);
+  };
+  
+  /// non-const getLink()
+  void getLink(const std::string& name,my_shared_ptr<Link> &link) const
+  {
+    my_shared_ptr<Link> ptr;
+    if (this->links_.find(name) == this->links_.end())
+      ptr.reset(0);
+    else
+      ptr = this->links_.find(name)->second;
+    link = ptr;
+  };
+  
+  /// non-const getMaterial()
+  my_shared_ptr<Material> getMaterial(const std::string& name) const
+  {
+    my_shared_ptr<Material> ptr;
+    if (this->materials_.find(name) == this->materials_.end())
+      ptr.reset(0);
+    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,my_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;
+      
+      if (parent_link_name.empty() || child_link_name.empty())
+      {
+		  assert(0);
+
+      //  throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification.");
+      }
+      else
+      {
+        // find child and parent links
+        my_shared_ptr<Link> child_link, parent_link;
+        this->getLink(child_link_name, child_link);
+        if (!child_link)
+        {
+			assert(0);
+//          throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found");
+        }
+        this->getLink(parent_link_name, parent_link);
+        if (!parent_link)
+        {
+			assert(0);
+
+/*          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(0);
+    
+    // find the links that have no parent in the tree
+    for (std::map<std::string, my_shared_ptr<Link> >::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
+        {
+			assert(0);
+      //    throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]");
+        }
+      }
+    }
+    if (!this->root_link_)
+    {
+		assert(0);
+      //throw ParseError("No root link found. The robot xml is not a valid tree.");
+    }
+  }
+  
+  
+  /// \brief complete list of Links
+  std::map<std::string, my_shared_ptr<Link> > links_;
+  /// \brief complete list of Joints
+  std::map<std::string, my_shared_ptr<Joint> > joints_;
+  /// \brief complete list of Materials
+  std::map<std::string, my_shared_ptr<Material> > 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)
+  my_shared_ptr<Link> root_link_;
+
+
+
+};
+
+}
+
+#endif

btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h

@@ -0,0 +1,265 @@
+/*********************************************************************
+* 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 <sstream>
+#include <vector>
+#include <math.h>
+#ifndef M_PI
+#define M_PI 3.141592538
+#endif //M_PI
+
+#ifdef URDF_USE_BOOST
+	#include <boost/algorithm/string.hpp>
+	#include <boost/lexical_cast.hpp>
+#else
+	#include <string_split.h>
+	#include <lexical_cast.h>
+#endif //URDF_USE_BOOST
+
+#include <urdf_exception/exception.h>
+#include <assert.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;
+    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)
+		{
+			assert(0);
+         // throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)");
+        }
+      }
+    }
+	  
+	  
+	  
+  if (xyz.size() != 3)
+  {
+	  assert(0);
+	 // throw ParseError("Parser found " + boost::lexical_cast<std::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;
+
+    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;
+
+  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

btgui/urdf/urdfdom_headers/urdf_model/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
+

btgui/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h

@@ -0,0 +1,141 @@
+/*********************************************************************
+* 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 <boost/shared_ptr.hpp>
+#include <boost/weak_ptr.hpp>
+
+#include "urdf_model/pose.h"
+#include <urdf_model/twist.h>
+
+
+namespace urdf{
+
+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<boost::shared_ptr<JointState> > joint_states;
+
+};
+
+}
+
+#endif
+

btgui/urdf/urdfdom_headers/urdf_model_state/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

btgui/urdf/urdfdom_headers/urdf_sensor/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 <boost/shared_ptr.hpp>
+#include <boost/weak_ptr.hpp>
+#include "urdf_model/pose.h"
+#include "urdf_model/joint.h"
+#include "urdf_model/link.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
+  boost::shared_ptr<VisualSensor> sensor;
+
+
+  /// Parent link element name.  A pointer is stored in parent_link_.
+  std::string parent_link_name;
+
+  boost::shared_ptr<Link> getParent() const
+  {return parent_link_.lock();};
+
+  void setParent(boost::shared_ptr<Link> parent)
+  {  this->parent_link_ = parent; }
+  
+  void clear()
+  {
+    this->name.clear();
+    this->sensor.reset();
+    this->parent_link_name.clear();
+    this->parent_link_.reset();
+  };
+
+private:
+  boost::weak_ptr<Link> parent_link_;
+
+};
+}
+#endif

btgui/urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h

@@ -0,0 +1,114 @@
+/*********************************************************************
+* 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 and
+   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 USDF_STATE_H
+#define USDF_STATE_H
+
+#include <string>
+#include <vector>
+#include <map>
+#include <tinyxml.h>
+#include <boost/shared_ptr.hpp>
+#include <boost/weak_ptr.hpp>
+
+#include "urdf_model/model.h"
+#include "urdf_model/pose.h"
+#include "urdf_model/twist.h"
+
+namespace urdf{
+
+class Entity
+{
+public:
+  boost::shared_ptr<ModelInterface> model;
+  Pose origin;
+  Twist twist;
+};
+
+class World
+{
+public:
+  World() { this->clear(); };
+
+  /// world name must be unique
+  std::string name;
+
+  std::vector<Entity> models;
+
+  void initXml(TiXmlElement* config);
+
+  void clear()
+  {
+    this->name.clear();
+  };
+};
+}
+
+#endif
+

build3/premake4.lua

@@ -79,9 +79,9 @@
 			}
 	end
 
-
-	flags { "NoRTTI", "NoExceptions"}
-	defines { "_HAS_EXCEPTIONS=0" }
+-- comment-out for now, URDF reader needs exceptions
+--	flags { "NoRTTI", "NoExceptions"}
+--	defines { "_HAS_EXCEPTIONS=0" }
 	targetdir "../bin"
 	location("./" .. act .. postfix)
 
@@ -117,6 +117,7 @@ if findOpenGL() then
 
 --	include "../Demos3/ImplicitCloth"
 	include "../Demos3/SimpleOpenGL3"
+	include "../btgui/urdf"
 
 	include "../btgui/lua-5.2.3"
 	include "../test/lua"