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

third_party/kdl_parser/include/urdf_model/color.h

@@ -0,0 +1,105 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Josh Faust */
+
+#ifndef URDF_INTERFACE_COLOR_H
+#define URDF_INTERFACE_COLOR_H
+
+#include <stdexcept>
+#include <string>
+#include <vector>
+#include <math.h>
+
+#include <urdf_model/utils.h>
+#include <urdf_exception/exception.h>
+
+namespace urdf
+{
+
+class Color
+{
+public:
+  Color() {this->clear();};
+  float r;
+  float g;
+  float b;
+  float a;
+
+  void clear()
+  {
+    r = g = b = 0.0f;
+    a = 1.0f;
+  }
+  bool init(const std::string &vector_str)
+  {
+    this->clear();
+    std::vector<std::string> pieces;
+    std::vector<float> rgba;
+    urdf::split_string( pieces, vector_str, " ");
+    for (unsigned int i = 0; i < pieces.size(); ++i)
+    {
+      if (!pieces[i].empty())
+      {
+        try
+        {
+          double piece = strToDouble(pieces[i].c_str());
+          if ((piece < 0) || (piece > 1))
+            throw ParseError("Component [" + pieces[i] + "] is outside the valid range for colors [0, 1]");
+          rgba.push_back(static_cast<float>(piece));
+        }
+        catch (std::runtime_error &/*e*/) {
+          throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a color value)");
+        }
+      }
+    }
+
+    if (rgba.size() != 4)
+    {
+      return false;
+    }
+
+    this->r = rgba[0];
+    this->g = rgba[1];
+    this->b = rgba[2];
+    this->a = rgba[3];
+
+    return true;
+  };
+};
+
+}
+
+#endif
+

third_party/kdl_parser/include/urdf_model/joint.h

@@ -0,0 +1,230 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+#ifndef URDF_INTERFACE_JOINT_H
+#define URDF_INTERFACE_JOINT_H
+
+#include <string>
+#include <vector>
+
+#include "urdf_model/pose.h"
+#include "urdf_model/types.h"
+
+
+namespace urdf{
+
+class Link;
+
+class JointDynamics
+{
+public:
+  JointDynamics() { this->clear(); };
+  double damping;
+  double friction;
+
+  void clear()
+  {
+    damping = 0;
+    friction = 0;
+  };
+};
+
+class JointLimits
+{
+public:
+  JointLimits() { this->clear(); };
+  double lower;
+  double upper;
+  double effort;
+  double velocity;
+
+  void clear()
+  {
+    lower = 0;
+    upper = 0;
+    effort = 0;
+    velocity = 0;
+  };
+};
+
+/// \brief Parameters for Joint Safety Controllers
+class JointSafety
+{
+public:
+  /// clear variables on construction
+  JointSafety() { this->clear(); };
+
+  /// 
+  /// IMPORTANT:  The safety controller support is very much PR2 specific, not intended for generic usage.
+  /// 
+  /// Basic safety controller operation is as follows
+  /// 
+  /// current safety controllers will take effect on joints outside the position range below:
+  ///
+  /// position range: [JointSafety::soft_lower_limit  + JointLimits::velocity / JointSafety::k_position, 
+  ///                  JointSafety::soft_uppper_limit - JointLimits::velocity / JointSafety::k_position]
+  ///
+  /// if (joint_position is outside of the position range above)
+  ///     velocity_limit_min = -JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_lower_limit)
+  ///     velocity_limit_max =  JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_upper_limit)
+  /// else
+  ///     velocity_limit_min = -JointLimits::velocity
+  ///     velocity_limit_max =  JointLimits::velocity
+  ///
+  /// velocity range: [velocity_limit_min + JointLimits::effort / JointSafety::k_velocity,
+  ///                  velocity_limit_max - JointLimits::effort / JointSafety::k_velocity]
+  ///
+  /// if (joint_velocity is outside of the velocity range above)
+  ///     effort_limit_min = -JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_min)
+  ///     effort_limit_max =  JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_max)
+  /// else
+  ///     effort_limit_min = -JointLimits::effort
+  ///     effort_limit_max =  JointLimits::effort
+  ///
+  /// Final effort command sent to the joint is saturated by [effort_limit_min,effort_limit_max]
+  ///
+  /// Please see wiki for more details: http://www.ros.org/wiki/pr2_controller_manager/safety_limits
+  /// 
+  double soft_upper_limit;
+  double soft_lower_limit;
+  double k_position;
+  double k_velocity;
+
+  void clear()
+  {
+    soft_upper_limit = 0;
+    soft_lower_limit = 0;
+    k_position = 0;
+    k_velocity = 0;
+  };
+};
+
+
+class JointCalibration
+{
+public:
+  JointCalibration() { this->clear(); };
+  double reference_position;
+  DoubleSharedPtr rising, falling;
+
+  void clear()
+  {
+    reference_position = 0;
+  };
+};
+
+class JointMimic
+{
+public:
+  JointMimic() { this->clear(); };
+  double offset;
+  double multiplier;
+  std::string joint_name;
+
+  void clear()
+  {
+    offset = 0.0;
+    multiplier = 0.0;
+    joint_name.clear();
+  };
+};
+
+
+class Joint
+{
+public:
+
+  Joint() { this->clear(); };
+
+  std::string name;
+  enum
+  {
+    UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED
+  } type;
+
+  /// \brief     type_       meaning of axis_
+  /// ------------------------------------------------------
+  ///            UNKNOWN     unknown type
+  ///            REVOLUTE    rotation axis
+  ///            PRISMATIC   translation axis
+  ///            FLOATING    N/A
+  ///            PLANAR      plane normal axis
+  ///            FIXED       N/A
+  Vector3 axis;
+
+  /// child Link element
+  ///   child link frame is the same as the Joint frame
+  std::string child_link_name;
+
+  /// parent Link element
+  ///   origin specifies the transform from Parent Link to Joint Frame
+  std::string parent_link_name;
+  /// transform from Parent Link frame to Joint frame
+  Pose  parent_to_joint_origin_transform;
+
+  /// Joint Dynamics
+  JointDynamicsSharedPtr dynamics;
+
+  /// Joint Limits
+  JointLimitsSharedPtr limits;
+
+  /// Unsupported Hidden Feature
+  JointSafetySharedPtr safety;
+
+  /// Unsupported Hidden Feature
+  JointCalibrationSharedPtr calibration;
+
+  /// Option to Mimic another Joint
+  JointMimicSharedPtr mimic;
+
+  void clear()
+  {
+    this->axis.clear();
+    this->child_link_name.clear();
+    this->parent_link_name.clear();
+    this->parent_to_joint_origin_transform.clear();
+    this->dynamics.reset();
+    this->limits.reset();
+    this->safety.reset();
+    this->calibration.reset();
+    this->mimic.reset();
+    this->type = UNKNOWN;
+  };
+};
+
+}
+
+#endif

