diff --git a/CMakeLists.txt b/CMakeLists.txt
index 698cdea..aedcb65 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,8 +2,8 @@ cmake_minimum_required( VERSION 2.8 FATAL_ERROR )
 project (urdfdom_headers)
 
 set (URDF_MAJOR_VERSION 0)
-set (URDF_MINOR_VERSION 1)
-set (URDF_PATCH_VERSION 4)
+set (URDF_MINOR_VERSION 2)
+set (URDF_PATCH_VERSION 0)
 
 set (URDF_VERSION ${URDF_MAJOR_VERSION}.${URDF_MINOR_VERSION}.${URDF_PATCH_VERSION})
 
@@ -18,6 +18,7 @@ endif (MSVC OR MSVC90 OR MSVC10)
 add_subdirectory(urdf_sensor)
 add_subdirectory(urdf_model)
 add_subdirectory(urdf_model_state)
+add_subdirectory(urdf_exception)
 
 set(PACKAGE_NAME ${PROJECT_NAME})
 set(cmake_conf_file "${CMAKE_CURRENT_SOURCE_DIR}/cmake/${PROJECT_NAME}-config.cmake")
diff --git a/urdf_exception/CMakeLists.txt b/urdf_exception/CMakeLists.txt
new file mode 100644
index 0000000..82caa8d
--- /dev/null
+++ b/urdf_exception/CMakeLists.txt
@@ -0,0 +1 @@
+INSTALL(DIRECTORY include/urdf_exception DESTINATION include)
diff --git a/urdf_exception/include/urdf_exception/exception.h b/urdf_exception/include/urdf_exception/exception.h
new file mode 100644
index 0000000..4a85482
--- /dev/null
+++ b/urdf_exception/include/urdf_exception/exception.h
@@ -0,0 +1,19 @@
+// 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
diff --git a/urdf_model/include/urdf_model/color.h b/urdf_model/include/urdf_model/color.h
index 9c2ab05..caba362 100644
--- a/urdf_model/include/urdf_model/color.h
+++ b/urdf_model/include/urdf_model/color.h
@@ -76,14 +76,13 @@ class Color
         }
         catch (boost::bad_lexical_cast &e)
         {
-          throw("color rgba element ("+pieces[i]+") is not a valid float");
+          return false;
         }
       }
     }
 
     if (rgba.size() != 4)
     {
-      //ROS_ERROR("Color contains %i elements instead of 4 elements", (int)rgba.size());
       return false;
     }
 
diff --git a/urdf_model/include/urdf_model/joint.h b/urdf_model/include/urdf_model/joint.h
index 3812c0e..c68a298 100644
--- a/urdf_model/include/urdf_model/joint.h
+++ b/urdf_model/include/urdf_model/joint.h
@@ -41,9 +41,8 @@
 #include <vector>
 #include <boost/shared_ptr.hpp>
 
-#include "pose.h"
+#include "urdf_model/pose.h"
 
-class TiXmlElement;
 
 namespace urdf{
 
@@ -61,7 +60,6 @@ class JointDynamics
     damping = 0;
     friction = 0;
   };
-  bool initXml(TiXmlElement* config);
 };
 
 class JointLimits
@@ -80,7 +78,6 @@ class JointLimits
     effort = 0;
     velocity = 0;
   };
-  bool initXml(TiXmlElement* config);
 };
 
 /// \brief Parameters for Joint Safety Controllers
@@ -133,7 +130,6 @@ class JointSafety
     k_position = 0;
     k_velocity = 0;
   };
-  bool initXml(TiXmlElement* config);
 };
 
 
@@ -148,7 +144,6 @@ class JointCalibration
   {
     reference_position = 0;
   };
-  bool initXml(TiXmlElement* config);
 };
 
 class JointMimic
@@ -165,7 +160,6 @@ class JointMimic
     multiplier = 0.0;
     joint_name.clear();
   };
-  bool initXml(TiXmlElement* config);
 };
 
 
@@ -216,7 +210,6 @@ class Joint
   /// Option to Mimic another Joint
   boost::shared_ptr<JointMimic> mimic;
 
