diff --git a/urdf/include/urdf/color.h b/urdf/include/urdf/color.h index b3fb9dd..fe71518 100644 --- a/urdf/include/urdf/color.h +++ b/urdf/include/urdf/color.h @@ -1 +1,3 @@ +#pragma message("WARNING: Including urdf/color.h is deprecated. Include urdf_interface/color.h instead") + #include "urdf_parser/color.h" diff --git a/urdf/include/urdf/joint.h b/urdf/include/urdf/joint.h index eefc258..1eb5939 100644 --- a/urdf/include/urdf/joint.h +++ b/urdf/include/urdf/joint.h @@ -1 +1,3 @@ +#pragma message("WARNING: Including urdf/joint.h is deprecated. Include urdf_interface/joint.h instead") + #include "urdf_parser/joint.h" diff --git a/urdf/include/urdf/link.h b/urdf/include/urdf/link.h index aa69f0f..1a8f1f5 100644 --- a/urdf/include/urdf/link.h +++ b/urdf/include/urdf/link.h @@ -1 +1,3 @@ +#pragma message("WARNING: Including urdf/link.h is deprecated. Include urdf_interface/link.h instead") + #include "urdf_parser/link.h" diff --git a/urdf/include/urdf/pose.h b/urdf/include/urdf/pose.h index 3ed0115..7c285e1 100644 --- a/urdf/include/urdf/pose.h +++ b/urdf/include/urdf/pose.h @@ -1 +1,3 @@ +#pragma message("WARNING: Including urdf/pose.h is deprecated. Include urdf_interface/pose.h instead") + #include "urdf_parser/pose.h" diff --git a/urdf_parser/include/urdf_parser/color.h b/urdf_parser/include/urdf_parser/color.h deleted file mode 100644 index c132da4..0000000 --- a/urdf_parser/include/urdf_parser/color.h +++ /dev/null @@ -1,104 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Josh Faust */ - -#ifndef URDF_COLOR_H -#define URDF_COLOR_H - -#include -#include -#include -#include -#include -#include - -namespace urdf -{ - -class Color -{ -public: - Color() {this->clear();}; - float r; - float g; - float b; - float a; - - void clear() - { - r = g = b = 0.0f; - a = 1.0f; - } - bool init(const std::string &vector_str) - { - this->clear(); - std::vector pieces; - std::vector rgba; - boost::split( pieces, vector_str, boost::is_any_of(" ")); - for (unsigned int i = 0; i < pieces.size(); ++i) - { - if (!pieces[i].empty()) - { - try - { - rgba.push_back(boost::lexical_cast(pieces[i].c_str())); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("color rgba element (%s) is not a valid float",pieces[i].c_str()); - return false; - } - } - } - - if (rgba.size() != 4) - { - ROS_ERROR("Color contains %i elements instead of 4 elements", (int)rgba.size()); - return false; - } - - this->r = rgba[0]; - this->g = rgba[1]; - this->b = rgba[2]; - this->a = rgba[3]; - - return true; - }; -}; - -} - -#endif - diff --git a/urdf_parser/include/urdf_parser/joint.h b/urdf_parser/include/urdf_parser/joint.h deleted file mode 100644 index 551e2c6..0000000 --- a/urdf_parser/include/urdf_parser/joint.h +++ /dev/null @@ -1,235 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#ifndef URDF_JOINT_H -#define URDF_JOINT_H - -#include -#include -#include -#include - -#include "pose.h" - -namespace urdf{ - -class Link; - -class JointDynamics -{ -public: - JointDynamics() { this->clear(); }; - double damping; - double friction; - - void clear() - { - damping = 0; - friction = 0; - }; - bool initXml(TiXmlElement* config); -}; - -class JointLimits -{ -public: - JointLimits() { this->clear(); }; - double lower; - double upper; - double effort; - double velocity; - - void clear() - { - lower = 0; - upper = 0; - effort = 0; - velocity = 0; - }; - bool initXml(TiXmlElement* config); -}; - -/// \brief Parameters for Joint Safety Controllers -class JointSafety -{ -public: - /// clear variables on construction - JointSafety() { this->clear(); }; - - /// - /// IMPORTANT: The safety controller support is very much PR2 specific, not intended for generic usage. - /// - /// Basic safety controller operation is as follows - /// - /// current safety controllers will take effect on joints outside the position range below: - /// - /// position range: [JointSafety::soft_lower_limit + JointLimits::velocity / JointSafety::k_position, - /// JointSafety::soft_uppper_limit - JointLimits::velocity / JointSafety::k_position] - /// - /// if (joint_position is outside of the position range above) - /// velocity_limit_min = -JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_lower_limit) - /// velocity_limit_max = JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_upper_limit) - /// else - /// velocity_limit_min = -JointLimits::velocity - /// velocity_limit_max = JointLimits::velocity - /// - /// velocity range: [velocity_limit_min + JointLimits::effort / JointSafety::k_velocity, - /// velocity_limit_max - JointLimits::effort / JointSafety::k_velocity] - /// - /// if (joint_velocity is outside of the velocity range above) - /// effort_limit_min = -JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_min) - /// effort_limit_max = JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_max) - /// else - /// effort_limit_min = -JointLimits::effort - /// effort_limit_max = JointLimits::effort - /// - /// Final effort command sent to the joint is saturated by [effort_limit_min,effort_limit_max] - /// - /// Please see wiki for more details: http://www.ros.org/wiki/pr2_controller_manager/safety_limits - /// - double soft_upper_limit; - double soft_lower_limit; - double k_position; - double k_velocity; - - void clear() - { - soft_upper_limit = 0; - soft_lower_limit = 0; - k_position = 0; - k_velocity = 0; - }; - bool initXml(TiXmlElement* config); -}; - - -class JointCalibration -{ -public: - JointCalibration() { this->clear(); }; - double reference_position; - boost::shared_ptr rising, falling; - - void clear() - { - reference_position = 0; - }; - bool initXml(TiXmlElement* config); -}; - -class JointMimic -{ -public: - JointMimic() { this->clear(); }; - double offset; - double multiplier; - std::string joint_name; - - void clear() - { - offset = 0; - multiplier = 0; - joint_name.clear(); - }; - bool initXml(TiXmlElement* config); -}; - - -class Joint -{ -public: - - Joint() { this->clear(); }; - - std::string name; - enum - { - UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED - } type; - - /// \brief type_ meaning of axis_ - /// ------------------------------------------------------ - /// UNKNOWN unknown type - /// REVOLUTE rotation axis - /// PRISMATIC translation axis - /// FLOATING N/A - /// PLANAR plane normal axis - /// FIXED N/A - Vector3 axis; - - /// child Link element - /// child link frame is the same as the Joint frame - std::string child_link_name; - - /// parent Link element - /// origin specifies the transform from Parent Link to Joint Frame - std::string parent_link_name; - /// transform from Parent Link frame to Joint frame - Pose parent_to_joint_origin_transform; - - /// Joint Dynamics - boost::shared_ptr dynamics; - - /// Joint Limits - boost::shared_ptr limits; - - /// Unsupported Hidden Feature - boost::shared_ptr safety; - - /// Unsupported Hidden Feature - boost::shared_ptr calibration; - - /// Option to Mimic another Joint - boost::shared_ptr mimic; - - bool initXml(TiXmlElement* xml); - void clear() - { - this->axis.clear(); - this->child_link_name.clear(); - this->parent_link_name.clear(); - this->parent_to_joint_origin_transform.clear(); - this->dynamics.reset(); - this->limits.reset(); - this->safety.reset(); - this->calibration.reset(); - this->type = UNKNOWN; - }; -}; - -} - -#endif diff --git a/urdf_parser/include/urdf_parser/link.h b/urdf_parser/include/urdf_parser/link.h deleted file mode 100644 index 96f4528..0000000 --- a/urdf_parser/include/urdf_parser/link.h +++ /dev/null @@ -1,259 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#ifndef URDF_LINK_H -#define URDF_LINK_H - -#include -#include -#include -#include - -#include "joint.h" -#include "color.h" - -namespace urdf{ - -class Geometry -{ -public: - enum {SPHERE, BOX, CYLINDER, MESH} type; - - virtual bool initXml(TiXmlElement *) = 0; - -}; - -class Sphere : public Geometry -{ -public: - Sphere() { this->clear(); }; - double radius; - - void clear() - { - radius = 0; - }; - bool initXml(TiXmlElement *); -}; - -class Box : public Geometry -{ -public: - Box() { this->clear(); }; - Vector3 dim; - - void clear() - { - dim.clear(); - }; - bool initXml(TiXmlElement *); -}; - -class Cylinder : public Geometry -{ -public: - Cylinder() { this->clear(); }; - double length; - double radius; - - void clear() - { - length = 0; - radius = 0; - }; - bool initXml(TiXmlElement *); -}; - -class Mesh : public Geometry -{ -public: - Mesh() { this->clear(); }; - std::string filename; - Vector3 scale; - - void clear() - { - filename.clear(); - // default scale - scale.x = 1; - scale.y = 1; - scale.z = 1; - }; - bool initXml(TiXmlElement *); - bool fileExists(std::string filename); -}; - -class Material -{ -public: - Material() { this->clear(); }; - std::string name; - std::string texture_filename; - Color color; - - void clear() - { - color.clear(); - texture_filename.clear(); - name.clear(); - }; - bool initXml(TiXmlElement* config); -}; - -class Inertial -{ -public: - Inertial() { this->clear(); }; - Pose origin; - double mass; - double ixx,ixy,ixz,iyy,iyz,izz; - - void clear() - { - origin.clear(); - mass = 0; - ixx = ixy = ixz = iyy = iyz = izz = 0; - }; - bool initXml(TiXmlElement* config); -}; - -class Visual -{ -public: - Visual() { this->clear(); }; - Pose origin; - boost::shared_ptr geometry; - - std::string material_name; - boost::shared_ptr material; - - void clear() - { - origin.clear(); - material_name.clear(); - material.reset(); - geometry.reset(); - this->group_name.clear(); - }; - bool initXml(TiXmlElement* config); - std::string group_name; -}; - -class Collision -{ -public: - Collision() { this->clear(); }; - Pose origin; - boost::shared_ptr geometry; - - void clear() - { - origin.clear(); - geometry.reset(); - this->group_name.clear(); - }; - bool initXml(TiXmlElement* config); - std::string group_name; -}; - - -class Link -{ -public: - Link() { this->clear(); }; - - std::string name; - - /// inertial element - boost::shared_ptr inertial; - - /// visual element - boost::shared_ptr visual; - - /// collision element - boost::shared_ptr collision; - - /// a collection of visual elements, keyed by a string tag called "group" - std::map > > > visual_groups; - - /// a collection of collision elements, keyed by a string tag called "group" - std::map > > > collision_groups; - - /// Parent Joint element - /// explicitly stating "parent" because we want directional-ness for tree structure - /// every link can have one parent - boost::shared_ptr parent_joint; - - std::vector > child_joints; - std::vector > child_links; - - bool initXml(TiXmlElement* config); - - boost::shared_ptr getParent() const - {return parent_link_.lock();}; - - void setParent(boost::shared_ptr parent); - - void clear() - { - this->name.clear(); - this->inertial.reset(); - this->visual.reset(); - this->collision.reset(); - this->parent_joint.reset(); - this->child_joints.clear(); - this->child_links.clear(); - this->collision_groups.clear(); - }; - void setParentJoint(boost::shared_ptr child); - void addChild(boost::shared_ptr child); - void addChildJoint(boost::shared_ptr child); - - void addVisual(std::string group_name, boost::shared_ptr visual); - boost::shared_ptr > > getVisuals(const std::string& group_name) const; - void addCollision(std::string group_name, boost::shared_ptr collision); - boost::shared_ptr > > getCollisions(const std::string& group_name) const; -private: - boost::weak_ptr parent_link_; - -}; - - - - -} - -#endif diff --git a/urdf_parser/include/urdf_parser/parser.h b/urdf_parser/include/urdf_parser/parser.h index 7ce0e84..1293330 100644 --- a/urdf_parser/include/urdf_parser/parser.h +++ b/urdf_parser/include/urdf_parser/parser.h @@ -41,7 +41,7 @@ #include #include #include -#include "urdf_parser/link.h" +#include namespace urdf{ diff --git a/urdf_parser/include/urdf_parser/pose.h b/urdf_parser/include/urdf_parser/pose.h deleted file mode 100644 index 9d8bd79..0000000 --- a/urdf_parser/include/urdf_parser/pose.h +++ /dev/null @@ -1,306 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#ifndef URDF_POSE_H -#define URDF_POSE_H - -#include -#include -#include -#include -#include -#include - -namespace urdf{ - -class Vector3 -{ -public: - Vector3(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;}; - Vector3() {this->clear();}; - double x; - double y; - double z; - - void clear() {this->x=this->y=this->z=0.0;}; - bool init(const std::string &vector_str) - { - this->clear(); - std::vector pieces; - std::vector xyz; - boost::split( pieces, vector_str, boost::is_any_of(" ")); - for (unsigned int i = 0; i < pieces.size(); ++i){ - if (pieces[i] != ""){ - try - { - xyz.push_back(boost::lexical_cast(pieces[i].c_str())); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("Vector3 xyz element (%s) is not a valid float",pieces[i].c_str()); - return false; - } - } - } - - if (xyz.size() != 3) { - ROS_ERROR("Vector contains %i elements instead of 3 elements", (int)xyz.size()); - return false; - } - - this->x = xyz[0]; - this->y = xyz[1]; - this->z = xyz[2]; - - return true; - }; - Vector3 operator+(Vector3 vec) - { - return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z); - }; -}; - -class Rotation -{ -public: - Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;}; - Rotation() {this->clear();}; - void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const - { - quat_x = this->x; - quat_y = this->y; - quat_z = this->z; - quat_w = this->w; - }; - void getRPY(double &roll,double &pitch,double &yaw) const - { - double sqw; - double sqx; - double sqy; - double sqz; - - sqx = this->x * this->x; - sqy = this->y * this->y; - sqz = this->z * this->z; - sqw = this->w * this->w; - - roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz); - double sarg = -2 * (this->x*this->z - this->w*this->y); - pitch = sarg <= -1.0 ? -0.5*M_PI : (sarg >= 1.0 ? 0.5*M_PI : asin(sarg)); - yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz); - - }; - void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w) - { - this->x = quat_x; - this->y = quat_y; - this->z = quat_z; - this->w = quat_w; - this->normalize(); - }; - void setFromRPY(double roll, double pitch, double yaw) - { - double phi, the, psi; - - phi = roll / 2.0; - the = pitch / 2.0; - psi = yaw / 2.0; - - this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi); - this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi); - this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi); - this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi); - - this->normalize(); - }; - - double x,y,z,w; - - bool init(const std::string &rotation_str) - { - this->clear(); - - Vector3 rpy; - - if (!rpy.init(rotation_str)) - return false; - else - { - this->setFromRPY(rpy.x,rpy.y,rpy.z); - return true; - } - - }; - - void clear() { this->x=this->y=this->z=0.0;this->w=1.0; } - - void normalize() - { - double s = sqrt(this->x * this->x + - this->y * this->y + - this->z * this->z + - this->w * this->w); - if (s == 0.0) - { - this->x = 0.0; - this->y = 0.0; - this->z = 0.0; - this->w = 1.0; - } - else - { - this->x /= s; - this->y /= s; - this->z /= s; - this->w /= s; - } - }; - - // Multiplication operator (copied from gazebo) - Rotation operator*( const Rotation &qt ) const - { - Rotation c; - - c.x = this->w * qt.x + this->x * qt.w + this->y * qt.z - this->z * qt.y; - c.y = this->w * qt.y - this->x * qt.z + this->y * qt.w + this->z * qt.x; - c.z = this->w * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.w; - c.w = this->w * qt.w - this->x * qt.x - this->y * qt.y - this->z * qt.z; - - return c; - }; - /// Rotate a vector using the quaternion - Vector3 operator*(Vector3 vec) const - { - Rotation tmp; - Vector3 result; - - tmp.w = 0.0; - tmp.x = vec.x; - tmp.y = vec.y; - tmp.z = vec.z; - - tmp = (*this) * (tmp * this->GetInverse()); - - result.x = tmp.x; - result.y = tmp.y; - result.z = tmp.z; - - return result; - }; - // Get the inverse of this quaternion - Rotation GetInverse() const - { - Rotation q; - - double norm = this->w*this->w+this->x*this->x+this->y*this->y+this->z*this->z; - - if (norm > 0.0) - { - q.w = this->w / norm; - q.x = -this->x / norm; - q.y = -this->y / norm; - q.z = -this->z / norm; - } - - return q; - }; - - -}; - -class Pose -{ -public: - Pose() { this->clear(); }; - - Vector3 position; - Rotation rotation; - - void clear() - { - this->position.clear(); - this->rotation.clear(); - }; - bool initXml(TiXmlElement* xml) - { - this->clear(); - if (!xml) - { - ROS_DEBUG("parsing pose: xml empty"); - return false; - } - else - { - const char* xyz_str = xml->Attribute("xyz"); - if (xyz_str == NULL) - { - ROS_DEBUG("parsing pose: no xyz, using default values."); - return true; - } - else - { - if (!this->position.init(xyz_str)) - { - ROS_ERROR("malformed xyz"); - this->position.clear(); - return false; - } - } - - const char* rpy_str = xml->Attribute("rpy"); - if (rpy_str == NULL) - { - ROS_DEBUG("parsing pose: no rpy, using default values."); - return true; - } - else - { - if (!this->rotation.init(rpy_str)) - { - ROS_ERROR("malformed rpy"); - return false; - this->rotation.clear(); - } - } - - return true; - } - }; -}; - -} - -#endif diff --git a/urdf_parser/manifest.xml b/urdf_parser/manifest.xml index 6f67aaa..fa19cf4 100644 --- a/urdf_parser/manifest.xml +++ b/urdf_parser/manifest.xml @@ -12,6 +12,7 @@ + diff --git a/urdf_parser/src/joint.cpp b/urdf_parser/src/joint.cpp index e00d1e4..2e02cf5 100644 --- a/urdf_parser/src/joint.cpp +++ b/urdf_parser/src/joint.cpp @@ -34,7 +34,7 @@ /* Author: John Hsu */ -#include +#include #include #include diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index 09d594f..6c6702d 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -35,7 +35,7 @@ /* Author: Wim Meeussen */ -#include "urdf_parser/link.h" +#include #include #include #include