third_party/kdl_parser/include/urdf_model/link.h

@@ -0,0 +1,247 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+#ifndef URDF_INTERFACE_LINK_H
+#define URDF_INTERFACE_LINK_H
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include "joint.h"
+#include "color.h"
+#include "types.h"
+
+namespace urdf{
+
+class Geometry
+{
+public:
+  enum {SPHERE, BOX, CYLINDER, MESH} type;
+
+  virtual ~Geometry(void)
+  {
+  }  
+};
+
+class Sphere : public Geometry
+{
+public:
+  Sphere() { this->clear(); type = SPHERE; };
+  double radius;
+
+  void clear()
+  {
+    radius = 0;
+  };
+};
+
+class Box : public Geometry
+{
+public:
+  Box() { this->clear(); type = BOX; };
+  Vector3 dim;
+
+  void clear()
+  {
+    this->dim.clear();
+  };
+};
+
+class Cylinder : public Geometry
+{
+public:
+  Cylinder() { this->clear(); type = CYLINDER; };
+  double length;
+  double radius;
+
+  void clear()
+  {
+    length = 0;
+    radius = 0;
+  };
+};
+
+class Mesh : public Geometry
+{
+public:
+  Mesh() { this->clear(); type = MESH; };
+  std::string filename;
+  Vector3 scale;
+
+  void clear()
+  {
+    filename.clear();
+    // default scale
+    scale.x = 1;
+    scale.y = 1;
+    scale.z = 1;
+  };
+};
+
+class Material
+{
+public:
+  Material() { this->clear(); };
+  std::string name;
+  std::string texture_filename;
+  Color color;
+
+  void clear()
+  {
+    color.clear();
+    texture_filename.clear();
+    name.clear();
+  };
+};
+
+class Inertial
+{
+public:
+  Inertial() { this->clear(); };
+  Pose origin;
+  double mass;
+  double ixx,ixy,ixz,iyy,iyz,izz;
+
+  void clear()
+  {
+    origin.clear();
+    mass = 0;
+    ixx = ixy = ixz = iyy = iyz = izz = 0;
+  };
+};
+
+class Visual
+{
+public:
+  Visual() { this->clear(); };
+  Pose origin;
+  GeometrySharedPtr geometry;
+
+  std::string material_name;
+  MaterialSharedPtr material;
+
+  void clear()
+  {
+    origin.clear();
+    material_name.clear();
+    material.reset();
+    geometry.reset();
+    name.clear();
+  };
+
+  std::string name;
+};
+
+class Collision
+{
+public:
+  Collision() { this->clear(); };
+  Pose origin;
+  GeometrySharedPtr geometry;
+
+  void clear()
+  {
+    origin.clear();
+    geometry.reset();
+    name.clear();
+  };
+
+  std::string name;
+
+};
+
+
+class Link
+{
+public:
+  Link() { this->clear(); };
+
+  std::string name;
+
+  /// inertial element
+  InertialSharedPtr inertial;
+
+  /// visual element
+  VisualSharedPtr visual;
+
+  /// collision element
+  CollisionSharedPtr collision;
+
+  /// if more than one collision element is specified, all collision elements are placed in this array (the collision member points to the first element of the array)
+  std::vector<CollisionSharedPtr> collision_array;
+
+  /// if more than one visual element is specified, all visual elements are placed in this array (the visual member points to the first element of the array)
+  std::vector<VisualSharedPtr> visual_array;
+
+  /// Parent Joint element
+  ///   explicitly stating "parent" because we want directional-ness for tree structure
+  ///   every link can have one parent
+  JointSharedPtr parent_joint;
+
+  std::vector<JointSharedPtr> child_joints;
+  std::vector<LinkSharedPtr> child_links;
+
+  LinkSharedPtr getParent() const
+  {return parent_link_.lock();};
+
+  void setParent(const LinkSharedPtr &parent)
+  { parent_link_ = parent; }
+  
+  void clear()
+  {
+    this->name.clear();
+    this->inertial.reset();
+    this->visual.reset();
+    this->collision.reset();
+    this->parent_joint.reset();
+    this->child_joints.clear();
+    this->child_links.clear();
+    this->collision_array.clear();
+    this->visual_array.clear();
+  };
+
+private:
+  LinkWeakPtr parent_link_;
+
+};
+
+
+
+
+}
+
+#endif