-  bool initXml(TiXmlElement* xml);
   void clear()
   {
     this->axis.clear();
diff --git a/urdf_model/include/urdf_model/link.h b/urdf_model/include/urdf_model/link.h
index f4585e7..c8b6c16 100644
--- a/urdf_model/include/urdf_model/link.h
+++ b/urdf_model/include/urdf_model/link.h
@@ -46,8 +46,6 @@
 #include "joint.h"
 #include "color.h"
 
-class TiXmlElement;
-
 namespace urdf{
 
 class Geometry
@@ -55,8 +53,9 @@ class Geometry
 public:
   enum {SPHERE, BOX, CYLINDER, MESH} type;
 
-  virtual void initXml(TiXmlElement *) = 0;
-
+  virtual ~Geometry(void)
+  {
+  }  
 };
 
 class Sphere : public Geometry
@@ -69,7 +68,6 @@ class Sphere : public Geometry
   {
     radius = 0;
   };
-  void initXml(TiXmlElement *);
 };
 
 class Box : public Geometry
@@ -82,7 +80,6 @@ class Box : public Geometry
   {
     this->dim.clear();
   };
-  void initXml(TiXmlElement *);
 };
 
 class Cylinder : public Geometry
@@ -97,7 +94,6 @@ class Cylinder : public Geometry
     length = 0;
     radius = 0;
   };
-  void initXml(TiXmlElement *);
 };
 
 class Mesh : public Geometry
@@ -115,8 +111,6 @@ class Mesh : public Geometry
     scale.y = 1;
     scale.z = 1;
   };
-  void initXml(TiXmlElement *);
-  bool fileExists(std::string filename);
 };
 
 class Material
@@ -133,7 +127,6 @@ class Material
     texture_filename.clear();
     name.clear();
   };
-  void initXml(TiXmlElement* config);
 };
 
 class Inertial
@@ -150,7 +143,6 @@ class Inertial
     mass = 0;
     ixx = ixy = ixz = iyy = iyz = izz = 0;
   };
-  void initXml(TiXmlElement* config);
 };
 
 class Visual
@@ -171,7 +163,6 @@ class Visual
     geometry.reset();
     this->group_name.clear();
   };
-  void initXml(TiXmlElement* config);
   std::string group_name;
 };
 
@@ -188,7 +179,6 @@ class Collision
     geometry.reset();
     this->group_name.clear();
   };
-  void initXml(TiXmlElement* config);
   std::string group_name;
 };
 
@@ -223,13 +213,12 @@ class Link
   std::vector<boost::shared_ptr<Joint> > child_joints;
   std::vector<boost::shared_ptr<Link> > child_links;
 
-  void initXml(TiXmlElement* config);
-
   boost::shared_ptr<Link> getParent() const
   {return parent_link_.lock();};
 
-  void setParent(boost::shared_ptr<Link> parent);
-
+  void setParent(const boost::shared_ptr<Link> &parent)
+  { parent_link_ = parent; }
+  
   void clear()
   {
     this->name.clear();
@@ -241,14 +230,28 @@ class Link
     this->child_links.clear();
     this->collision_groups.clear();
   };
+
+  boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > getVisuals(const std::string& group_name) const
+  {
+    if (this->visual_groups.find(group_name) != this->visual_groups.end())
+      return this->visual_groups.at(group_name);
+    return boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > >();
+  }
+
+  boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > getCollisions(const std::string& group_name) const
+  {
+    if (this->collision_groups.find(group_name) != this->collision_groups.end())
+      return this->collision_groups.at(group_name);
+    return boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > >();
+  }
+  
+  /*
   void setParentJoint(boost::shared_ptr<Joint> child);
   void addChild(boost::shared_ptr<Link> child);
   void addChildJoint(boost::shared_ptr<Joint> child);
 
-  void addVisual(std::string group_name, boost::shared_ptr<Visual> visual);
-  boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > getVisuals(const std::string& group_name) const;
-  void addCollision(std::string group_name, boost::shared_ptr<Collision> collision);
-  boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > getCollisions(const std::string& group_name) const;
+
+  */
 private:
   boost::weak_ptr<Link> parent_link_;
 
diff --git a/urdf_model/include/urdf_model/model.h b/urdf_model/include/urdf_model/model.h
index 5a76c16..3a3aa7b 100644
--- a/urdf_model/include/urdf_model/model.h
+++ b/urdf_model/include/urdf_model/model.h
@@ -41,7 +41,7 @@
 #include <map>
 #include <boost/function.hpp>
 #include <urdf_model/link.h>
-
+#include <urdf_exception/exception.h>
 
 namespace urdf {
 
@@ -58,7 +58,7 @@ class ModelInterface
       ptr = this->links_.find(name)->second;
     return ptr;
   };
