ros-urdfdom_headers-ff99bea/0000775000175000017500000000000015031020010016137 5ustar jriverojriveroros-urdfdom_headers-ff99bea/include/0000775000175000017500000000000015031020010017562 5ustar jriverojriveroros-urdfdom_headers-ff99bea/include/urdf_exception/0000775000175000017500000000000015031020010022600 5ustar jriverojriveroros-urdfdom_headers-ff99bea/include/urdf_exception/exception.h0000664000175000017500000000403015031020010024744 0ustar jriverojrivero/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // URDF exceptions #ifndef URDF_INTERFACE_EXCEPTION_H_ #define URDF_INTERFACE_EXCEPTION_H_ #include #include namespace urdf { class ParseError: public std::runtime_error { public: ParseError(const std::string &error_msg) : std::runtime_error(error_msg) {}; }; } #endif ros-urdfdom_headers-ff99bea/include/urdf_sensor/0000775000175000017500000000000015031020010022113 5ustar jriverojriveroros-urdfdom_headers-ff99bea/include/urdf_sensor/types.h0000664000175000017500000000373515031020010023440 0ustar jriverojrivero/********************************************************************* * 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_H #define URDF_SENSOR_TYPES_H #include namespace urdf{ class VisualSensor; // typedef shared pointers typedef std::shared_ptr VisualSensorSharedPtr; } #endif ros-urdfdom_headers-ff99bea/include/urdf_sensor/sensor.h0000664000175000017500000001065215031020010023601 0ustar jriverojrivero/********************************************************************* * 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_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(); }; private: LinkWeakPtr parent_link_; }; } #endif ros-urdfdom_headers-ff99bea/include/urdf_world/0000775000175000017500000000000015031020010021731 5ustar jriverojriveroros-urdfdom_headers-ff99bea/include/urdf_world/types.h0000664000175000017500000000374115031020010023253 0ustar jriverojrivero/********************************************************************* * 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_H #define URDF_WORLD_TYPES_H #include namespace urdf{ class ModelInterface; // typedef shared pointers typedef std::shared_ptr ModelInterfaceSharedPtr; } #endif ros-urdfdom_headers-ff99bea/include/urdf_world/world.h0000664000175000017500000000605415031020010023236 0ustar jriverojrivero/********************************************************************* * 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_H #define URDF_WORLD_H #include #include #include #include "urdf_model/model.h" #include "urdf_model/pose.h" #include "urdf_model/twist.h" #include "urdf_world/types.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; void clear() { this->name.clear(); }; }; } #endif ros-urdfdom_headers-ff99bea/include/urdf_model/0000775000175000017500000000000015031020010021702 5ustar jriverojriveroros-urdfdom_headers-ff99bea/include/urdf_model/joint.h0000664000175000017500000001451115031020010023200 0ustar jriverojrivero/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* Author: Wim Meeussen */ #ifndef URDF_INTERFACE_JOINT_H #define URDF_INTERFACE_JOINT_H #include #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; void clear() { lower = 0; upper = 0; effort = 0; velocity = 0; }; }; /// \brief Parameters for Joint Safety Controllers class JointSafety { public: /// clear variables on construction JointSafety() { this->clear(); }; /// /// IMPORTANT: The safety controller support is very much PR2 specific, not intended for generic usage. /// /// Basic safety controller operation is as follows /// /// current safety controllers will take effect on joints outside the position range below: /// /// position range: [JointSafety::soft_lower_limit + JointLimits::velocity / JointSafety::k_position, /// JointSafety::soft_uppper_limit - JointLimits::velocity / JointSafety::k_position] /// /// if (joint_position is outside of the position range above) /// velocity_limit_min = -JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_lower_limit) /// velocity_limit_max = JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_upper_limit) /// else /// velocity_limit_min = -JointLimits::velocity /// velocity_limit_max = JointLimits::velocity /// /// velocity range: [velocity_limit_min + JointLimits::effort / JointSafety::k_velocity, /// velocity_limit_max - JointLimits::effort / JointSafety::k_velocity] /// /// if (joint_velocity is outside of the velocity range above) /// effort_limit_min = -JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_min) /// effort_limit_max = JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_max) /// else /// effort_limit_min = -JointLimits::effort /// effort_limit_max = JointLimits::effort /// /// Final effort command sent to the joint is saturated by [effort_limit_min,effort_limit_max] /// /// Please see wiki for more details: http://www.ros.org/wiki/pr2_controller_manager/safety_limits /// double soft_upper_limit; double soft_lower_limit; double k_position; double k_velocity; void clear() { soft_upper_limit = 0; soft_lower_limit = 0; k_position = 0; k_velocity = 0; }; }; class JointCalibration { public: JointCalibration() { this->clear(); }; double reference_position; 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 ros-urdfdom_headers-ff99bea/include/urdf_model/model.h0000664000175000017500000001551215031020010023157 0ustar jriverojrivero/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* Author: Wim Meeussen */ #ifndef URDF_INTERFACE_MODEL_H #define URDF_INTERFACE_MODEL_H #include #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 ros-urdfdom_headers-ff99bea/include/urdf_model/types.h0000664000175000017500000000633315031020010023224 0ustar jriverojrivero/********************************************************************* * 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_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); // 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 ros-urdfdom_headers-ff99bea/include/urdf_model/pose.h0000664000175000017500000001676115031020010023034 0ustar jriverojrivero/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* Author: Wim Meeussen */ #ifndef URDF_INTERFACE_POSE_H #define URDF_INTERFACE_POSE_H #include #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 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 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 ros-urdfdom_headers-ff99bea/include/urdf_model/utils.h0000664000175000017500000000612515031020010023217 0ustar jriverojrivero/********************************************************************* * 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_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; } } #endif ros-urdfdom_headers-ff99bea/include/urdf_model/color.h0000664000175000017500000000606515031020010023200 0ustar jriverojrivero/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* Author: Josh Faust */ #ifndef URDF_INTERFACE_COLOR_H #define URDF_INTERFACE_COLOR_H #include #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 ros-urdfdom_headers-ff99bea/include/urdf_model/twist.h0000664000175000017500000000422615031020010023231 0ustar jriverojrivero/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* Author: John Hsu */ #ifndef URDF_TWIST_H #define URDF_TWIST_H #include #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 ros-urdfdom_headers-ff99bea/include/urdf_model/link.h0000664000175000017500000001216115031020010023011 0ustar jriverojrivero/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* Author: Wim Meeussen */ #ifndef URDF_INTERFACE_LINK_H #define URDF_INTERFACE_LINK_H #include #include #include #include "joint.h" #include "color.h" #include "types.h" namespace urdf{ class Geometry { public: enum {SPHERE, BOX, CYLINDER, MESH} type; virtual ~Geometry(void) { } }; class Sphere : public Geometry { public: Sphere() { this->clear(); type = SPHERE; }; double radius; void clear() { radius = 0; }; }; class Box : public Geometry { public: Box() { this->clear(); type = BOX; }; Vector3 dim; void clear() { this->dim.clear(); }; }; class Cylinder : public Geometry { public: Cylinder() { this->clear(); type = CYLINDER; }; double length; double radius; void clear() { length = 0; radius = 0; }; }; class Mesh : public Geometry { public: Mesh() { this->clear(); type = MESH; }; std::string filename; Vector3 scale; void clear() { filename.clear(); // default scale scale.x = 1; scale.y = 1; scale.z = 1; }; }; class Material { public: Material() { this->clear(); }; std::string name; std::string texture_filename; Color color; void clear() { color.clear(); texture_filename.clear(); name.clear(); }; }; class Inertial { public: Inertial() { this->clear(); }; Pose origin; double mass; double ixx,ixy,ixz,iyy,iyz,izz; void clear() { origin.clear(); mass = 0; ixx = ixy = ixz = iyy = iyz = izz = 0; }; }; class Visual { public: Visual() { this->clear(); }; Pose origin; 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 ros-urdfdom_headers-ff99bea/CMakeLists.txt0000664000175000017500000000647715031020010020715 0ustar jriverojriverocmake_minimum_required( VERSION 3.8 FATAL_ERROR ) project (urdfdom_headers) include(GNUInstallDirs) option(APPEND_PROJECT_NAME_TO_INCLUDEDIR "When ON headers are installed to a folder ending with an extra ${PROJECT_NAME}. \ This avoids include directory search order issues when overriding this package from a merged catkin, ament, or colcon workspace." ON) if(APPEND_PROJECT_NAME_TO_INCLUDEDIR) set(CMAKE_INSTALL_INCLUDEDIR "${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME}") endif() set (URDF_MAJOR_VERSION 2) set (URDF_MINOR_VERSION 0) set (URDF_PATCH_VERSION 0) set (URDF_VERSION ${URDF_MAJOR_VERSION}.${URDF_MINOR_VERSION}.${URDF_PATCH_VERSION}) message (STATUS "${PROJECT_NAME} version ${URDF_VERSION}") # This shouldn't be necessary, but there has been trouble # with MSVC being set off, but MSVCXX ON. if(MSVC OR MSVC90 OR MSVC10) set(MSVC ON) endif (MSVC OR MSVC90 OR MSVC10) install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) if(WIN32 AND NOT CYGWIN) set(CMAKE_CONFIG_INSTALL_DIR CMake) else() set(CMAKE_CONFIG_INSTALL_DIR ${CMAKE_INSTALL_LIBDIR}/${PROJECT_NAME}/cmake) endif() string(REGEX REPLACE "[^/]+" ".." RELATIVE_PATH_CMAKE_DIR_TO_PREFIX "${CMAKE_CONFIG_INSTALL_DIR}") string(REGEX REPLACE "[^/]+" ".." RELATIVE_PATH_LIBDIR_TO_PREFIX "${CMAKE_INSTALL_LIBDIR}") set(PACKAGE_NAME ${PROJECT_NAME}) set(cmake_conf_file "${PROJECT_NAME}-config.cmake") configure_file("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${cmake_conf_file}.in" "${CMAKE_BINARY_DIR}/${cmake_conf_file}" @ONLY) set(cmake_conf_version_file "${PROJECT_NAME}-config-version.cmake") # Use write_basic_package_version_file to generate a ConfigVersion file that # allow users of gazebo to specify the API or version to depend on # TODO: keep this instruction until deprecate Ubuntu/Precise and update with # https://github.com/Kitware/CMake/blob/v2.8.8/Modules/CMakePackageConfigHelpers.cmake include(WriteBasicConfigVersionFile) write_basic_config_version_file( ${CMAKE_CURRENT_BINARY_DIR}/${cmake_conf_version_file} VERSION "${URDF_VERSION}" COMPATIBILITY SameMajorVersion) install(FILES "${CMAKE_BINARY_DIR}/${cmake_conf_file}" "${CMAKE_BINARY_DIR}/${cmake_conf_version_file}" DESTINATION ${CMAKE_CONFIG_INSTALL_DIR} COMPONENT cmake) # Make the package config file set(PACKAGE_DESC "Unified Robot Description Format") set(pkg_conf_file "urdfdom_headers.pc") configure_file("${CMAKE_CURRENT_SOURCE_DIR}/cmake/pkgconfig/${pkg_conf_file}.in" "${CMAKE_BINARY_DIR}/${pkg_conf_file}" @ONLY) install(FILES "${CMAKE_BINARY_DIR}/${pkg_conf_file}" DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig/ COMPONENT pkgconfig) add_library(${PROJECT_NAME} INTERFACE) target_include_directories(${PROJECT_NAME} INTERFACE "$") install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}) install( EXPORT ${PROJECT_NAME} DESTINATION ${CMAKE_CONFIG_INSTALL_DIR} NAMESPACE "${PROJECT_NAME}::" FILE "${PROJECT_NAME}Export.cmake" ) # Add uninstall target # Ref: http://www.cmake.org/Wiki/CMake_FAQ#Can_I_do_.22make_uninstall.22_with_CMake.3F configure_file("${PROJECT_SOURCE_DIR}/cmake/uninstall.cmake.in" "${PROJECT_BINARY_DIR}/uninstall.cmake" IMMEDIATE @ONLY) add_custom_target(uninstall "${CMAKE_COMMAND}" -P "${PROJECT_BINARY_DIR}/uninstall.cmake") message(STATUS "Configuration successful. Type make install to install urdfdom_headers.") ros-urdfdom_headers-ff99bea/package.xml0000664000175000017500000000105015031020010020250 0ustar jriverojrivero urdfdom_headers 2.0.0 C++ headers for URDF. Steven! Ragnarök BSD http://ros.org/wiki/urdf cmake cmake ros-urdfdom_headers-ff99bea/cmake/0000775000175000017500000000000015031020010017217 5ustar jriverojriveroros-urdfdom_headers-ff99bea/cmake/pkgconfig/0000775000175000017500000000000015031020010021166 5ustar jriverojriveroros-urdfdom_headers-ff99bea/cmake/pkgconfig/urdfdom_headers.pc.in0000664000175000017500000000044015031020010025250 0ustar jriverojrivero# This file was generated by CMake for @PROJECT_NAME@ prefix=${pcfiledir}/../@RELATIVE_PATH_LIBDIR_TO_PREFIX@ exec_prefix=${prefix} includedir=${prefix}/@CMAKE_INSTALL_INCLUDEDIR@ Name: @PACKAGE_NAME@ Description: @PACKAGE_DESC@ Version: @URDF_VERSION@ Requires: Cflags: -I${includedir} ros-urdfdom_headers-ff99bea/cmake/urdfdom_headers-config.cmake.in0000664000175000017500000000053215031020010025224 0ustar jriverojriveroif (@PACKAGE_NAME@_CONFIG_INCLUDED) return() endif() set(@PACKAGE_NAME@_CONFIG_INCLUDED TRUE) set(@PACKAGE_NAME@_INCLUDE_DIRS "${@PROJECT_NAME@_DIR}/@RELATIVE_PATH_CMAKE_DIR_TO_PREFIX@/@CMAKE_INSTALL_INCLUDEDIR@") include("${@PACKAGE_NAME@_DIR}/@PACKAGE_NAME@Export.cmake") list(APPEND @PACKAGE_NAME@_TARGETS @PACKAGE_NAME@::@PACKAGE_NAME@) ros-urdfdom_headers-ff99bea/cmake/uninstall.cmake.in0000664000175000017500000000212415031020010022636 0ustar jriverojrivero# Ref: http://www.cmake.org/Wiki/CMake_FAQ#Can_I_do_.22make_uninstall.22_with_CMake.3F if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") message(FATAL_ERROR "Cannot find install manifest: @CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files) string(REGEX REPLACE "\n" ";" files "${files}") foreach(file ${files}) message(STATUS "Uninstalling $ENV{DESTDIR}${file}") if(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") exec_program("@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\"" OUTPUT_VARIABLE rm_out RETURN_VALUE rm_retval) if(NOT "${rm_retval}" STREQUAL 0) message(FATAL_ERROR "Problem when removing $ENV{DESTDIR}${file}") endif(NOT "${rm_retval}" STREQUAL 0) else(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") message(STATUS "File $ENV{DESTDIR}${file} does not exist.") endif(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") endforeach(file) ros-urdfdom_headers-ff99bea/LICENSE0000664000175000017500000000335615031020010017153 0ustar jriverojrivero/********************************************************************* * 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. *********************************************************************/ ros-urdfdom_headers-ff99bea/README.md0000664000175000017500000000032215031020010017413 0ustar jriverojriverourdfdom_headers =============== The URDF (U-Robot Description Format) headers provides core data structure headers for URDF. For now, the details of the URDF specifications reside on http://ros.org/wiki/urdf