third_party/kdl_parser/include/urdf_model/model.h

@@ -0,0 +1,206 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+#ifndef URDF_INTERFACE_MODEL_H
+#define URDF_INTERFACE_MODEL_H
+
+#include <string>
+#include <map>
+#include <urdf_model/link.h>
+#include <urdf_model/types.h>
+#include <urdf_exception/exception.h>
+
+namespace urdf {
+
+class ModelInterface
+{
+public:
+  LinkConstSharedPtr getRoot(void) const{return this->root_link_;};
+  LinkConstSharedPtr getLink(const std::string& name) const
+  {
+    LinkConstSharedPtr ptr;
+    if (this->links_.find(name) == this->links_.end())
+      ptr.reset();
+    else
+      ptr = this->links_.find(name)->second;
+    return ptr;
+  };
+  
+  JointConstSharedPtr getJoint(const std::string& name) const
+  {
+    JointConstSharedPtr ptr;
+    if (this->joints_.find(name) == this->joints_.end())
+      ptr.reset();
+    else
+      ptr = this->joints_.find(name)->second;
+    return ptr;
+  };
+  
+  
+  const std::string& getName() const {return name_;};
+  void getLinks(std::vector<LinkSharedPtr >& links) const
+  {
+    for (std::map<std::string,LinkSharedPtr>::const_iterator link = this->links_.begin();link != this->links_.end(); link++)
+    {
+      links.push_back(link->second);
+    }
+  };
+  
+  void clear()
+  {
+    name_.clear();
+    this->links_.clear();
+    this->joints_.clear();
+    this->materials_.clear();
+    this->root_link_.reset();
+  };
+  
+  /// non-const getLink()
+  void getLink(const std::string& name, LinkSharedPtr &link) const
+  {
+    LinkSharedPtr ptr;
+    if (this->links_.find(name) == this->links_.end())
+      ptr.reset();
+    else
+      ptr = this->links_.find(name)->second;
+    link = ptr;
+  };
+  
+  /// non-const getMaterial()
+  MaterialSharedPtr getMaterial(const std::string& name) const
+  {
+    MaterialSharedPtr ptr;
+    if (this->materials_.find(name) == this->materials_.end())
+      ptr.reset();
+    else
+      ptr = this->materials_.find(name)->second;
+    return ptr;
+  };
+  
+  void initTree(std::map<std::string, std::string> &parent_link_tree)
+  {
+    // loop through all joints, for every link, assign children links and children joints
+    for (std::map<std::string, JointSharedPtr>::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
+    {
+      std::string parent_link_name = joint->second->parent_link_name;
+      std::string child_link_name = joint->second->child_link_name;
+      
+      if (parent_link_name.empty() || child_link_name.empty())
+      {
+        throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification.");
+      }
+      else
+      {
+        // find child and parent links
+        LinkSharedPtr child_link, parent_link;
+        this->getLink(child_link_name, child_link);
+        if (!child_link)
+        {
+          throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found");
+        }
+        this->getLink(parent_link_name, parent_link);
+        if (!parent_link)
+        {
+          throw ParseError("parent link [" + parent_link_name + "] of joint [" + joint->first + "] not found.  This is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint [" + joint->first + "] from your urdf file, or add \"<link name=\"" + parent_link_name + "\" />\" to your urdf file.");
+        }
+        
+        //set parent link for child link
+        child_link->setParent(parent_link);
+
+        //set parent joint for child link        
+        child_link->parent_joint = joint->second;
+        
+        //set child joint for parent link
+        parent_link->child_joints.push_back(joint->second);
+
+        //set child link for parent link
+        parent_link->child_links.push_back(child_link);
+
+        // fill in child/parent string map
+        parent_link_tree[child_link->name] = parent_link_name;
+      }
+    }
+  }
+  
+  void initRoot(const std::map<std::string, std::string> &parent_link_tree)
+  { 
+    this->root_link_.reset();
+    
+    // find the links that have no parent in the tree
+    for (std::map<std::string, LinkSharedPtr>::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++)
+    {
+      std::map<std::string, std::string >::const_iterator parent = parent_link_tree.find(l->first);
+      if (parent == parent_link_tree.end())
+      {
+        // store root link
+        if (!this->root_link_)
+        {
+          getLink(l->first, this->root_link_);
+        }
+        // we already found a root link
+        else
+        {
+          throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]");
+        }
+      }
+    }
+    if (!this->root_link_)
+    {
+      throw ParseError("No root link found. The robot xml is not a valid tree.");
+    }
+  }
+  
+  
+  /// \brief complete list of Links
+  std::map<std::string, LinkSharedPtr> links_;
+  /// \brief complete list of Joints
+  std::map<std::string, JointSharedPtr> joints_;
+  /// \brief complete list of Materials
+  std::map<std::string, MaterialSharedPtr> materials_;
+
+  /// \brief The name of the robot model
+  std::string name_;
+
+  /// \brief The root is always a link (the parent of the tree describing the robot)
+  LinkSharedPtr root_link_;
+
+
+
+};
+
+}
+
+#endif

