Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

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

Browse Source 
(removed BOOST dependency and make it compile on Windows)
This commit is contained in:
Erwin Coumans
2014-07-31 13:38:31 -07:00
parent 4b8c8e7910
commit 04632538ec
40 changed files with 10434 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
btgui/urdf/urdfdom_headers/LICENSE Normal file
View File

@@ -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.

6
btgui/urdf/urdfdom_headers/README.txt Normal file
View File

@@ -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

53
btgui/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h Normal file
View File

@@ -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

102
btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h Normal file
View File

@@ -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

234
btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h Normal file
View File

@@ -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

258
btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h Normal file
View File

@@ -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

215
btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h Normal file
View File

@@ -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

265
btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h Normal file
View File

@@ -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

68
btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h Normal file
View File

@@ -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

141
btgui/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h Normal file
View File

@@ -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

42
btgui/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/twist.h Normal file
View File

@@ -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

176
btgui/urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h Normal file
View File

@@ -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

114
btgui/urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h Normal file
View File

@@ -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