diff --git a/include/urdf_exception/exception.h b/include/urdf_exception/exception.h index 24222f1..8b086c9 100644 --- a/include/urdf_exception/exception.h +++ b/include/urdf_exception/exception.h @@ -32,22 +32,15 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -// URDF exceptions #ifndef URDF_INTERFACE_EXCEPTION_H_ #define URDF_INTERFACE_EXCEPTION_H_ -#include -#include - -namespace urdf -{ - -class ParseError: public std::runtime_error -{ -public: - ParseError(const std::string &error_msg) : std::runtime_error(error_msg) {}; -}; +#if defined(_MSC_VER) + #pragma message("warning: urdf_exception/exception.h is deprecated. Please use urdf_exception/exception.hpp instead.") +#else + #warning urdf_exception/exception.h is deprecated. Please use urdf_exception/exception.hpp instead. +#endif -} +#include #endif diff --git a/include/urdf_exception/exception.hpp b/include/urdf_exception/exception.hpp new file mode 100644 index 0000000..a1a2ced --- /dev/null +++ b/include/urdf_exception/exception.hpp @@ -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_HPP_ +#define URDF_INTERFACE_EXCEPTION_HPP_ + +#include +#include + +namespace urdf +{ + +class ParseError: public std::runtime_error +{ +public: + ParseError(const std::string &error_msg) : std::runtime_error(error_msg) {}; +}; + +} + +#endif diff --git a/include/urdf_model/color.h b/include/urdf_model/color.h index 2886571..2a38292 100644 --- a/include/urdf_model/color.h +++ b/include/urdf_model/color.h @@ -1,13 +1,13 @@ /********************************************************************* * 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 @@ -17,7 +17,7 @@ * * 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 @@ -32,74 +32,15 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Josh Faust */ - #ifndef URDF_INTERFACE_COLOR_H #define URDF_INTERFACE_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; - urdf::split_string( pieces, vector_str, " "); - for (unsigned int i = 0; i < pieces.size(); ++i) - { - if (!pieces[i].empty()) - { - try - { - double piece = strToDouble(pieces[i].c_str()); - if ((piece < 0) || (piece > 1)) - throw ParseError("Component [" + pieces[i] + "] is outside the valid range for colors [0, 1]"); - rgba.push_back(static_cast(piece)); - } - catch (std::runtime_error &/*e*/) { - throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a color value)"); - } - } - } - - if (rgba.size() != 4) - { - return false; - } - - this->r = rgba[0]; - this->g = rgba[1]; - this->b = rgba[2]; - this->a = rgba[3]; - - return true; - }; -}; +#if defined(_MSC_VER) + #pragma message("warning: urdf_model/color.h is deprecated. Please use urdf_model/color.hpp instead.") +#else + #warning urdf_model/color.h is deprecated. Please use urdf_model/color.hpp instead. +#endif -} +#include #endif - diff --git a/include/urdf_model/color.hpp b/include/urdf_model/color.hpp new file mode 100644 index 0000000..1936279 --- /dev/null +++ b/include/urdf_model/color.hpp @@ -0,0 +1,105 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Josh Faust */ + +#ifndef URDF_INTERFACE_COLOR_HPP +#define URDF_INTERFACE_COLOR_HPP + +#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; + urdf::split_string( pieces, vector_str, " "); + for (unsigned int i = 0; i < pieces.size(); ++i) + { + if (!pieces[i].empty()) + { + try + { + double piece = strToDouble(pieces[i].c_str()); + if ((piece < 0) || (piece > 1)) + throw ParseError("Component [" + pieces[i] + "] is outside the valid range for colors [0, 1]"); + rgba.push_back(static_cast(piece)); + } + catch (std::runtime_error &/*e*/) { + throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a color value)"); + } + } + } + + if (rgba.size() != 4) + { + return false; + } + + this->r = rgba[0]; + this->g = rgba[1]; + this->b = rgba[2]; + this->a = rgba[3]; + + return true; + }; +}; + +} + +#endif + diff --git a/include/urdf_model/joint.h b/include/urdf_model/joint.h index c8b62f0..495070b 100644 --- a/include/urdf_model/joint.h +++ b/include/urdf_model/joint.h @@ -1,13 +1,13 @@ /********************************************************************* * 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 @@ -17,7 +17,7 @@ * * 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 @@ -32,206 +32,15 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Wim Meeussen */ - #ifndef URDF_INTERFACE_JOINT_H #define URDF_INTERFACE_JOINT_H -#include -#include -#include - -#include "urdf_model/pose.h" -#include "urdf_model/types.h" - - -namespace urdf{ - -class Link; - -class JointDynamics -{ -public: - JointDynamics() { this->clear(); }; - double damping; - double friction; - - void clear() - { - damping = 0; - friction = 0; - }; -}; - -class JointLimits -{ -public: - JointLimits() { this->clear(); }; - double lower; - double upper; - double effort; - double velocity; - double acceleration; - double deceleration; - double jerk; - - void clear() - { - lower = 0; - upper = 0; - effort = 0; - velocity = 0; - acceleration = std::numeric_limits::infinity(); - deceleration = std::numeric_limits::infinity(); - jerk = std::numeric_limits::infinity(); - }; -}; - -/// \brief Parameters for Joint Safety Controllers -class JointSafety -{ -public: - /// clear variables on construction - JointSafety() { this->clear(); }; - - /// - /// IMPORTANT: The safety controller support is very much PR2 specific, not intended for generic usage. - /// - /// Basic safety controller operation is as follows - /// - /// current safety controllers will take effect on joints outside the position range below: - /// - /// position range: [JointSafety::soft_lower_limit + JointLimits::velocity / JointSafety::k_position, - /// JointSafety::soft_uppper_limit - JointLimits::velocity / JointSafety::k_position] - /// - /// if (joint_position is outside of the position range above) - /// velocity_limit_min = -JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_lower_limit) - /// velocity_limit_max = JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_upper_limit) - /// else - /// velocity_limit_min = -JointLimits::velocity - /// velocity_limit_max = JointLimits::velocity - /// - /// velocity range: [velocity_limit_min + JointLimits::effort / JointSafety::k_velocity, - /// velocity_limit_max - JointLimits::effort / JointSafety::k_velocity] - /// - /// if (joint_velocity is outside of the velocity range above) - /// effort_limit_min = -JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_min) - /// effort_limit_max = JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_max) - /// else - /// effort_limit_min = -JointLimits::effort - /// effort_limit_max = JointLimits::effort - /// - /// Final effort command sent to the joint is saturated by [effort_limit_min,effort_limit_max] - /// - /// Please see wiki for more details: http://www.ros.org/wiki/pr2_controller_manager/safety_limits - /// - double soft_upper_limit; - double soft_lower_limit; - double k_position; - double k_velocity; - - void clear() - { - soft_upper_limit = 0; - soft_lower_limit = 0; - k_position = 0; - k_velocity = 0; - }; -}; - - -class JointCalibration -{ -public: - JointCalibration() { this->clear(); }; - double reference_position; - DoubleSharedPtr rising, falling; - - void clear() - { - reference_position = 0; - }; -}; - -class JointMimic -{ -public: - JointMimic() { this->clear(); }; - double offset; - double multiplier; - std::string joint_name; - - void clear() - { - offset = 0.0; - multiplier = 0.0; - joint_name.clear(); - }; -}; - - -class Joint -{ -public: - - Joint() { this->clear(); }; - - std::string name; - enum - { - UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED - } type; - - /// \brief type_ meaning of axis_ - /// ------------------------------------------------------ - /// UNKNOWN unknown type - /// REVOLUTE rotation axis - /// PRISMATIC translation axis - /// FLOATING N/A - /// PLANAR plane normal axis - /// FIXED N/A - Vector3 axis; - - /// child Link element - /// child link frame is the same as the Joint frame - std::string child_link_name; - - /// parent Link element - /// origin specifies the transform from Parent Link to Joint Frame - std::string parent_link_name; - /// transform from Parent Link frame to Joint frame - Pose parent_to_joint_origin_transform; - - /// Joint Dynamics - JointDynamicsSharedPtr dynamics; - - /// Joint Limits - JointLimitsSharedPtr limits; - - /// Unsupported Hidden Feature - JointSafetySharedPtr safety; - - /// Unsupported Hidden Feature - JointCalibrationSharedPtr calibration; - - /// Option to Mimic another Joint - JointMimicSharedPtr mimic; - - void clear() - { - this->axis.clear(); - this->child_link_name.clear(); - this->parent_link_name.clear(); - this->parent_to_joint_origin_transform.clear(); - this->dynamics.reset(); - this->limits.reset(); - this->safety.reset(); - this->calibration.reset(); - this->mimic.reset(); - this->type = UNKNOWN; - }; -}; +#if defined(_MSC_VER) + #pragma message("warning: urdf_model/joint.h is deprecated. Please use urdf_model/joint.hpp instead.") +#else + #warning urdf_model/joint.h is deprecated. Please use urdf_model/joint.hpp instead. +#endif -} +#include #endif diff --git a/include/urdf_model/joint.hpp b/include/urdf_model/joint.hpp new file mode 100644 index 0000000..809c258 --- /dev/null +++ b/include/urdf_model/joint.hpp @@ -0,0 +1,237 @@ +/********************************************************************* +* 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_HPP +#define URDF_INTERFACE_JOINT_HPP + +#include +#include +#include + +#include "urdf_model/pose.hpp" +#include "urdf_model/types.hpp" + + +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; + double acceleration; + double deceleration; + double jerk; + + void clear() + { + lower = 0; + upper = 0; + effort = 0; + velocity = 0; + acceleration = std::numeric_limits::infinity(); + deceleration = std::numeric_limits::infinity(); + jerk = std::numeric_limits::infinity(); + }; +}; + +/// \brief Parameters for Joint Safety Controllers +class JointSafety +{ +public: + /// clear variables on construction + JointSafety() { this->clear(); }; + + /// + /// IMPORTANT: The safety controller support is very much PR2 specific, not intended for generic usage. + /// + /// Basic safety controller operation is as follows + /// + /// current safety controllers will take effect on joints outside the position range below: + /// + /// position range: [JointSafety::soft_lower_limit + JointLimits::velocity / JointSafety::k_position, + /// JointSafety::soft_uppper_limit - JointLimits::velocity / JointSafety::k_position] + /// + /// if (joint_position is outside of the position range above) + /// velocity_limit_min = -JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_lower_limit) + /// velocity_limit_max = JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_upper_limit) + /// else + /// velocity_limit_min = -JointLimits::velocity + /// velocity_limit_max = JointLimits::velocity + /// + /// velocity range: [velocity_limit_min + JointLimits::effort / JointSafety::k_velocity, + /// velocity_limit_max - JointLimits::effort / JointSafety::k_velocity] + /// + /// if (joint_velocity is outside of the velocity range above) + /// effort_limit_min = -JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_min) + /// effort_limit_max = JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_max) + /// else + /// effort_limit_min = -JointLimits::effort + /// effort_limit_max = JointLimits::effort + /// + /// Final effort command sent to the joint is saturated by [effort_limit_min,effort_limit_max] + /// + /// Please see wiki for more details: http://www.ros.org/wiki/pr2_controller_manager/safety_limits + /// + double soft_upper_limit; + double soft_lower_limit; + double k_position; + double k_velocity; + + void clear() + { + soft_upper_limit = 0; + soft_lower_limit = 0; + k_position = 0; + k_velocity = 0; + }; +}; + + +class JointCalibration +{ +public: + JointCalibration() { this->clear(); }; + double reference_position; + DoubleSharedPtr rising, falling; + + void clear() + { + reference_position = 0; + }; +}; + +class JointMimic +{ +public: + JointMimic() { this->clear(); }; + double offset; + double multiplier; + std::string joint_name; + + void clear() + { + offset = 0.0; + multiplier = 0.0; + joint_name.clear(); + }; +}; + + +class Joint +{ +public: + + Joint() { this->clear(); }; + + std::string name; + enum + { + UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED + } type; + + /// \brief type_ meaning of axis_ + /// ------------------------------------------------------ + /// UNKNOWN unknown type + /// REVOLUTE rotation axis + /// PRISMATIC translation axis + /// FLOATING N/A + /// PLANAR plane normal axis + /// FIXED N/A + Vector3 axis; + + /// child Link element + /// child link frame is the same as the Joint frame + std::string child_link_name; + + /// parent Link element + /// origin specifies the transform from Parent Link to Joint Frame + std::string parent_link_name; + /// transform from Parent Link frame to Joint frame + Pose parent_to_joint_origin_transform; + + /// Joint Dynamics + JointDynamicsSharedPtr dynamics; + + /// Joint Limits + JointLimitsSharedPtr limits; + + /// Unsupported Hidden Feature + JointSafetySharedPtr safety; + + /// Unsupported Hidden Feature + JointCalibrationSharedPtr calibration; + + /// Option to Mimic another Joint + JointMimicSharedPtr mimic; + + void clear() + { + this->axis.clear(); + this->child_link_name.clear(); + this->parent_link_name.clear(); + this->parent_to_joint_origin_transform.clear(); + this->dynamics.reset(); + this->limits.reset(); + this->safety.reset(); + this->calibration.reset(); + this->mimic.reset(); + this->type = UNKNOWN; + }; +}; + +} + +#endif diff --git a/include/urdf_model/link.h b/include/urdf_model/link.h index 855445b..8475ea9 100644 --- a/include/urdf_model/link.h +++ b/include/urdf_model/link.h @@ -1,13 +1,13 @@ /********************************************************************* * 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 @@ -17,7 +17,7 @@ * * 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 @@ -32,230 +32,15 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Wim Meeussen */ - #ifndef URDF_INTERFACE_LINK_H #define URDF_INTERFACE_LINK_H -#include -#include -#include - -#include "joint.h" -#include "color.h" -#include "types.h" - -namespace urdf{ - -class Geometry -{ -public: - enum {SPHERE, BOX, CYLINDER, MESH, CAPSULE} 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 Capsule : public Geometry -{ -public: - Capsule() { this->clear(); type = CAPSULE; }; - double length; - double radius; - - void clear() - { - length = 0; - radius = 0; - }; -}; - -class Material -{ -public: - Material() { this->clear(); }; - std::string name; - std::string texture_filename; - Color color; - - void clear() - { - color.clear(); - texture_filename.clear(); - name.clear(); - }; -}; - -class Inertial -{ -public: - Inertial() { this->clear(); }; - Pose origin; - double mass; - double ixx,ixy,ixz,iyy,iyz,izz; - - void clear() - { - origin.clear(); - mass = 0; - ixx = ixy = ixz = iyy = iyz = izz = 0; - }; -}; - -class Visual -{ -public: - Visual() { this->clear(); }; - Pose origin; - GeometrySharedPtr geometry; - - std::string material_name; - MaterialSharedPtr material; - - void clear() - { - origin.clear(); - material_name.clear(); - material.reset(); - geometry.reset(); - name.clear(); - }; - - std::string name; -}; - -class Collision -{ -public: - Collision() { this->clear(); }; - Pose origin; - GeometrySharedPtr geometry; - - void clear() - { - origin.clear(); - geometry.reset(); - name.clear(); - }; - - std::string name; - -}; - - -class Link -{ -public: - Link() { this->clear(); }; - - std::string name; - - /// inertial element - InertialSharedPtr inertial; - - /// visual element - VisualSharedPtr visual; - - /// collision element - CollisionSharedPtr collision; - - /// if more than one collision element is specified, all collision elements are placed in this array (the collision member points to the first element of the array) - std::vector 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 visual_array; - - /// Parent Joint element - /// explicitly stating "parent" because we want directional-ness for tree structure - /// every link can have one parent - JointSharedPtr parent_joint; - - std::vector child_joints; - std::vector child_links; - - LinkSharedPtr getParent() const - {return parent_link_.lock();}; - - void setParent(const LinkSharedPtr &parent) - { parent_link_ = parent; } - - void clear() - { - this->name.clear(); - this->inertial.reset(); - this->visual.reset(); - this->collision.reset(); - this->parent_joint.reset(); - this->child_joints.clear(); - this->child_links.clear(); - this->collision_array.clear(); - this->visual_array.clear(); - }; - -private: - LinkWeakPtr parent_link_; - -}; - - - +#if defined(_MSC_VER) + #pragma message("warning: urdf_model/link.h is deprecated. Please use urdf_model/link.hpp instead.") +#else + #warning urdf_model/link.h is deprecated. Please use urdf_model/link.hpp instead. +#endif -} +#include #endif diff --git a/include/urdf_model/link.hpp b/include/urdf_model/link.hpp new file mode 100644 index 0000000..dcf3b4c --- /dev/null +++ b/include/urdf_model/link.hpp @@ -0,0 +1,261 @@ +/********************************************************************* +* 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_HPP +#define URDF_INTERFACE_LINK_HPP + +#include +#include +#include + +#include "urdf_model/joint.hpp" +#include "urdf_model/color.hpp" +#include "urdf_model/types.hpp" + +namespace urdf{ + +class Geometry +{ +public: + enum {SPHERE, BOX, CYLINDER, MESH, CAPSULE} 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 Capsule : public Geometry +{ +public: + Capsule() { this->clear(); type = CAPSULE; }; + double length; + double radius; + + void clear() + { + length = 0; + radius = 0; + }; +}; + +class Material +{ +public: + Material() { this->clear(); }; + std::string name; + std::string texture_filename; + Color color; + + void clear() + { + color.clear(); + texture_filename.clear(); + name.clear(); + }; +}; + +class Inertial +{ +public: + Inertial() { this->clear(); }; + Pose origin; + double mass; + double ixx,ixy,ixz,iyy,iyz,izz; + + void clear() + { + origin.clear(); + mass = 0; + ixx = ixy = ixz = iyy = iyz = izz = 0; + }; +}; + +class Visual +{ +public: + Visual() { this->clear(); }; + Pose origin; + GeometrySharedPtr geometry; + + std::string material_name; + MaterialSharedPtr material; + + void clear() + { + origin.clear(); + material_name.clear(); + material.reset(); + geometry.reset(); + name.clear(); + }; + + std::string name; +}; + +class Collision +{ +public: + Collision() { this->clear(); }; + Pose origin; + GeometrySharedPtr geometry; + + void clear() + { + origin.clear(); + geometry.reset(); + name.clear(); + }; + + std::string name; + +}; + + +class Link +{ +public: + Link() { this->clear(); }; + + std::string name; + + /// inertial element + InertialSharedPtr inertial; + + /// visual element + VisualSharedPtr visual; + + /// collision element + CollisionSharedPtr collision; + + /// if more than one collision element is specified, all collision elements are placed in this array (the collision member points to the first element of the array) + std::vector 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 visual_array; + + /// Parent Joint element + /// explicitly stating "parent" because we want directional-ness for tree structure + /// every link can have one parent + JointSharedPtr parent_joint; + + std::vector child_joints; + std::vector child_links; + + LinkSharedPtr getParent() const + {return parent_link_.lock();}; + + void setParent(const LinkSharedPtr &parent) + { parent_link_ = parent; } + + void clear() + { + this->name.clear(); + this->inertial.reset(); + this->visual.reset(); + this->collision.reset(); + this->parent_joint.reset(); + this->child_joints.clear(); + this->child_links.clear(); + this->collision_array.clear(); + this->visual_array.clear(); + }; + +private: + LinkWeakPtr parent_link_; + +}; + + + + +} + +#endif diff --git a/include/urdf_model/model.h b/include/urdf_model/model.h index 8b7ced8..db85341 100644 --- a/include/urdf_model/model.h +++ b/include/urdf_model/model.h @@ -1,13 +1,13 @@ /********************************************************************* * 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 @@ -17,7 +17,7 @@ * * 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 @@ -32,175 +32,15 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Wim Meeussen */ - #ifndef URDF_INTERFACE_MODEL_H #define URDF_INTERFACE_MODEL_H -#include -#include -#include -#include -#include - -namespace urdf { - -class ModelInterface -{ -public: - LinkConstSharedPtr getRoot(void) const{return this->root_link_;}; - LinkConstSharedPtr getLink(const std::string& name) const - { - LinkConstSharedPtr ptr; - if (this->links_.find(name) == this->links_.end()) - ptr.reset(); - else - ptr = this->links_.find(name)->second; - return ptr; - }; - - JointConstSharedPtr getJoint(const std::string& name) const - { - JointConstSharedPtr ptr; - if (this->joints_.find(name) == this->joints_.end()) - ptr.reset(); - else - ptr = this->joints_.find(name)->second; - return ptr; - }; - - - const std::string& getName() const {return name_;}; - void getLinks(std::vector& links) const - { - for (std::map::const_iterator link = this->links_.begin();link != this->links_.end(); link++) - { - links.push_back(link->second); - } - }; - - void clear() - { - name_.clear(); - this->links_.clear(); - this->joints_.clear(); - this->materials_.clear(); - this->root_link_.reset(); - }; - - /// non-const getLink() - void getLink(const std::string& name, LinkSharedPtr &link) const - { - LinkSharedPtr ptr; - if (this->links_.find(name) == this->links_.end()) - ptr.reset(); - else - ptr = this->links_.find(name)->second; - link = ptr; - }; - - /// non-const getMaterial() - MaterialSharedPtr getMaterial(const std::string& name) const - { - MaterialSharedPtr ptr; - if (this->materials_.find(name) == this->materials_.end()) - ptr.reset(); - else - ptr = this->materials_.find(name)->second; - return ptr; - }; - - void initTree(std::map &parent_link_tree) - { - // loop through all joints, for every link, assign children links and children joints - for (std::map::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++) - { - std::string parent_link_name = joint->second->parent_link_name; - std::string child_link_name = joint->second->child_link_name; - - if (parent_link_name.empty() || child_link_name.empty()) - { - throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification."); - } - else - { - // find child and parent links - LinkSharedPtr child_link, parent_link; - this->getLink(child_link_name, child_link); - if (!child_link) - { - throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found"); - } - this->getLink(parent_link_name, parent_link); - if (!parent_link) - { - throw ParseError("parent link [" + parent_link_name + "] of joint [" + joint->first + "] not found. This is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint [" + joint->first + "] from your urdf file, or add \"\" 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 &parent_link_tree) - { - this->root_link_.reset(); - - // find the links that have no parent in the tree - for (std::map::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++) - { - std::map::const_iterator parent = parent_link_tree.find(l->first); - if (parent == parent_link_tree.end()) - { - // store root link - if (!this->root_link_) - { - getLink(l->first, this->root_link_); - } - // we already found a root link - else - { - throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]"); - } - } - } - if (!this->root_link_) - { - throw ParseError("No root link found. The robot xml is not a valid tree."); - } - } - - - /// \brief complete list of Links - std::map links_; - /// \brief complete list of Joints - std::map joints_; - /// \brief complete list of Materials - std::map materials_; - - /// \brief The name of the robot model - std::string name_; - - /// \brief The root is always a link (the parent of the tree describing the robot) - LinkSharedPtr root_link_; - - - -}; +#if defined(_MSC_VER) + #pragma message("warning: urdf_model/model.h is deprecated. Please use urdf_model/model.hpp instead.") +#else + #warning urdf_model/model.h is deprecated. Please use urdf_model/model.hpp instead. +#endif -} +#include #endif diff --git a/include/urdf_model/model.hpp b/include/urdf_model/model.hpp new file mode 100644 index 0000000..90b7096 --- /dev/null +++ b/include/urdf_model/model.hpp @@ -0,0 +1,206 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#ifndef URDF_INTERFACE_MODEL_HPP +#define URDF_INTERFACE_MODEL_HPP + +#include +#include +#include +#include +#include + +namespace urdf { + +class ModelInterface +{ +public: + LinkConstSharedPtr getRoot(void) const{return this->root_link_;}; + LinkConstSharedPtr getLink(const std::string& name) const + { + LinkConstSharedPtr ptr; + if (this->links_.find(name) == this->links_.end()) + ptr.reset(); + else + ptr = this->links_.find(name)->second; + return ptr; + }; + + JointConstSharedPtr getJoint(const std::string& name) const + { + JointConstSharedPtr ptr; + if (this->joints_.find(name) == this->joints_.end()) + ptr.reset(); + else + ptr = this->joints_.find(name)->second; + return ptr; + }; + + + const std::string& getName() const {return name_;}; + void getLinks(std::vector& links) const + { + for (std::map::const_iterator link = this->links_.begin();link != this->links_.end(); link++) + { + links.push_back(link->second); + } + }; + + void clear() + { + name_.clear(); + this->links_.clear(); + this->joints_.clear(); + this->materials_.clear(); + this->root_link_.reset(); + }; + + /// non-const getLink() + void getLink(const std::string& name, LinkSharedPtr &link) const + { + LinkSharedPtr ptr; + if (this->links_.find(name) == this->links_.end()) + ptr.reset(); + else + ptr = this->links_.find(name)->second; + link = ptr; + }; + + /// non-const getMaterial() + MaterialSharedPtr getMaterial(const std::string& name) const + { + MaterialSharedPtr ptr; + if (this->materials_.find(name) == this->materials_.end()) + ptr.reset(); + else + ptr = this->materials_.find(name)->second; + return ptr; + }; + + void initTree(std::map &parent_link_tree) + { + // loop through all joints, for every link, assign children links and children joints + for (std::map::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++) + { + std::string parent_link_name = joint->second->parent_link_name; + std::string child_link_name = joint->second->child_link_name; + + if (parent_link_name.empty() || child_link_name.empty()) + { + throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification."); + } + else + { + // find child and parent links + LinkSharedPtr child_link, parent_link; + this->getLink(child_link_name, child_link); + if (!child_link) + { + throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found"); + } + this->getLink(parent_link_name, parent_link); + if (!parent_link) + { + throw ParseError("parent link [" + parent_link_name + "] of joint [" + joint->first + "] not found. This is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint [" + joint->first + "] from your urdf file, or add \"\" 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 &parent_link_tree) + { + this->root_link_.reset(); + + // find the links that have no parent in the tree + for (std::map::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++) + { + std::map::const_iterator parent = parent_link_tree.find(l->first); + if (parent == parent_link_tree.end()) + { + // store root link + if (!this->root_link_) + { + getLink(l->first, this->root_link_); + } + // we already found a root link + else + { + throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]"); + } + } + } + if (!this->root_link_) + { + throw ParseError("No root link found. The robot xml is not a valid tree."); + } + } + + + /// \brief complete list of Links + std::map links_; + /// \brief complete list of Joints + std::map joints_; + /// \brief complete list of Materials + std::map materials_; + + /// \brief The name of the robot model + std::string name_; + + /// \brief The root is always a link (the parent of the tree describing the robot) + LinkSharedPtr root_link_; + + + +}; + +} + +#endif diff --git a/include/urdf_model/pose.h b/include/urdf_model/pose.h index e4677f2..f284244 100644 --- a/include/urdf_model/pose.h +++ b/include/urdf_model/pose.h @@ -32,289 +32,15 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Wim Meeussen */ - #ifndef URDF_INTERFACE_POSE_H #define URDF_INTERFACE_POSE_H -#include -#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;}; - void init(const std::string &vector_str) - { - this->clear(); - std::vector pieces; - std::vector xyz; - urdf::split_string( pieces, vector_str, " "); - for (unsigned int i = 0; i < pieces.size(); ++i){ - if (pieces[i] != ""){ - try { - xyz.push_back(strToDouble(pieces[i].c_str())); - } catch(std::runtime_error &) { - throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)"); - } - } - } - - if (xyz.size() != 3) - throw ParseError("Parser found " + std::to_string(xyz.size()) + " elements but 3 expected while parsing vector [" + vector_str + "]"); - - this->x = xyz[0]; - this->y = xyz[1]; - this->z = xyz[2]; - } - - Vector3 operator+(Vector3 vec) - { - return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z); - }; -}; - -class Vector4 -{ - public: - Vector4(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;}; - Vector4() {this->clear();}; - double x; - double y; - double z; - double w; - - void clear() {this->x=this->y=this->z=this->w=0.0;}; - void init(const std::string &vector_str) - { - this->clear(); - std::vector pieces; - std::vector xyzw; - urdf::split_string( pieces, vector_str, " "); - for (unsigned int i = 0; i < pieces.size(); ++i){ - if (pieces[i] != ""){ - try { - xyzw.push_back(strToDouble(pieces[i].c_str())); - } catch(std::runtime_error &) { - throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)"); - } - } - } - - if (xyzw.size() != 4) - throw ParseError("Parser found " + std::to_string(xyzw.size()) + " elements but 4 expected while parsing vector [" + vector_str + "]"); - - this->x = xyzw[0]; - this->y = xyzw[1]; - this->z = xyzw[2]; - this->w = xyzw[3]; - } -}; - -class Rotation -{ -public: - Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;}; - Rotation() {this->clear();}; - void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const - { - quat_x = this->x; - quat_y = this->y; - quat_z = this->z; - quat_w = this->w; - }; - void getRPY(double &roll,double &pitch,double &yaw) const - { - double sqw; - double sqx; - double sqy; - double sqz; - - sqx = this->x * this->x; - sqy = this->y * this->y; - sqz = this->z * this->z; - sqw = this->w * this->w; - - // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/ - double sarg = -2 * (this->x*this->z - this->w*this->y); - const double pi_2 = 1.57079632679489661923; - if (sarg <= -0.99999) { - pitch = -pi_2; - roll = 0; - yaw = 2 * atan2(this->x, -this->y); - } else if (sarg >= 0.99999) { - pitch = pi_2; - roll = 0; - yaw = 2 * atan2(-this->x, this->y); - } else { - pitch = asin(sarg); - roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz); - yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz); - } - - }; - void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w) - { - this->x = quat_x; - this->y = quat_y; - this->z = quat_z; - this->w = quat_w; - this->normalize(); - }; - void setFromRPY(double roll, double pitch, double yaw) - { - double phi, the, psi; - - phi = roll / 2.0; - the = pitch / 2.0; - psi = yaw / 2.0; - - this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi); - this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi); - this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi); - this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi); - - this->normalize(); - }; - - double x,y,z,w; - - void init(const std::string &rotation_str) - { - this->clear(); - Vector3 rpy; - rpy.init(rotation_str); - setFromRPY(rpy.x, rpy.y, rpy.z); - } - - void initQuaternion(const std::string &rotation_str) - { - this->clear(); - Vector4 xyzw_quat; - xyzw_quat.init(rotation_str); - this->x = xyzw_quat.x; - this->y = xyzw_quat.y; - this->z = xyzw_quat.z; - this->w = xyzw_quat.w; - this->normalize(); - } - - void clear() { this->x=this->y=this->z=0.0;this->w=1.0; } - - void normalize() - { - const double squared_norm = - this->x * this->x + - this->y * this->y + - this->z * this->z + - this->w * this->w; - - // Note: This function historically checked if the norm of the elements - // equaled 0 using a strict floating point equals (s == 0.0) to prevent a - // divide by zero error. This comparison will cause the compiler to issue a - // warning with `-Wfloat-equal`. Comparing against a small epsilon would - // likely be more ideal, but its choice may be application specific and - // could change behavior of legacy code if chosen poorly. Using `<=` - // silences the warning without changing the behavior of this function, even - // though there are no situations squared_norm can be strictly less than 0. - if (squared_norm <= 0.0) - { - this->x = 0.0; - this->y = 0.0; - this->z = 0.0; - this->w = 1.0; - } - else - { - const double s = sqrt(squared_norm); - 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(); - }; -}; +#if defined(_MSC_VER) + #pragma message("warning: urdf_model/pose.h is deprecated. Please use urdf_model/pose.hpp instead.") +#else + #warning urdf_model/pose.h is deprecated. Please use urdf_model/pose.hpp instead. +#endif -} +#include #endif diff --git a/include/urdf_model/pose.hpp b/include/urdf_model/pose.hpp new file mode 100644 index 0000000..705d6f1 --- /dev/null +++ b/include/urdf_model/pose.hpp @@ -0,0 +1,320 @@ +/********************************************************************* +* 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_HPP +#define URDF_INTERFACE_POSE_HPP + +#include +#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;}; + void init(const std::string &vector_str) + { + this->clear(); + std::vector pieces; + std::vector xyz; + urdf::split_string( pieces, vector_str, " "); + for (unsigned int i = 0; i < pieces.size(); ++i){ + if (pieces[i] != ""){ + try { + xyz.push_back(strToDouble(pieces[i].c_str())); + } catch(std::runtime_error &) { + throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)"); + } + } + } + + if (xyz.size() != 3) + throw ParseError("Parser found " + std::to_string(xyz.size()) + " elements but 3 expected while parsing vector [" + vector_str + "]"); + + this->x = xyz[0]; + this->y = xyz[1]; + this->z = xyz[2]; + } + + Vector3 operator+(Vector3 vec) + { + return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z); + }; +}; + +class Vector4 +{ + public: + Vector4(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;}; + Vector4() {this->clear();}; + double x; + double y; + double z; + double w; + + void clear() {this->x=this->y=this->z=this->w=0.0;}; + void init(const std::string &vector_str) + { + this->clear(); + std::vector pieces; + std::vector xyzw; + urdf::split_string( pieces, vector_str, " "); + for (unsigned int i = 0; i < pieces.size(); ++i){ + if (pieces[i] != ""){ + try { + xyzw.push_back(strToDouble(pieces[i].c_str())); + } catch(std::runtime_error &) { + throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)"); + } + } + } + + if (xyzw.size() != 4) + throw ParseError("Parser found " + std::to_string(xyzw.size()) + " elements but 4 expected while parsing vector [" + vector_str + "]"); + + this->x = xyzw[0]; + this->y = xyzw[1]; + this->z = xyzw[2]; + this->w = xyzw[3]; + } +}; + +class Rotation +{ +public: + Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;}; + Rotation() {this->clear();}; + void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const + { + quat_x = this->x; + quat_y = this->y; + quat_z = this->z; + quat_w = this->w; + }; + void getRPY(double &roll,double &pitch,double &yaw) const + { + double sqw; + double sqx; + double sqy; + double sqz; + + sqx = this->x * this->x; + sqy = this->y * this->y; + sqz = this->z * this->z; + sqw = this->w * this->w; + + // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/ + double sarg = -2 * (this->x*this->z - this->w*this->y); + const double pi_2 = 1.57079632679489661923; + if (sarg <= -0.99999) { + pitch = -pi_2; + roll = 0; + yaw = 2 * atan2(this->x, -this->y); + } else if (sarg >= 0.99999) { + pitch = pi_2; + roll = 0; + yaw = 2 * atan2(-this->x, this->y); + } else { + pitch = asin(sarg); + roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz); + yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz); + } + + }; + void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w) + { + this->x = quat_x; + this->y = quat_y; + this->z = quat_z; + this->w = quat_w; + this->normalize(); + }; + void setFromRPY(double roll, double pitch, double yaw) + { + double phi, the, psi; + + phi = roll / 2.0; + the = pitch / 2.0; + psi = yaw / 2.0; + + this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi); + this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi); + this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi); + this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi); + + this->normalize(); + }; + + double x,y,z,w; + + void init(const std::string &rotation_str) + { + this->clear(); + Vector3 rpy; + rpy.init(rotation_str); + setFromRPY(rpy.x, rpy.y, rpy.z); + } + + void initQuaternion(const std::string &rotation_str) + { + this->clear(); + Vector4 xyzw_quat; + xyzw_quat.init(rotation_str); + this->x = xyzw_quat.x; + this->y = xyzw_quat.y; + this->z = xyzw_quat.z; + this->w = xyzw_quat.w; + this->normalize(); + } + + void clear() { this->x=this->y=this->z=0.0;this->w=1.0; } + + void normalize() + { + const double squared_norm = + this->x * this->x + + this->y * this->y + + this->z * this->z + + this->w * this->w; + + // Note: This function historically checked if the norm of the elements + // equaled 0 using a strict floating point equals (s == 0.0) to prevent a + // divide by zero error. This comparison will cause the compiler to issue a + // warning with `-Wfloat-equal`. Comparing against a small epsilon would + // likely be more ideal, but its choice may be application specific and + // could change behavior of legacy code if chosen poorly. Using `<=` + // silences the warning without changing the behavior of this function, even + // though there are no situations squared_norm can be strictly less than 0. + if (squared_norm <= 0.0) + { + this->x = 0.0; + this->y = 0.0; + this->z = 0.0; + this->w = 1.0; + } + else + { + const double s = sqrt(squared_norm); + 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 diff --git a/include/urdf_model/twist.h b/include/urdf_model/twist.h index ba57e4c..d1ccf29 100644 --- a/include/urdf_model/twist.h +++ b/include/urdf_model/twist.h @@ -1,13 +1,13 @@ /********************************************************************* * 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 @@ -17,7 +17,7 @@ * * 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 @@ -32,37 +32,15 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: John Hsu */ - #ifndef URDF_TWIST_H #define URDF_TWIST_H -#include -#include -#include -#include -#include - -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(); - }; -}; +#if defined(_MSC_VER) + #pragma message("warning: urdf_model/twist.h is deprecated. Please use urdf_model/twist.hpp instead.") +#else + #warning urdf_model/twist.h is deprecated. Please use urdf_model/twist.hpp instead. +#endif -} +#include #endif - diff --git a/include/urdf_model/twist.hpp b/include/urdf_model/twist.hpp new file mode 100644 index 0000000..9c92f57 --- /dev/null +++ b/include/urdf_model/twist.hpp @@ -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_HPP +#define URDF_TWIST_HPP + +#include +#include +#include +#include +#include + +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 + diff --git a/include/urdf_model/types.h b/include/urdf_model/types.h index 7165c41..91670e4 100644 --- a/include/urdf_model/types.h +++ b/include/urdf_model/types.h @@ -32,62 +32,15 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Steve Peters */ - #ifndef URDF_MODEL_TYPES_H #define URDF_MODEL_TYPES_H -#include - -#define URDF_TYPEDEF_CLASS_POINTER(Class) \ -class Class; \ -typedef std::shared_ptr Class##SharedPtr; \ -typedef std::shared_ptr Class##ConstSharedPtr; \ -typedef std::weak_ptr Class##WeakPtr - -namespace urdf{ - -// shared pointer used in joint.h -typedef std::shared_ptr DoubleSharedPtr; - -URDF_TYPEDEF_CLASS_POINTER(Box); -URDF_TYPEDEF_CLASS_POINTER(Collision); -URDF_TYPEDEF_CLASS_POINTER(Cylinder); -URDF_TYPEDEF_CLASS_POINTER(Geometry); -URDF_TYPEDEF_CLASS_POINTER(Inertial); -URDF_TYPEDEF_CLASS_POINTER(Joint); -URDF_TYPEDEF_CLASS_POINTER(JointCalibration); -URDF_TYPEDEF_CLASS_POINTER(JointDynamics); -URDF_TYPEDEF_CLASS_POINTER(JointLimits); -URDF_TYPEDEF_CLASS_POINTER(JointMimic); -URDF_TYPEDEF_CLASS_POINTER(JointSafety); -URDF_TYPEDEF_CLASS_POINTER(Link); -URDF_TYPEDEF_CLASS_POINTER(Material); -URDF_TYPEDEF_CLASS_POINTER(Mesh); -URDF_TYPEDEF_CLASS_POINTER(Sphere); -URDF_TYPEDEF_CLASS_POINTER(Visual); - -URDF_TYPEDEF_CLASS_POINTER(ModelInterface); - -// create *_pointer_cast functions in urdf namespace -template -std::shared_ptr const_pointer_cast(std::shared_ptr const & r) -{ - return std::const_pointer_cast(r); -} - -template -std::shared_ptr dynamic_pointer_cast(std::shared_ptr const & r) -{ - return std::dynamic_pointer_cast(r); -} - -template -std::shared_ptr static_pointer_cast(std::shared_ptr const & r) -{ - return std::static_pointer_cast(r); -} +#if defined(_MSC_VER) + #pragma message("warning: urdf_model/types.h is deprecated. Please use urdf_model/types.hpp instead.") +#else + #warning urdf_model/types.h is deprecated. Please use urdf_model/types.hpp instead. +#endif -} +#include #endif diff --git a/include/urdf_model/types.hpp b/include/urdf_model/types.hpp new file mode 100644 index 0000000..ee8d2b8 --- /dev/null +++ b/include/urdf_model/types.hpp @@ -0,0 +1,93 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Steve Peters */ + +#ifndef URDF_MODEL_TYPES_HPP +#define URDF_MODEL_TYPES_HPP + +#include + +#define URDF_TYPEDEF_CLASS_POINTER(Class) \ +class Class; \ +typedef std::shared_ptr Class##SharedPtr; \ +typedef std::shared_ptr Class##ConstSharedPtr; \ +typedef std::weak_ptr Class##WeakPtr + +namespace urdf{ + +// shared pointer used in joint.h +typedef std::shared_ptr DoubleSharedPtr; + +URDF_TYPEDEF_CLASS_POINTER(Box); +URDF_TYPEDEF_CLASS_POINTER(Collision); +URDF_TYPEDEF_CLASS_POINTER(Cylinder); +URDF_TYPEDEF_CLASS_POINTER(Geometry); +URDF_TYPEDEF_CLASS_POINTER(Inertial); +URDF_TYPEDEF_CLASS_POINTER(Joint); +URDF_TYPEDEF_CLASS_POINTER(JointCalibration); +URDF_TYPEDEF_CLASS_POINTER(JointDynamics); +URDF_TYPEDEF_CLASS_POINTER(JointLimits); +URDF_TYPEDEF_CLASS_POINTER(JointMimic); +URDF_TYPEDEF_CLASS_POINTER(JointSafety); +URDF_TYPEDEF_CLASS_POINTER(Link); +URDF_TYPEDEF_CLASS_POINTER(Material); +URDF_TYPEDEF_CLASS_POINTER(Mesh); +URDF_TYPEDEF_CLASS_POINTER(Sphere); +URDF_TYPEDEF_CLASS_POINTER(Visual); + +URDF_TYPEDEF_CLASS_POINTER(ModelInterface); + +// create *_pointer_cast functions in urdf namespace +template +std::shared_ptr const_pointer_cast(std::shared_ptr const & r) +{ + return std::const_pointer_cast(r); +} + +template +std::shared_ptr dynamic_pointer_cast(std::shared_ptr const & r) +{ + return std::dynamic_pointer_cast(r); +} + +template +std::shared_ptr static_pointer_cast(std::shared_ptr const & r) +{ + return std::static_pointer_cast(r); +} + +} + +#endif diff --git a/include/urdf_model/utils.h b/include/urdf_model/utils.h index f9f59c1..072a1fc 100644 --- a/include/urdf_model/utils.h +++ b/include/urdf_model/utils.h @@ -32,61 +32,15 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Steve Peters */ - #ifndef URDF_INTERFACE_UTILS_H #define URDF_INTERFACE_UTILS_H -#include -#include -#include -#include -#include - -namespace urdf { - -// Replacement for boost::split( ... , ... , boost::is_any_of(" ")) -inline -void split_string(std::vector &result, - const std::string &input, - const std::string &isAnyOf) -{ - std::string::size_type start = 0; - std::string::size_type end = input.find_first_of(isAnyOf, start); - while (end != std::string::npos) - { - result.push_back(input.substr(start, end-start)); - start = end + 1; - end = input.find_first_of(isAnyOf, start); - } - if (start < input.length()) - { - result.push_back(input.substr(start)); - } -} - -// This is a locale-safe version of string-to-double, which is suprisingly -// difficult to do correctly. This function ensures that the C locale is used -// for parsing, as that matches up with what the XSD for double specifies. -// On success, the double is returned; on failure, a std::runtime_error is -// thrown. -static inline double strToDouble(const char *in) -{ - std::stringstream ss; - ss.imbue(std::locale::classic()); - - ss << in; - - double out; - ss >> out; - - if (ss.fail() || !ss.eof()) { - throw std::runtime_error("Failed converting string to double"); - } - - return out; -} +#if defined(_MSC_VER) + #pragma message("warning: urdf_model/utils.h is deprecated. Please use urdf_model/utils.hpp instead.") +#else + #warning urdf_model/utils.h is deprecated. Please use urdf_model/utils.hpp instead. +#endif -} +#include #endif diff --git a/include/urdf_model/utils.hpp b/include/urdf_model/utils.hpp new file mode 100644 index 0000000..52982e1 --- /dev/null +++ b/include/urdf_model/utils.hpp @@ -0,0 +1,92 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2016, Open Source Robotics Foundation (OSRF) +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the OSRF nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Steve Peters */ + +#ifndef URDF_INTERFACE_UTILS_HPP +#define URDF_INTERFACE_UTILS_HPP + +#include +#include +#include +#include +#include + +namespace urdf { + +// Replacement for boost::split( ... , ... , boost::is_any_of(" ")) +inline +void split_string(std::vector &result, + const std::string &input, + const std::string &isAnyOf) +{ + std::string::size_type start = 0; + std::string::size_type end = input.find_first_of(isAnyOf, start); + while (end != std::string::npos) + { + result.push_back(input.substr(start, end-start)); + start = end + 1; + end = input.find_first_of(isAnyOf, start); + } + if (start < input.length()) + { + result.push_back(input.substr(start)); + } +} + +// This is a locale-safe version of string-to-double, which is suprisingly +// difficult to do correctly. This function ensures that the C locale is used +// for parsing, as that matches up with what the XSD for double specifies. +// On success, the double is returned; on failure, a std::runtime_error is +// thrown. +static inline double strToDouble(const char *in) +{ + std::stringstream ss; + ss.imbue(std::locale::classic()); + + ss << in; + + double out; + ss >> out; + + if (ss.fail() || !ss.eof()) { + throw std::runtime_error("Failed converting string to double"); + } + + return out; +} + +} + +#endif diff --git a/include/urdf_sensor/sensor.h b/include/urdf_sensor/sensor.h index 60126aa..2f82b48 100644 --- a/include/urdf_sensor/sensor.h +++ b/include/urdf_sensor/sensor.h @@ -1,13 +1,13 @@ /********************************************************************* * 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 @@ -17,7 +17,7 @@ * * 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 @@ -32,145 +32,15 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: John Hsu */ - -/* example - - - - - 1.5708 - - - - - - - - - - - - - - -*/ - - - #ifndef URDF_SENSOR_H #define URDF_SENSOR_H -#include -#include -#include -#include "urdf_model/pose.h" -#include "urdf_model/joint.h" -#include "urdf_model/link.h" -#include "urdf_model/types.h" -#include "urdf_sensor/types.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 - VisualSensorSharedPtr sensor; - - - /// Parent link element name. A pointer is stored in parent_link_. - std::string parent_link_name; - - LinkSharedPtr getParent() const - {return parent_link_.lock();}; - - void setParent(LinkSharedPtr parent) - { this->parent_link_ = parent; } - - void clear() - { - this->name.clear(); - this->sensor.reset(); - this->parent_link_name.clear(); - this->parent_link_.reset(); - }; +#if defined(_MSC_VER) + #pragma message("warning: urdf_sensor/sensor.h is deprecated. Please use urdf_sensor/sensor.hpp instead.") +#else + #warning urdf_sensor/sensor.h is deprecated. Please use urdf_sensor/sensor.hpp instead. +#endif -private: - LinkWeakPtr parent_link_; +#include -}; -} #endif diff --git a/include/urdf_sensor/sensor.hpp b/include/urdf_sensor/sensor.hpp new file mode 100644 index 0000000..b07b3e0 --- /dev/null +++ b/include/urdf_sensor/sensor.hpp @@ -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 + + + + + 1.5708 + + + + + + + + + + + + + + +*/ + + + +#ifndef URDF_SENSOR_HPP +#define URDF_SENSOR_HPP + +#include +#include +#include +#include "urdf_model/pose.hpp" +#include "urdf_model/joint.hpp" +#include "urdf_model/link.hpp" +#include "urdf_model/types.hpp" +#include "urdf_sensor/types.hpp" + +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 + VisualSensorSharedPtr sensor; + + + /// Parent link element name. A pointer is stored in parent_link_. + std::string parent_link_name; + + LinkSharedPtr getParent() const + {return parent_link_.lock();}; + + void setParent(LinkSharedPtr parent) + { this->parent_link_ = parent; } + + void clear() + { + this->name.clear(); + this->sensor.reset(); + this->parent_link_name.clear(); + this->parent_link_.reset(); + }; + +private: + LinkWeakPtr parent_link_; + +}; +} +#endif diff --git a/include/urdf_sensor/types.h b/include/urdf_sensor/types.h index aa55aec..6ea2ab7 100644 --- a/include/urdf_sensor/types.h +++ b/include/urdf_sensor/types.h @@ -32,21 +32,15 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Steve Peters */ - #ifndef URDF_SENSOR_TYPES_H #define URDF_SENSOR_TYPES_H -#include - - -namespace urdf{ - -class VisualSensor; - -// typedef shared pointers -typedef std::shared_ptr VisualSensorSharedPtr; +#if defined(_MSC_VER) + #pragma message("warning: urdf_sensor/types.h is deprecated. Please use urdf_sensor/types.hpp instead.") +#else + #warning urdf_sensor/types.h is deprecated. Please use urdf_sensor/types.hpp instead. +#endif -} +#include #endif diff --git a/include/urdf_sensor/types.hpp b/include/urdf_sensor/types.hpp new file mode 100644 index 0000000..dd73de7 --- /dev/null +++ b/include/urdf_sensor/types.hpp @@ -0,0 +1,52 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Steve Peters */ + +#ifndef URDF_SENSOR_TYPES_HPP +#define URDF_SENSOR_TYPES_HPP + +#include + + +namespace urdf{ + +class VisualSensor; + +// typedef shared pointers +typedef std::shared_ptr VisualSensorSharedPtr; + +} + +#endif diff --git a/include/urdf_world/types.h b/include/urdf_world/types.h index ae66721..5fbc6c5 100644 --- a/include/urdf_world/types.h +++ b/include/urdf_world/types.h @@ -32,17 +32,15 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Steve Peters */ - #ifndef URDF_WORLD_TYPES_H #define URDF_WORLD_TYPES_H #if defined(_MSC_VER) - #pragma message("warning: urdf_world/types.h is deprecated. Please use urdf_model/types.h instead.") + #pragma message("warning: urdf_world/types.h is deprecated. Please use urdf_model/types.hpp instead.") #else - #warning urdf_world/types.h is deprecated. Please use urdf_model/types.h instead. + #warning urdf_world/types.h is deprecated. Please use urdf_model/types.hpp instead. #endif -#include +#include #endif diff --git a/include/urdf_world/types.hpp b/include/urdf_world/types.hpp new file mode 100644 index 0000000..c2226b5 --- /dev/null +++ b/include/urdf_world/types.hpp @@ -0,0 +1,48 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Steve Peters */ + +#ifndef URDF_WORLD_TYPES_HPP +#define URDF_WORLD_TYPES_HPP + +#if defined(_MSC_VER) + #pragma message("warning: urdf_world/types.h is deprecated. Please use urdf_model/types.hpp instead.") +#else + #warning urdf_world/types.h is deprecated. Please use urdf_model/types.hpp instead. +#endif + +#include + +#endif diff --git a/include/urdf_world/world.h b/include/urdf_world/world.h index 909d599..d044aab 100644 --- a/include/urdf_world/world.h +++ b/include/urdf_world/world.h @@ -1,13 +1,13 @@ /********************************************************************* * 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 @@ -17,7 +17,7 @@ * * 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 @@ -32,77 +32,15 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: John Hsu */ - -/* encapsulates components in a world - see http://ros.org/wiki/usdf/XML/urdf_world - for details -*/ -/* example world XML - - - - - ... - - - - - - - - - - - - - - - - - - - -*/ - #ifndef URDF_WORLD_H #define URDF_WORLD_H -#include -#include -#include - -#include "urdf_model/types.h" -#include "urdf_model/pose.h" -#include "urdf_model/twist.h" - -namespace urdf{ - -class Entity -{ -public: - ModelInterfaceSharedPtr model; - Pose origin; - Twist twist; -}; - -class World -{ -public: - World() { this->clear(); }; - - /// world name must be unique - std::string name; - - std::vector models; +#if defined(_MSC_VER) + #pragma message("warning: urdf_world/world.h is deprecated. Please use urdf_world/world.hpp instead.") +#else + #warning urdf_world/world.h is deprecated. Please use urdf_world/world.hpp instead. +#endif - void clear() - { - this->name.clear(); - }; -}; -} +#include #endif diff --git a/include/urdf_world/world.hpp b/include/urdf_world/world.hpp new file mode 100644 index 0000000..3aabdbd --- /dev/null +++ b/include/urdf_world/world.hpp @@ -0,0 +1,108 @@ +/********************************************************************* +* 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 + for details +*/ +/* example world XML + + + + + ... + + + + + + + + + + + + + + + + + + + +*/ + +#ifndef URDF_WORLD_HPP +#define URDF_WORLD_HPP + +#include +#include +#include + +#include "urdf_model/types.hpp" +#include "urdf_model/pose.hpp" +#include "urdf_model/twist.hpp" + +namespace urdf{ + +class Entity +{ +public: + ModelInterfaceSharedPtr model; + Pose origin; + Twist twist; +}; + +class World +{ +public: + World() { this->clear(); }; + + /// world name must be unique + std::string name; + + std::vector models; + + void clear() + { + this->name.clear(); + }; +}; +} + +#endif