third_party/kdl_parser/include/urdf_model/pose.h

@@ -0,0 +1,260 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+#ifndef URDF_INTERFACE_POSE_H
+#define URDF_INTERFACE_POSE_H
+
+#include <cmath>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include <urdf_exception/exception.h>
+#include <urdf_model/utils.h>
+
+namespace urdf{
+
+class Vector3
+{
+public:
+  Vector3(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;};
+  Vector3() {this->clear();};
+  double x;
+  double y;
+  double z;
+
+  void clear() {this->x=this->y=this->z=0.0;};
+  void init(const std::string &vector_str)
+  {
+    this->clear();
+    std::vector<std::string> pieces;
+    std::vector<double> xyz;
+    urdf::split_string( pieces, vector_str, " ");
+    for (unsigned int i = 0; i < pieces.size(); ++i){
+      if (pieces[i] != ""){
+        try {
+          xyz.push_back(strToDouble(pieces[i].c_str()));
+        } catch(std::runtime_error &) {
+          throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)");
+        }
+      }
+    }
+
+    if (xyz.size() != 3)
+      throw ParseError("Parser found " + std::to_string(xyz.size())  + " elements but 3 expected while parsing vector [" + vector_str + "]");
+
+    this->x = xyz[0];
+    this->y = xyz[1];
+    this->z = xyz[2];
+  }
+
+  Vector3 operator+(Vector3 vec)
+  {
+    return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z);
+  };
+};
+
+class Rotation
+{
+public:
+  Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;};
+  Rotation() {this->clear();};
+  void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const
+  {
+    quat_x = this->x;
+    quat_y = this->y;
+    quat_z = this->z;
+    quat_w = this->w;
+  };
+  void getRPY(double &roll,double &pitch,double &yaw) const
+  {
+    double sqw;
+    double sqx;
+    double sqy;
+    double sqz;
+
+    sqx = this->x * this->x;
+    sqy = this->y * this->y;
+    sqz = this->z * this->z;
+    sqw = this->w * this->w;
+
+    // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
+    double sarg = -2 * (this->x*this->z - this->w*this->y);
+    const double pi_2 = 1.57079632679489661923;
+    if (sarg <= -0.99999) {
+      pitch = -pi_2;
+      roll  = 0;
+      yaw   = 2 * atan2(this->x, -this->y);
+    } else if (sarg >= 0.99999) {
+      pitch = pi_2;
+      roll  = 0;
+      yaw   = 2 * atan2(-this->x, this->y);
+    } else {
+      pitch = asin(sarg);
+      roll  = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz);
+      yaw   = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz);
+    }
+
+  };
+  void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w)
+  {
+    this->x = quat_x;
+    this->y = quat_y;
+    this->z = quat_z;
+    this->w = quat_w;
+    this->normalize();
+  };
+  void setFromRPY(double roll, double pitch, double yaw)
+  {
+    double phi, the, psi;
+
+    phi = roll / 2.0;
+    the = pitch / 2.0;
+    psi = yaw / 2.0;
+
+    this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi);
+    this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi);
+    this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi);
+    this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi);
+
+    this->normalize();
+  };
+
+  double x,y,z,w;
+
+  void init(const std::string &rotation_str)
+  {
+    this->clear();
+    Vector3 rpy;
+    rpy.init(rotation_str);
+    setFromRPY(rpy.x, rpy.y, rpy.z);
+  }
+
+  void clear() { this->x=this->y=this->z=0.0;this->w=1.0; }
+
+  void normalize()
+  {
+    double s = sqrt(this->x * this->x +
+                    this->y * this->y +
+                    this->z * this->z +
+                    this->w * this->w);
+    if (s == 0.0)
+    {
+      this->x = 0.0;
+      this->y = 0.0;
+      this->z = 0.0;
+      this->w = 1.0;
+    }
+    else
+    {
+      this->x /= s;
+      this->y /= s;
+      this->z /= s;
+      this->w /= s;
+    }
+  };
+
+  // Multiplication operator (copied from gazebo)
+  Rotation operator*( const Rotation &qt ) const
+  {
+    Rotation c;
+
+    c.x = this->w * qt.x + this->x * qt.w + this->y * qt.z - this->z * qt.y;
+    c.y = this->w * qt.y - this->x * qt.z + this->y * qt.w + this->z * qt.x;
+    c.z = this->w * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.w;
+    c.w = this->w * qt.w - this->x * qt.x - this->y * qt.y - this->z * qt.z;
+
+    return c;
+  };
+  /// Rotate a vector using the quaternion
+  Vector3 operator*(Vector3 vec) const
+  {
+    Rotation tmp;
+    Vector3 result;
+
+    tmp.w = 0.0;
+    tmp.x = vec.x;
+    tmp.y = vec.y;
+    tmp.z = vec.z;
+
+    tmp = (*this) * (tmp * this->GetInverse());
+
+    result.x = tmp.x;
+    result.y = tmp.y;
+    result.z = tmp.z;
+
+    return result;
+  };
+  // Get the inverse of this quaternion
+  Rotation GetInverse() const
+  {
+    Rotation q;
+
+    double norm = this->w*this->w+this->x*this->x+this->y*this->y+this->z*this->z;
+
+    if (norm > 0.0)
+    {
+      q.w = this->w / norm;
+      q.x = -this->x / norm;
+      q.y = -this->y / norm;
+      q.z = -this->z / norm;
+    }
+
+    return q;
+  };
+
+
+};
+
+class Pose
+{
+public:
+  Pose() { this->clear(); };
+
+  Vector3  position;
+  Rotation rotation;
+
+  void clear()
+  {
+    this->position.clear();
+    this->rotation.clear();
+  };
+};
+
+}
+
+#endif

