bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

splitting off urdf_interface

This commit is contained in:
Wim Meeussen 2011-06-03 09:39:53 -07:00
parent 1be003b8d1
commit 818b5f6788
8 changed files with 1119 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
urdf_interface/CMakeLists.txt Normal file
View File

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

1
urdf_interface/Makefile Normal file
View File

@ -0,0 +1 @@
include $(shell rospack find mk)/cmake.mk

104
urdf_interface/include/urdf_interface/color.h Normal file
View File

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

235
urdf_interface/include/urdf_interface/joint.h Normal file
View File

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

259
urdf_interface/include/urdf_interface/link.h Normal file
View File

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

306
urdf_interface/include/urdf_interface/pose.h Normal file
View File

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

159
urdf_interface/mainpage.dox Normal file
View File

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

16
urdf_interface/manifest.xml Normal file
View File

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