diff --git a/urdf_interface/CMakeLists.txt b/urdf_interface/CMakeLists.txt new file mode 100644 index 0000000..94f624b --- /dev/null +++ b/urdf_interface/CMakeLists.txt @@ -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) diff --git a/urdf_interface/Makefile b/urdf_interface/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/urdf_interface/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/urdf_interface/include/urdf_interface/color.h b/urdf_interface/include/urdf_interface/color.h new file mode 100644 index 0000000..c132da4 --- /dev/null +++ b/urdf_interface/include/urdf_interface/color.h @@ -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 +#include +#include +#include +#include +#include + +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 pieces; + std::vector 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(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 + diff --git a/urdf_interface/include/urdf_interface/joint.h b/urdf_interface/include/urdf_interface/joint.h new file mode 100644 index 0000000..551e2c6 --- /dev/null +++ b/urdf_interface/include/urdf_interface/joint.h @@ -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 +#include +#include +#include + +#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 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 dynamics; + + /// Joint Limits + boost::shared_ptr limits; + + /// Unsupported Hidden Feature + boost::shared_ptr safety; + + /// Unsupported Hidden Feature + boost::shared_ptr calibration; + + /// Option to Mimic another Joint + boost::shared_ptr 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 diff --git a/urdf_interface/include/urdf_interface/link.h b/urdf_interface/include/urdf_interface/link.h new file mode 100644 index 0000000..96f4528 --- /dev/null +++ b/urdf_interface/include/urdf_interface/link.h @@ -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 +#include +#include +#include + +#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; + + std::string material_name; + boost::shared_ptr 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; + + 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; + + /// visual element + boost::shared_ptr visual; + + /// collision element + boost::shared_ptr collision; + + /// a collection of visual elements, keyed by a string tag called "group" + std::map > > > visual_groups; + + /// a collection of collision elements, keyed by a string tag called "group" + std::map > > > 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 parent_joint; + + std::vector > child_joints; + std::vector > child_links; + + bool initXml(TiXmlElement* config); + + boost::shared_ptr getParent() const + {return parent_link_.lock();}; + + void setParent(boost::shared_ptr 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 child); + void addChild(boost::shared_ptr child); + void addChildJoint(boost::shared_ptr child); + + void addVisual(std::string group_name, boost::shared_ptr visual); + boost::shared_ptr > > getVisuals(const std::string& group_name) const; + void addCollision(std::string group_name, boost::shared_ptr collision); + boost::shared_ptr > > getCollisions(const std::string& group_name) const; +private: + boost::weak_ptr parent_link_; + +}; + + + + +} + +#endif diff --git a/urdf_interface/include/urdf_interface/pose.h b/urdf_interface/include/urdf_interface/pose.h new file mode 100644 index 0000000..9d8bd79 --- /dev/null +++ b/urdf_interface/include/urdf_interface/pose.h @@ -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 +#include +#include +#include +#include +#include + +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 pieces; + std::vector 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(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 diff --git a/urdf_interface/mainpage.dox b/urdf_interface/mainpage.dox new file mode 100644 index 0000000..9287a45 --- /dev/null +++ b/urdf_interface/mainpage.dox @@ -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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @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 + + + + + + + + +*/ diff --git a/urdf_interface/manifest.xml b/urdf_interface/manifest.xml new file mode 100644 index 0000000..2e71051 --- /dev/null +++ b/urdf_interface/manifest.xml @@ -0,0 +1,16 @@ + + + This package contains URDF C++ interface + + Wim Meeussen, John Hsu + BSD + + http://ros.org/wiki/urdf_interface + + + + + + + +