third_party/kdl_parser/include/urdf_model/twist.h

@@ -0,0 +1,68 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: John Hsu */
+
+#ifndef URDF_TWIST_H
+#define URDF_TWIST_H
+
+#include <string>
+#include <sstream>
+#include <vector>
+#include <math.h>
+#include <urdf_model/pose.h>
+
+namespace urdf{
+
+
+class Twist
+{
+public:
+  Twist() { this->clear(); };
+
+  Vector3  linear;
+  // Angular velocity represented by Euler angles
+  Vector3  angular;
+
+  void clear()
+  {
+    this->linear.clear();
+    this->angular.clear();
+  };
+};
+
+}
+
+#endif
+

third_party/kdl_parser/include/urdf_model/types.h

@@ -0,0 +1,91 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Steve Peters */
+
+#ifndef URDF_MODEL_TYPES_H
+#define URDF_MODEL_TYPES_H
+
+#include <memory>
+
+#define URDF_TYPEDEF_CLASS_POINTER(Class) \
+class Class; \
+typedef std::shared_ptr<Class> Class##SharedPtr; \
+typedef std::shared_ptr<const Class> Class##ConstSharedPtr; \
+typedef std::weak_ptr<Class> Class##WeakPtr
+
+namespace urdf{
+
+// shared pointer used in joint.h
+typedef std::shared_ptr<double> DoubleSharedPtr;
+
+URDF_TYPEDEF_CLASS_POINTER(Box);
+URDF_TYPEDEF_CLASS_POINTER(Collision);
+URDF_TYPEDEF_CLASS_POINTER(Cylinder);
+URDF_TYPEDEF_CLASS_POINTER(Geometry);
+URDF_TYPEDEF_CLASS_POINTER(Inertial);
+URDF_TYPEDEF_CLASS_POINTER(Joint);
+URDF_TYPEDEF_CLASS_POINTER(JointCalibration);
+URDF_TYPEDEF_CLASS_POINTER(JointDynamics);
+URDF_TYPEDEF_CLASS_POINTER(JointLimits);
+URDF_TYPEDEF_CLASS_POINTER(JointMimic);
+URDF_TYPEDEF_CLASS_POINTER(JointSafety);
+URDF_TYPEDEF_CLASS_POINTER(Link);
+URDF_TYPEDEF_CLASS_POINTER(Material);
+URDF_TYPEDEF_CLASS_POINTER(Mesh);
+URDF_TYPEDEF_CLASS_POINTER(Sphere);
+URDF_TYPEDEF_CLASS_POINTER(Visual);
+
+// create *_pointer_cast functions in urdf namespace
+template<class T, class U>
+std::shared_ptr<T> const_pointer_cast(std::shared_ptr<U> const & r)
+{
+  return std::const_pointer_cast<T>(r);
+}
+
+template<class T, class U>
+std::shared_ptr<T> dynamic_pointer_cast(std::shared_ptr<U> const & r)
+{
+  return std::dynamic_pointer_cast<T>(r);
+}
+
+template<class T, class U>
+std::shared_ptr<T> static_pointer_cast(std::shared_ptr<U> const & r)
+{
+  return std::static_pointer_cast<T>(r);
+}
+
+}
+
+#endif

