splitting off urdf_interface
This commit is contained in:
Wim Meeussen
parent
1be003b8d1
commit
818b5f6788
|
|
@ -0,0 +1,39 @@
|
|||
cmake_minimum_required(VERSION 2.4.6)
|
||||
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
|
||||
|
||||
# Set the build type. Options are:
|
||||
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
|
||||
# Debug : w/ debug symbols, w/o optimization
|
||||
# Release : w/o debug symbols, w/ optimization
|
||||
# RelWithDebInfo : w/ debug symbols, w/ optimization
|
||||
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
|
||||
set(ROS_BUILD_TYPE Debug)
|
||||
|
||||
rosbuild_init()
|
||||
|
||||
#set the default path for built executables to the "bin" directory
|
||||
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
|
||||
#set the default path for built libraries to the "lib" directory
|
||||
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
|
||||
|
||||
#uncomment if you have defined messages
|
||||
#rosbuild_genmsg()
|
||||
|
||||
rosbuild_gensrv()
|
||||
|
||||
# necessary for collada reader to create the temporary dae files due to limitations in the URDF geometry
|
||||
check_function_exists(mkstemps HAVE_MKSTEMPS)
|
||||
if( HAVE_MKSTEMPS )
|
||||
add_definitions("-DHAVE_MKSTEMPS")
|
||||
endif()
|
||||
|
||||
#common commands for building c++ executables and libraries
|
||||
rosbuild_add_library(${PROJECT_NAME} src/model.cpp src/collada_model_reader.cpp)
|
||||
#target_link_libraries(${PROJECT_NAME} another_library)
|
||||
rosbuild_add_boost_directories()
|
||||
#rosbuild_link_boost(${PROJECT_NAME} thread)
|
||||
|
||||
rosbuild_add_executable(test_parser EXCLUDE_FROM_ALL test/test_robot_model_parser.cpp)
|
||||
rosbuild_add_gtest_build_flags(test_parser)
|
||||
target_link_libraries(test_parser ${PROJECT_NAME})
|
||||
rosbuild_add_rostest(${PROJECT_SOURCE_DIR}/test/test_robot_model_parser.launch)
|
||||
|
|
@ -0,0 +1 @@
|
|||
include $(shell rospack find mk)/cmake.mk
|
||||
|
|
@ -0,0 +1,104 @@
|
|||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Willow Garage nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/* Author: Josh Faust */
|
||||
|
||||
#ifndef URDF_COLOR_H
|
||||
#define URDF_COLOR_H
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <math.h>
|
||||
#include <ros/ros.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;
|
||||
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)
|
||||
{
|
||||
ROS_ERROR("color rgba element (%s) is not a valid float",pieces[i].c_str());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (rgba.size() != 4)
|
||||
{
|
||||
ROS_ERROR("Color contains %i elements instead of 4 elements", (int)rgba.size());
|
||||
return false;
|
||||
}
|
||||
|
||||
this->r = rgba[0];
|
||||
this->g = rgba[1];
|
||||
this->b = rgba[2];
|
||||
this->a = rgba[3];
|
||||
|
||||
return true;
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -0,0 +1,235 @@
|
|||
/*********************************************************************
|
||||
* 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_JOINT_H
|
||||
#define URDF_JOINT_H
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <tinyxml/tinyxml.h>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include "pose.h"
|
||||
|
||||
namespace urdf{
|
||||
|
||||
class Link;
|
||||
|
||||
class JointDynamics
|
||||
{
|
||||
public:
|
||||
JointDynamics() { this->clear(); };
|
||||
double damping;
|
||||
double friction;
|
||||
|
||||
void clear()
|
||||
{
|
||||
damping = 0;
|
||||
friction = 0;
|
||||
};
|
||||
bool initXml(TiXmlElement* config);
|
||||
};
|
||||
|
||||
class JointLimits
|
||||
{
|
||||
public:
|
||||
JointLimits() { this->clear(); };
|
||||
double lower;
|
||||
double upper;
|
||||
double effort;
|
||||
double velocity;
|
||||
|
||||
void clear()
|
||||
{
|
||||
lower = 0;
|
||||
upper = 0;
|
||||
effort = 0;
|
||||
velocity = 0;
|
||||
};
|
||||
bool initXml(TiXmlElement* config);
|
||||
};
|
||||
|
||||
/// \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;
|
||||
};
|
||||
bool initXml(TiXmlElement* config);
|
||||
};
|
||||
|
||||
|
||||
class JointCalibration
|
||||
{
|
||||
public:
|
||||
JointCalibration() { this->clear(); };
|
||||
double reference_position;
|
||||
boost::shared_ptr<double> rising, falling;
|
||||
|
||||
void clear()
|
||||
{
|
||||
reference_position = 0;
|
||||
};
|
||||
bool initXml(TiXmlElement* config);
|
||||
};
|
||||
|
||||
class JointMimic
|
||||
{
|
||||
public:
|
||||
JointMimic() { this->clear(); };
|
||||
double offset;
|
||||
double multiplier;
|
||||
std::string joint_name;
|
||||
|
||||
void clear()
|
||||
{
|
||||
offset = 0;
|
||||
multiplier = 0;
|
||||
joint_name.clear();
|
||||
};
|
||||
bool initXml(TiXmlElement* config);
|
||||
};
|
||||
|
||||
|
||||
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
|
||||
boost::shared_ptr<JointDynamics> dynamics;
|
||||
|
||||
/// Joint Limits
|
||||
boost::shared_ptr<JointLimits> limits;
|
||||
|
||||
/// Unsupported Hidden Feature
|
||||
boost::shared_ptr<JointSafety> safety;
|
||||
|
||||
/// Unsupported Hidden Feature
|
||||
boost::shared_ptr<JointCalibration> calibration;
|
||||
|
||||
/// Option to Mimic another Joint
|
||||
boost::shared_ptr<JointMimic> mimic;
|
||||
|
||||
bool initXml(TiXmlElement* xml);
|
||||
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->type = UNKNOWN;
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,259 @@
|
|||
/*********************************************************************
|
||||
* 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_LINK_H
|
||||
#define URDF_LINK_H
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <tinyxml/tinyxml.h>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include "joint.h"
|
||||
#include "color.h"
|
||||
|
||||
namespace urdf{
|
||||
|
||||
class Geometry
|
||||
{
|
||||
public:
|
||||
enum {SPHERE, BOX, CYLINDER, MESH} type;
|
||||
|
||||
virtual bool initXml(TiXmlElement *) = 0;
|
||||
|
||||
};
|
||||
|
||||
class Sphere : public Geometry
|
||||
{
|
||||
public:
|
||||
Sphere() { this->clear(); };
|
||||
double radius;
|
||||
|
||||
void clear()
|
||||
{
|
||||
radius = 0;
|
||||
};
|
||||
bool initXml(TiXmlElement *);
|
||||
};
|
||||
|
||||
class Box : public Geometry
|
||||
{
|
||||
public:
|
||||
Box() { this->clear(); };
|
||||
Vector3 dim;
|
||||
|
||||
void clear()
|
||||
{
|
||||
dim.clear();
|
||||
};
|
||||
bool initXml(TiXmlElement *);
|
||||
};
|
||||
|
||||
class Cylinder : public Geometry
|
||||
{
|
||||
public:
|
||||
Cylinder() { this->clear(); };
|
||||
double length;
|
||||
double radius;
|
||||
|
||||
void clear()
|
||||
{
|
||||
length = 0;
|
||||
radius = 0;
|
||||
};
|
||||
bool initXml(TiXmlElement *);
|
||||
};
|
||||
|
||||
class Mesh : public Geometry
|
||||
{
|
||||
public:
|
||||
Mesh() { this->clear(); };
|
||||
std::string filename;
|
||||
Vector3 scale;
|
||||
|
||||
void clear()
|
||||
{
|
||||
filename.clear();
|
||||
// default scale
|
||||
scale.x = 1;
|
||||
scale.y = 1;
|
||||
scale.z = 1;
|
||||
};
|
||||
bool initXml(TiXmlElement *);
|
||||
bool fileExists(std::string filename);
|
||||
};
|
||||
|
||||
class Material
|
||||
{
|
||||
public:
|
||||
Material() { this->clear(); };
|
||||
std::string name;
|
||||
std::string texture_filename;
|
||||
Color color;
|
||||
|
||||
void clear()
|
||||
{
|
||||
color.clear();
|
||||
texture_filename.clear();
|
||||
name.clear();
|
||||
};
|
||||
bool initXml(TiXmlElement* config);
|
||||
};
|
||||
|
||||
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;
|
||||
};
|
||||
bool initXml(TiXmlElement* config);
|
||||
};
|
||||
|
||||
class Visual
|
||||
{
|
||||
public:
|
||||
Visual() { this->clear(); };
|
||||
Pose origin;
|
||||
boost::shared_ptr<Geometry> geometry;
|
||||
|
||||
std::string material_name;
|
||||
boost::shared_ptr<Material> material;
|
||||
|
||||
void clear()
|
||||
{
|
||||
origin.clear();
|
||||
material_name.clear();
|
||||
material.reset();
|
||||
geometry.reset();
|
||||
this->group_name.clear();
|
||||
};
|
||||
bool initXml(TiXmlElement* config);
|
||||
std::string group_name;
|
||||
};
|
||||
|
||||
class Collision
|
||||
{
|
||||
public:
|
||||
Collision() { this->clear(); };
|
||||
Pose origin;
|
||||
boost::shared_ptr<Geometry> geometry;
|
||||
|
||||
void clear()
|
||||
{
|
||||
origin.clear();
|
||||
geometry.reset();
|
||||
this->group_name.clear();
|
||||
};
|
||||
bool initXml(TiXmlElement* config);
|
||||
std::string group_name;
|
||||
};
|
||||
|
||||
|
||||
class Link
|
||||
{
|
||||
public:
|
||||
Link() { this->clear(); };
|
||||
|
||||
std::string name;
|
||||
|
||||
/// inertial element
|
||||
boost::shared_ptr<Inertial> inertial;
|
||||
|
||||
/// visual element
|
||||
boost::shared_ptr<Visual> visual;
|
||||
|
||||
/// collision element
|
||||
boost::shared_ptr<Collision> collision;
|
||||
|
||||
/// a collection of visual elements, keyed by a string tag called "group"
|
||||
std::map<std::string, boost::shared_ptr<std::vector<boost::shared_ptr<Visual> > > > visual_groups;
|
||||
|
||||
/// a collection of collision elements, keyed by a string tag called "group"
|
||||
std::map<std::string, boost::shared_ptr<std::vector<boost::shared_ptr<Collision> > > > collision_groups;
|
||||
|
||||
/// Parent Joint element
|
||||
/// explicitly stating "parent" because we want directional-ness for tree structure
|
||||
/// every link can have one parent
|
||||
boost::shared_ptr<Joint> parent_joint;
|
||||
|
||||
std::vector<boost::shared_ptr<Joint> > child_joints;
|
||||
std::vector<boost::shared_ptr<Link> > child_links;
|
||||
|
||||
bool initXml(TiXmlElement* config);
|
||||
|
||||
boost::shared_ptr<Link> getParent() const
|
||||
{return parent_link_.lock();};
|
||||
|
||||
void setParent(boost::shared_ptr<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_groups.clear();
|
||||
};
|
||||
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_;
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,306 @@
|
|||
/*********************************************************************
|
||||
* 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_POSE_H
|
||||
#define URDF_POSE_H
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <math.h>
|
||||
#include <ros/ros.h>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
|
||||
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;};
|
||||
bool 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)
|
||||
{
|
||||
ROS_ERROR("Vector3 xyz element (%s) is not a valid float",pieces[i].c_str());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (xyz.size() != 3) {
|
||||
ROS_ERROR("Vector contains %i elements instead of 3 elements", (int)xyz.size());
|
||||
return false;
|
||||
}
|
||||
|
||||
this->x = xyz[0];
|
||||
this->y = xyz[1];
|
||||
this->z = xyz[2];
|
||||
|
||||
return true;
|
||||
};
|
||||
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;
|
||||
|
||||
bool init(const std::string &rotation_str)
|
||||
{
|
||||
this->clear();
|
||||
|
||||
Vector3 rpy;
|
||||
|
||||
if (!rpy.init(rotation_str))
|
||||
return false;
|
||||
else
|
||||
{
|
||||
this->setFromRPY(rpy.x,rpy.y,rpy.z);
|
||||
return true;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
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();
|
||||
};
|
||||
bool initXml(TiXmlElement* xml)
|
||||
{
|
||||
this->clear();
|
||||
if (!xml)
|
||||
{
|
||||
ROS_DEBUG("parsing pose: xml empty");
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
const char* xyz_str = xml->Attribute("xyz");
|
||||
if (xyz_str == NULL)
|
||||
{
|
||||
ROS_DEBUG("parsing pose: no xyz, using default values.");
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (!this->position.init(xyz_str))
|
||||
{
|
||||
ROS_ERROR("malformed xyz");
|
||||
this->position.clear();
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
const char* rpy_str = xml->Attribute("rpy");
|
||||
if (rpy_str == NULL)
|
||||
{
|
||||
ROS_DEBUG("parsing pose: no rpy, using default values.");
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (!this->rotation.init(rpy_str))
|
||||
{
|
||||
ROS_ERROR("malformed rpy");
|
||||
return false;
|
||||
this->rotation.clear();
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,159 @@
|
|||
/**
|
||||
\mainpage
|
||||
\htmlinclude manifest.html
|
||||
|
||||
urdf::Model is a class containing robot model data structure.
|
||||
Every Robot Description File (URDF) can be described as a list of Links (urdf::Model::links_) and Joints (urdf::Model::joints_).
|
||||
The connection between links(nodes) and joints(edges) should define a tree (i.e. 1 parent link, 0+ children links).
|
||||
\li Here is an example Robot Description Describing a Parent Link 'P', a Child Link 'C', and a Joint 'J'
|
||||
@verbatim
|
||||
<joint name="J" type="revolute">
|
||||
<dynamics damping="1" friction="0"/>
|
||||
<limit lower="0.9" upper="2.1" effort="1000" velocity="1"/>
|
||||
<safety_controller soft_lower_limit="0.7" soft_upper_limit="2.1" k_position="1" k_velocity="1" />
|
||||
<calibration reference_position="0.7" />
|
||||
<mimic joint="J100" offset="0" multiplier="0.7" />
|
||||
|
||||
<!-- origin: origin of the joint in the parent frame -->
|
||||
<!-- child link frame is the joint frame -->
|
||||
<!-- axis is in the joint frame -->
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<parent link="P"/>
|
||||
<child link="C"/>
|
||||
</joint>
|
||||
|
||||
<link name="C">
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="Green"/>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="1.01 1.01 1.01"/>
|
||||
</geometry>
|
||||
<contact_coefficient mu="0" resitution="0" k_p="0" k_d="0" />
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<material name="Green">
|
||||
<texture filename="...texture file..." />
|
||||
<!--color rgb="255 255 255" /-->
|
||||
</material>
|
||||
@endverbatim
|
||||
|
||||
|
||||
|
||||
\section codeapi Code API
|
||||
|
||||
The URDF parser API contains the following methods:
|
||||
\li Parse and build tree from XML: urdf::Model::initXml
|
||||
\li Parse and build tree from File: urdf::Model::initFile
|
||||
\li Parse and build tree from String: urdf::Model::initString
|
||||
\li Get Root Link: urdf::Model::getRoot
|
||||
\li Get Link by name urdf::Model::getLink
|
||||
\li Get all Link's urdf::Model::getLinks
|
||||
\li Get Joint by name urdf::Model::getJoint
|
||||
|
||||
<!--
|
||||
\section rosapi ROS API
|
||||
|
||||
Names are very important in ROS because they can be remapped on the
|
||||
command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
|
||||
APPEAR IN THE CODE. You should list names of every topic, service and
|
||||
parameter used in your code. There is a template below that you can
|
||||
use to document each node separately.
|
||||
|
||||
List of nodes:
|
||||
- \b node_name1
|
||||
- \b node_name2
|
||||
-->
|
||||
|
||||
<!-- START: copy from here to 'END' for each node
|
||||
|
||||
<hr>
|
||||
|
||||
\subsection node_name node_name
|
||||
|
||||
node_name does (provide a basic description of your node)
|
||||
|
||||
\subsubsection Usage
|
||||
\verbatim
|
||||
$ node_type1 [standard ROS args]
|
||||
\endverbatim
|
||||
|
||||
\par Example
|
||||
|
||||
\verbatim
|
||||
$ node_type1
|
||||
\endverbatim
|
||||
|
||||
|
||||
\subsubsection topics ROS topics
|
||||
|
||||
Subscribes to:
|
||||
- \b "in": [std_msgs/FooType] description of in
|
||||
|
||||
Publishes to:
|
||||
- \b "out": [std_msgs/FooType] description of out
|
||||
|
||||
|
||||
\subsubsection parameters ROS parameters
|
||||
|
||||
Reads the following parameters from the parameter server
|
||||
|
||||
- \b "~param_name" : \b [type] description of param_name
|
||||
- \b "~my_param" : \b [string] description of my_param
|
||||
|
||||
Sets the following parameters on the parameter server
|
||||
|
||||
- \b "~param_name" : \b [type] description of param_name
|
||||
|
||||
|
||||
\subsubsection services ROS services
|
||||
- \b "foo_service": [std_srvs/FooType] description of foo_service
|
||||
|
||||
|
||||
END: copy for each node -->
|
||||
|
||||
|
||||
<!-- START: Uncomment if you have any command-line tools
|
||||
|
||||
\section commandline Command-line tools
|
||||
|
||||
This section is a catch-all for any additional tools that your package
|
||||
provides or uses that may be of use to the reader. For example:
|
||||
|
||||
- tools/scripts (e.g. rospack, roscd)
|
||||
- roslaunch .launch files
|
||||
- xmlparam files
|
||||
|
||||
\subsection script_name script_name
|
||||
|
||||
Description of what this script/file does.
|
||||
|
||||
\subsubsection Usage
|
||||
\verbatim
|
||||
$ ./script_name [args]
|
||||
\endverbatim
|
||||
|
||||
\par Example
|
||||
|
||||
\verbatim
|
||||
$ ./script_name foo bar
|
||||
\endverbatim
|
||||
|
||||
END: Command-Line Tools Section -->
|
||||
|
||||
*/
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
<package>
|
||||
<description brief="URDF C++ interface.">
|
||||
This package contains URDF C++ interface
|
||||
</description>
|
||||
<author>Wim Meeussen, John Hsu</author>
|
||||
<license>BSD</license>
|
||||
<review status="Doc reviewed" notes=""/>
|
||||
<url>http://ros.org/wiki/urdf_interface</url>
|
||||
|
||||
<export>
|
||||
<cpp cflags="-I${prefix}/include"/>
|
||||
</export>
|
||||
|
||||
</package>
|
||||
|
||||
|
||||
Loading…
Reference in New Issue