-
+  
   boost::shared_ptr<const Joint> getJoint(const std::string& name) const
   {
     boost::shared_ptr<const Joint> ptr;
@@ -68,8 +68,8 @@ class ModelInterface
       ptr = this->joints_.find(name)->second;
     return ptr;
   };
-
-
+  
+  
   const std::string& getName() const {return name_;};
   void getLinks(std::vector<boost::shared_ptr<Link> >& links) const
   {
@@ -78,7 +78,7 @@ class ModelInterface
       links.push_back(link->second);
     }
   };
-
+  
   void clear()
   {
     name_.clear();
@@ -87,7 +87,7 @@ class ModelInterface
     this->materials_.clear();
     this->root_link_.reset();
   };
-
+  
   /// non-const getLink()
   void getLink(const std::string& name,boost::shared_ptr<Link> &link) const
   {
@@ -98,7 +98,7 @@ class ModelInterface
       ptr = this->links_.find(name)->second;
     link = ptr;
   };
-
+  
   /// non-const getMaterial()
   boost::shared_ptr<Material> getMaterial(const std::string& name) const
   {
@@ -109,15 +109,81 @@ class ModelInterface
       ptr = this->materials_.find(name)->second;
     return ptr;
   };
-
-  /// in initXml(), onece all links are loaded,
-  /// it's time to build a tree
-  bool initTree(std::map<std::string, std::string> &parent_link_tree);
-
-  /// in initXml(), onece tree is built,
-  /// it's time to find the root Link
-  bool initRoot(std::map<std::string, std::string> &parent_link_tree);
-
+  
+  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,boost::shared_ptr<Joint> >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
+    {
+      std::string parent_link_name = joint->second->parent_link_name;
+      std::string child_link_name = joint->second->child_link_name;
+      
+      if (parent_link_name.empty() || child_link_name.empty())
+      {
+        throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification.");
+      }
+      else
+      {
+        // find child and parent links
+        boost::shared_ptr<Link> child_link, parent_link;
+        this->getLink(child_link_name, child_link);
+        if (!child_link)
+        {
+          throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found");
+        }
+        this->getLink(parent_link_name, parent_link);
+        if (!parent_link)
+        {
+          throw ParseError("parent link [" + parent_link_name + "] of joint [" + joint->first + "] not found.  This is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint [" + joint->first + "] from your urdf file, or add \"<link name=\"" + parent_link_name + "\" />\" to your urdf file.");
+        }
+        
+        //set parent link for child link
+        child_link->setParent(parent_link);
+
+        //set parent joint for child link        
+        child_link->parent_joint = joint->second;
+        
+        //set child joint for parent link
+        parent_link->child_joints.push_back(joint->second);
+
+        //set child link for parent link
+        parent_link->child_links.push_back(child_link);
+
+        // fill in child/parent string map
+        parent_link_tree[child_link->name] = parent_link_name;
+      }
+    }
+  }
+  
+  void initRoot(const std::map<std::string, std::string> &parent_link_tree)
+  { 
+    this->root_link_.reset();
+    
+    // find the links that have no parent in the tree
+    for (std::map<std::string, boost::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
+        {
+          throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]");
+        }
+      }
+    }
+    if (!this->root_link_)
+    {
+      throw ParseError("No root link found. The robot xml is not a valid tree.");
+    }
+  }
+  
+  
   /// \brief complete list of Links
   std::map<std::string, boost::shared_ptr<Link> > links_;
   /// \brief complete list of Joints
diff --git a/urdf_model/include/urdf_model/pose.h b/urdf_model/include/urdf_model/pose.h
index 57c25b7..a3be59b 100644
--- a/urdf_model/include/urdf_model/pose.h
+++ b/urdf_model/include/urdf_model/pose.h
@@ -43,8 +43,7 @@
 #include <math.h>
 #include <boost/algorithm/string.hpp>
 #include <boost/lexical_cast.hpp>
-
-class TiXmlElement;
+#include <urdf_exception/exception.h>
 
 namespace urdf{
 
@@ -58,7 +57,31 @@ class Vector3
   double z;
 
   void clear() {this->x=this->y=this->z=0.0;};
-  void init(const std::string &vector_str);
+  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) {
+          throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)");
+        }
+      }
+    }
+    
+    if (xyz.size() != 3)
+      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);
@@ -121,8 +144,14 @@ class Rotation
 
   double x,y,z,w;
 