third_party/kdl_parser/include/urdf_model/utils.h

@@ -0,0 +1,92 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2016, Open Source Robotics Foundation (OSRF)
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the OSRF nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Steve Peters */
+
+#ifndef URDF_INTERFACE_UTILS_H
+#define URDF_INTERFACE_UTILS_H
+
+#include <locale>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+namespace urdf {
+
+// Replacement for boost::split( ... , ... , boost::is_any_of(" "))
+inline
+void split_string(std::vector<std::string> &result,
+                  const std::string &input,
+                  const std::string &isAnyOf)
+{
+  std::string::size_type start = 0;
+  std::string::size_type end = input.find_first_of(isAnyOf, start);
+  while (end != std::string::npos)
+  {
+    result.push_back(input.substr(start, end-start));
+    start = end + 1;
+    end = input.find_first_of(isAnyOf, start);
+  }
+  if (start < input.length())
+  {
+    result.push_back(input.substr(start));
+  }
+}
+
+// This is a locale-safe version of string-to-double, which is suprisingly
+// difficult to do correctly.  This function ensures that the C locale is used
+// for parsing, as that matches up with what the XSD for double specifies.
+// On success, the double is returned; on failure, a std::runtime_error is
+// thrown.
+static inline double strToDouble(const char *in)
+{
+  std::stringstream ss;
+  ss.imbue(std::locale::classic());
+
+  ss << in;
+
+  double out;
+  ss >> out;
+
+  if (ss.fail() || !ss.eof()) {
+    throw std::runtime_error("Failed converting string to double");
+  }
+
+  return out;
+}
+
+}
+
+#endif