bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

move to using urdf_interface

This commit is contained in:
Wim Meeussen 2011-06-03 09:44:10 -07:00
parent 818b5f6788
commit 50c7462a35
12 changed files with 12 additions and 907 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
urdf/include/urdf/color.h
View File

@ -1 +1,3 @@
#pragma message("WARNING: Including urdf/color.h is deprecated. Include urdf_interface/color.h instead")
#include "urdf_parser/color.h"

2
urdf/include/urdf/joint.h
View File

@ -1 +1,3 @@
#pragma message("WARNING: Including urdf/joint.h is deprecated. Include urdf_interface/joint.h instead")
#include "urdf_parser/joint.h"

2
urdf/include/urdf/link.h
View File

@ -1 +1,3 @@
#pragma message("WARNING: Including urdf/link.h is deprecated. Include urdf_interface/link.h instead")
#include "urdf_parser/link.h"

2
urdf/include/urdf/pose.h
View File

@ -1 +1,3 @@
#pragma message("WARNING: Including urdf/pose.h is deprecated. Include urdf_interface/pose.h instead")
#include "urdf_parser/pose.h"

104
urdf_parser/include/urdf_parser/color.h
View File

@ -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 <string>
#include <vector>
#include <math.h>
#include <ros/ros.h>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
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<std::string> pieces;
std::vector<float> 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<double>(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

235
urdf_parser/include/urdf_parser/joint.h
View File

@ -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 <string>
#include <vector>
#include <tinyxml/tinyxml.h>
#include <boost/shared_ptr.hpp>
#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<double> 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<JointDynamics> dynamics;
/// Joint Limits
boost::shared_ptr<JointLimits> limits;
/// Unsupported Hidden Feature
boost::shared_ptr<JointSafety> safety;
/// Unsupported Hidden Feature
boost::shared_ptr<JointCalibration> calibration;
/// Option to Mimic another Joint
boost::shared_ptr<JointMimic> 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

259
urdf_parser/include/urdf_parser/link.h
View File

@ -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 <string>
#include <vector>
#include <tinyxml/tinyxml.h>
#include <boost/shared_ptr.hpp>
#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> geometry;
std::string material_name;
boost::shared_ptr<Material> 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> 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> inertial;
/// visual element
boost::shared_ptr<Visual> visual;
/// collision element
boost::shared_ptr<Collision> collision;
/// a collection of visual elements, keyed by a string tag called "group"
std::map<std::string, boost::shared_ptr<std::vector<boost::shared_ptr<Visual> > > > visual_groups;
/// a collection of collision elements, keyed by a string tag called "group"
std::map<std::string, boost::shared_ptr<std::vector<boost::shared_ptr<Collision> > > > 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<Joint> parent_joint;
std::vector<boost::shared_ptr<Joint> > child_joints;
std::vector<boost::shared_ptr<Link> > child_links;
bool initXml(TiXmlElement* config);
boost::shared_ptr<Link> getParent() const
{return parent_link_.lock();};
void setParent(boost::shared_ptr<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_groups.clear();
};
void setParentJoint(boost::shared_ptr<Joint> child);
void addChild(boost::shared_ptr<Link> child);
void addChildJoint(boost::shared_ptr<Joint> child);
void addVisual(std::string group_name, boost::shared_ptr<Visual> visual);
boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > getVisuals(const std::string& group_name) const;
void addCollision(std::string group_name, boost::shared_ptr<Collision> collision);
boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > getCollisions(const std::string& group_name) const;
private:
boost::weak_ptr<Link> parent_link_;
};
}
#endif

2
urdf_parser/include/urdf_parser/parser.h
View File

@ -41,7 +41,7 @@
#include <map>
#include <tinyxml/tinyxml.h>
#include <boost/function.hpp>
#include "urdf_parser/link.h"
#include <urdf_interface/link.h>
namespace urdf{

306
urdf_parser/include/urdf_parser/pose.h
View File

@ -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 <string>
#include <vector>
#include <math.h>
#include <ros/ros.h>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
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<std::string> pieces;
std::vector<double> 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<double>(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

1
urdf_parser/manifest.xml
View File

@ -12,6 +12,7 @@
<depend package="tinyxml" />
<depend package="roscpp" /> <!-- Only to call ROS_ERROR -->
<depend package="urdf_interface" />
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lurdf_parser"/>

2
urdf_parser/src/joint.cpp
View File

@ -34,7 +34,7 @@
/* Author: John Hsu */
#include <urdf_parser/joint.h>
#include <urdf_interface/joint.h>
#include <ros/ros.h>
#include <boost/lexical_cast.hpp>

2
urdf_parser/src/link.cpp
View File

@ -35,7 +35,7 @@
/* Author: Wim Meeussen */
#include "urdf_parser/link.h"
#include <urdf_interface/link.h>
#include <ros/ros.h>
#include <ros/package.h>
#include <fstream>