urdfdom_headers-1.0.4/0000775000175000017500000000000013470634053014600 5ustar jriverojriverourdfdom_headers-1.0.4/urdf_exception/0000775000175000017500000000000013470634053017616 5ustar jriverojriverourdfdom_headers-1.0.4/urdf_exception/CMakeLists.txt0000664000175000017500000000007613470634053022361 0ustar jriverojriveroINSTALL(DIRECTORY include/urdf_exception DESTINATION include) urdfdom_headers-1.0.4/urdf_exception/include/0000775000175000017500000000000013470634053021241 5ustar jriverojriverourdfdom_headers-1.0.4/urdf_exception/include/urdf_exception/0000775000175000017500000000000013470634053024257 5ustar jriverojriverourdfdom_headers-1.0.4/urdf_exception/include/urdf_exception/exception.h0000664000175000017500000000403013470634053026423 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 urdfdom_headers-1.0.4/urdf_model/0000775000175000017500000000000013470634053016720 5ustar jriverojriverourdfdom_headers-1.0.4/urdf_model/CMakeLists.txt0000664000175000017500000000007213470634053021457 0ustar jriverojriveroINSTALL(DIRECTORY include/urdf_model DESTINATION include) urdfdom_headers-1.0.4/urdf_model/include/0000775000175000017500000000000013470634053020343 5ustar jriverojriverourdfdom_headers-1.0.4/urdf_model/include/urdf_model/0000775000175000017500000000000013470634053022463 5ustar jriverojriverourdfdom_headers-1.0.4/urdf_model/include/urdf_model/pose.h0000664000175000017500000001554613470634053023615 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() { double s = sqrt(this->x * this->x + this->y * this->y + this->z * this->z + this->w * this->w); if (s == 0.0) { this->x = 0.0; this->y = 0.0; this->z = 0.0; this->w = 1.0; } else { this->x /= s; this->y /= s; this->z /= s; this->w /= s; } }; // Multiplication operator (copied from gazebo) Rotation operator*( const Rotation &qt ) const { Rotation c; c.x = this->w * qt.x + this->x * qt.w + this->y * qt.z - this->z * qt.y; c.y = this->w * qt.y - this->x * qt.z + this->y * qt.w + this->z * qt.x; c.z = this->w * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.w; c.w = this->w * qt.w - this->x * qt.x - this->y * qt.y - this->z * qt.z; return c; }; /// Rotate a vector using the quaternion Vector3 operator*(Vector3 vec) const { Rotation tmp; Vector3 result; tmp.w = 0.0; tmp.x = vec.x; tmp.y = vec.y; tmp.z = vec.z; tmp = (*this) * (tmp * this->GetInverse()); result.x = tmp.x; result.y = tmp.y; result.z = tmp.z; return result; }; // Get the inverse of this quaternion Rotation GetInverse() const { Rotation q; double norm = this->w*this->w+this->x*this->x+this->y*this->y+this->z*this->z; if (norm > 0.0) { q.w = this->w / norm; q.x = -this->x / norm; q.y = -this->y / norm; q.z = -this->z / norm; } return q; }; }; class Pose { public: Pose() { this->clear(); }; Vector3 position; Rotation rotation; void clear() { this->position.clear(); this->rotation.clear(); }; }; } #endif urdfdom_headers-1.0.4/urdf_model/include/urdf_model/twist.h0000664000175000017500000000422613470634053024012 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 urdfdom_headers-1.0.4/urdf_model/include/urdf_model/model.h0000664000175000017500000001551213470634053023740 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 urdfdom_headers-1.0.4/urdf_model/include/urdf_model/joint.h0000664000175000017500000001451113470634053023761 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 urdfdom_headers-1.0.4/urdf_model/include/urdf_model/color.h0000664000175000017500000000606513470634053023761 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 urdfdom_headers-1.0.4/urdf_model/include/urdf_model/types.h0000664000175000017500000000633313470634053024005 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 urdfdom_headers-1.0.4/urdf_model/include/urdf_model/link.h0000664000175000017500000001216113470634053023572 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 urdfdom_headers-1.0.4/urdf_model/include/urdf_model/utils.h0000664000175000017500000000612513470634053024000 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 urdfdom_headers-1.0.4/CMakeLists.txt0000664000175000017500000000500413470634053017337 0ustar jriverojriverocmake_minimum_required( VERSION 2.8 FATAL_ERROR ) project (urdfdom_headers) include(GNUInstallDirs) set (URDF_MAJOR_VERSION 1) set (URDF_MINOR_VERSION 0) set (URDF_PATCH_VERSION 4) 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) add_subdirectory(urdf_sensor) add_subdirectory(urdf_model) add_subdirectory(urdf_model_state) add_subdirectory(urdf_world) add_subdirectory(urdf_exception) if(WIN32 AND NOT CYGWIN) set(CMAKE_CONFIG_INSTALL_DIR CMake) else() set(CMAKE_CONFIG_INSTALL_DIR ${CMAKE_INSTALL_FULL_LIBDIR}/${PROJECT_NAME}/cmake/) endif() 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 if (NOT MSVC) 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 lib/pkgconfig/ COMPONENT pkgconfig) endif() # 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.") urdfdom_headers-1.0.4/urdf_model_state/0000775000175000017500000000000013470634053020120 5ustar jriverojriverourdfdom_headers-1.0.4/urdf_model_state/CMakeLists.txt0000664000175000017500000000010013470634053022647 0ustar jriverojriveroINSTALL(DIRECTORY include/urdf_model_state DESTINATION include) urdfdom_headers-1.0.4/urdf_model_state/include/0000775000175000017500000000000013470634053021543 5ustar jriverojriverourdfdom_headers-1.0.4/urdf_model_state/include/urdf_model_state/0000775000175000017500000000000013470634053025063 5ustar jriverojriverourdfdom_headers-1.0.4/urdf_model_state/include/urdf_model_state/twist.h0000664000175000017500000000361313470634053026411 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. *********************************************************************/ #ifndef URDF_MODEL_STATE_TWIST_ #define URDF_MODEL_STATE_TWIST_ #warning "Please Use #include " #include #endif urdfdom_headers-1.0.4/urdf_model_state/include/urdf_model_state/types.h0000664000175000017500000000374113470634053026405 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_STATE_TYPES_H #define URDF_MODEL_STATE_TYPES_H #include namespace urdf{ class JointState; // typedef shared pointers typedef std::shared_ptr JointStateSharedPtr; } #endif urdfdom_headers-1.0.4/urdf_model_state/include/urdf_model_state/model_state.h0000664000175000017500000000705613470634053027544 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_MODEL_STATE_H #define URDF_MODEL_STATE_H #include #include #include #include "urdf_model/pose.h" #include #include "urdf_model_state/types.h" namespace urdf{ //round is not defined in C++98 //So in Visual Studio <= 2012 is necessary to define it #ifdef _MSC_VER #if (_MSC_VER <= 1700) double round(double value) { return (value >= 0.0f)?(floor(value + 0.5f)):(ceil(value - 0.5f)); } #endif #endif class Time { public: Time() { this->clear(); }; void set(double _seconds) { this->sec = (int32_t)(floor(_seconds)); this->nsec = (int32_t)(round((_seconds - this->sec) * 1e9)); this->Correct(); }; operator double () { return (static_cast(this->sec) + static_cast(this->nsec)*1e-9); }; int32_t sec; int32_t nsec; void clear() { this->sec = 0; this->nsec = 0; }; private: void Correct() { // Make any corrections if (this->nsec >= 1e9) { this->sec++; this->nsec = (int32_t)(this->nsec - 1e9); } else if (this->nsec < 0) { this->sec--; this->nsec = (int32_t)(this->nsec + 1e9); } }; }; class JointState { public: JointState() { this->clear(); }; /// joint name std::string joint; std::vector position; std::vector velocity; std::vector effort; void clear() { this->joint.clear(); this->position.clear(); this->velocity.clear(); this->effort.clear(); } }; class ModelState { public: ModelState() { this->clear(); }; /// state name must be unique std::string name; Time time_stamp; void clear() { this->name.clear(); this->time_stamp.set(0); this->joint_states.clear(); }; std::vector joint_states; }; } #endif urdfdom_headers-1.0.4/LICENSE0000664000175000017500000000335613470634053015614 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. *********************************************************************/ urdfdom_headers-1.0.4/urdf_world/0000775000175000017500000000000013470634053016747 5ustar jriverojriverourdfdom_headers-1.0.4/urdf_world/CMakeLists.txt0000664000175000017500000000007213470634053021506 0ustar jriverojriveroINSTALL(DIRECTORY include/urdf_world DESTINATION include) urdfdom_headers-1.0.4/urdf_world/include/0000775000175000017500000000000013470634053020372 5ustar jriverojriverourdfdom_headers-1.0.4/urdf_world/include/urdf_world/0000775000175000017500000000000013470634053022541 5ustar jriverojriverourdfdom_headers-1.0.4/urdf_world/include/urdf_world/world.h0000664000175000017500000000605413470634053024046 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 urdfdom_headers-1.0.4/urdf_world/include/urdf_world/types.h0000664000175000017500000000374113470634053024063 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 urdfdom_headers-1.0.4/README.md0000664000175000017500000000170413470634053016061 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 ### Build Status [![Build Status](https://travis-ci.org/ros/urdfdom_headers.png)](https://travis-ci.org/ros/urdfdom_headers) ### Using with ROS If you choose to check this repository out for use with ROS, be aware that the necessary ``package.xml`` is not included in this repo but instead is added in during the ROS release process. To emulate this, pull the appropriate file into this repository using the following format. Be sure to replace the ALLCAPS words with the appropriate terms: ``` wget https://raw.github.com/ros-gbp/urdfdom_headers-release/debian/ROS_DISTRO/UBUNTU_DISTRO/urdfdom_headers/package.xml ``` For example: ``` wget https://raw.github.com/ros-gbp/urdfdom_headers-release/debian/hydro/precise/urdfdom_headers/package.xml ``` urdfdom_headers-1.0.4/cmake/0000775000175000017500000000000013470634053015660 5ustar jriverojriverourdfdom_headers-1.0.4/cmake/pkgconfig/0000775000175000017500000000000013470634053017627 5ustar jriverojriverourdfdom_headers-1.0.4/cmake/pkgconfig/urdfdom_headers.pc.in0000664000175000017500000000036313470634053023715 0ustar jriverojrivero# This file was generated by CMake for @PROJECT_NAME@ prefix=@CMAKE_INSTALL_PREFIX@ exec_prefix=${prefix} includedir=${prefix}/include Name: @PACKAGE_NAME@ Description: @PACKAGE_DESC@ Version: @URDF_VERSION@ Requires: Cflags: -I${includedir} urdfdom_headers-1.0.4/cmake/urdfdom_headers-config.cmake.in0000664000175000017500000000024313470634053023664 0ustar jriverojriveroif (@PACKAGE_NAME@_CONFIG_INCLUDED) return() endif() set(@PACKAGE_NAME@_CONFIG_INCLUDED TRUE) set(@PACKAGE_NAME@_INCLUDE_DIRS "@CMAKE_INSTALL_PREFIX@/include") urdfdom_headers-1.0.4/cmake/uninstall.cmake.in0000664000175000017500000000212413470634053021277 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) urdfdom_headers-1.0.4/urdf_sensor/0000775000175000017500000000000013470634053017131 5ustar jriverojriverourdfdom_headers-1.0.4/urdf_sensor/CMakeLists.txt0000664000175000017500000000007313470634053021671 0ustar jriverojriveroINSTALL(DIRECTORY include/urdf_sensor DESTINATION include) urdfdom_headers-1.0.4/urdf_sensor/include/0000775000175000017500000000000013470634053020554 5ustar jriverojriverourdfdom_headers-1.0.4/urdf_sensor/include/urdf_sensor/0000775000175000017500000000000013470634053023105 5ustar jriverojriverourdfdom_headers-1.0.4/urdf_sensor/include/urdf_sensor/types.h0000664000175000017500000000373513470634053024432 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 urdfdom_headers-1.0.4/urdf_sensor/include/urdf_sensor/sensor.h0000664000175000017500000001065213470634053024573 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