-  void init(const std::string &rotation_str);
-
+  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()
@@ -212,7 +241,6 @@ class Pose
     this->position.clear();
     this->rotation.clear();
   };
-  void initXml(TiXmlElement* xml);
 };
 
 }
diff --git a/urdf_model_state/include/urdf_model_state/model_state.h b/urdf_model_state/include/urdf_model_state/model_state.h
index f03999f..a752075 100644
--- a/urdf_model_state/include/urdf_model_state/model_state.h
+++ b/urdf_model_state/include/urdf_model_state/model_state.h
@@ -34,19 +34,8 @@
 
 /* Author: John Hsu */
 
-/* encapsulates the states of a model, scene or link
-   see http://ros.org/wiki/urdf/XML/scene_state and
-       http://ros.org/wiki/urdf/XML/model_state
-   for details
-*/
-/* examples
-
-
-
-*/
-
-#ifndef URDF_STATE_H
-#define URDF_STATE_H
+#ifndef URDF_MODEL_STATE_H
+#define URDF_MODEL_STATE_H
 
 #include <string>
 #include <vector>
@@ -57,7 +46,6 @@
 #include "urdf_model/pose.h"
 #include <urdf_model_state/twist.h>
 
-class TiXmlElement;
 
 namespace urdf{
 
@@ -136,8 +124,6 @@ class ModelState
 
   Time time_stamp;
 
-  void initXml(TiXmlElement* config);
-
   void clear()
   {
     this->name.clear();
diff --git a/urdf_model_state/include/urdf_model_state/twist.h b/urdf_model_state/include/urdf_model_state/twist.h
index b44c280..ba57e4c 100644
--- a/urdf_model_state/include/urdf_model_state/twist.h
+++ b/urdf_model_state/include/urdf_model_state/twist.h
@@ -43,8 +43,6 @@
 #include <math.h>
 #include <urdf_model/pose.h>
 
-class TiXmlElement;
-
 namespace urdf{
 
 
@@ -62,7 +60,6 @@ class Twist
     this->linear.clear();
     this->angular.clear();
   };
-  void initXml(TiXmlElement* xml);
 };
 
 }
diff --git a/urdf_sensor/include/urdf_sensor/sensor.h b/urdf_sensor/include/urdf_sensor/sensor.h
index dbd2e76..3b99695 100644
--- a/urdf_sensor/include/urdf_sensor/sensor.h
+++ b/urdf_sensor/include/urdf_sensor/sensor.h
@@ -70,22 +70,22 @@
 #include "urdf_model/joint.h"
 #include "urdf_model/link.h"
 
-class TiXmlElement;
-
 namespace urdf{
 
 class VisualSensor
 {
 public:
   enum {CAMERA, RAY} type;
-  virtual void initXml(TiXmlElement *) = 0;
+  virtual ~VisualSensor(void)
+  {
+  }
 };
 
 class Camera : public VisualSensor
 {
 public:
   Camera() { this->clear(); };
-  double width, height;
+  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;
@@ -102,18 +102,17 @@ class Camera : public VisualSensor
     near = 0;
     far = 0;
   };
-  void initXml(TiXmlElement *);
 };
 
 class Ray : public VisualSensor
 {
 public:
   Ray() { this->clear(); };
-  double horizontal_samples;
+  unsigned int horizontal_samples;
   double horizontal_resolution;
   double horizontal_min_angle;
   double horizontal_max_angle;
-  double vertical_samples;
+  unsigned int vertical_samples;
   double vertical_resolution;
   double vertical_min_angle;
   double vertical_max_angle;
@@ -130,7 +129,6 @@ class Ray : public VisualSensor
     vertical_min_angle = 0;
     vertical_max_angle = 0;
   };
-  void initXml(TiXmlElement *);
 };
 
 
@@ -156,13 +154,12 @@ class Sensor
   /// Parent link element name.  A pointer is stored in parent_link_.
   std::string parent_link_name;
 
-  void initXml(TiXmlElement* config);
-
   boost::shared_ptr<Link> getParent() const
   {return parent_link_.lock();};
 
-  void setParent(boost::shared_ptr<Link> parent);
-
+  void setParent(boost::shared_ptr<Link> parent)
+  {  this->parent_link_ = parent; }
+  
   void clear()
   {
     this->name.clear();