00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef URDF_POSE_H
00038 #define URDF_POSE_H
00039
00040 #include <string>
00041 #include <vector>
00042 #include <math.h>
00043 #include <ros/ros.h>
00044 #include <boost/algorithm/string.hpp>
00045 #include <boost/lexical_cast.hpp>
00046
00047 namespace urdf{
00048
00049 class Vector3
00050 {
00051 public:
00052 Vector3(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;};
00053 Vector3() {this->clear();};
00054 double x;
00055 double y;
00056 double z;
00057
00058 void clear() {this->x=this->y=this->z=0.0;};
00059 bool init(const std::string &vector_str)
00060 {
00061 this->clear();
00062 std::vector<std::string> pieces;
00063 std::vector<double> xyz;
00064 boost::split( pieces, vector_str, boost::is_any_of(" "));
00065 for (unsigned int i = 0; i < pieces.size(); ++i){
00066 if (pieces[i] != ""){
00067 try
00068 {
00069 xyz.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
00070 }
00071 catch (boost::bad_lexical_cast &e)
00072 {
00073 ROS_ERROR("Vector3 xyz element (%s) is not a valid float",pieces[i].c_str());
00074 return false;
00075 }
00076 }
00077 }
00078
00079 if (xyz.size() != 3) {
00080 ROS_ERROR("Vector contains %i elements instead of 3 elements", (int)xyz.size());
00081 return false;
00082 }
00083
00084 this->x = xyz[0];
00085 this->y = xyz[1];
00086 this->z = xyz[2];
00087
00088 return true;
00089 };
00090 };
00091
00092 class Rotation
00093 {
00094 public:
00095 Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;};
00096 Rotation() {this->clear();};
00097 void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const
00098 {
00099 quat_x = this->x;
00100 quat_y = this->y;
00101 quat_z = this->z;
00102 quat_w = this->w;
00103 };
00104 void getRPY(double &roll,double &pitch,double &yaw) const
00105 {
00106 double sqw;
00107 double sqx;
00108 double sqy;
00109 double sqz;
00110
00111 sqx = this->x * this->x;
00112 sqy = this->y * this->y;
00113 sqz = this->z * this->z;
00114 sqw = this->w * this->w;
00115
00116 roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz);
00117 double sarg = -2 * (this->x*this->z - this->w*this->y);
00118 pitch = sarg <= -1.0 ? -0.5*M_PI : (sarg >= 1.0 ? 0.5*M_PI : asin(sarg));
00119 yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz);
00120
00121 };
00122 void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w)
00123 {
00124 this->x = quat_x;
00125 this->y = quat_y;
00126 this->z = quat_z;
00127 this->w = quat_w;
00128 this->normalize();
00129 };
00130 void setFromRPY(double roll, double pitch, double yaw)
00131 {
00132 double phi, the, psi;
00133
00134 phi = roll / 2.0;
00135 the = pitch / 2.0;
00136 psi = yaw / 2.0;
00137
00138 this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi);
00139 this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi);
00140 this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi);
00141 this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi);
00142
00143 this->normalize();
00144 };
00145
00146 double x,y,z,w;
00147
00148 bool init(const std::string &rotation_str)
00149 {
00150 this->clear();
00151
00152 Vector3 rpy;
00153
00154 if (!rpy.init(rotation_str))
00155 return false;
00156 else
00157 {
00158 this->setFromRPY(rpy.x,rpy.y,rpy.z);
00159 return true;
00160 }
00161
00162 };
00163
00164 void clear() { this->x=this->y=this->z=0.0;this->w=1.0; }
00165
00166 void normalize()
00167 {
00168 double s = sqrt(this->x * this->x +
00169 this->y * this->y +
00170 this->z * this->z +
00171 this->w * this->w);
00172 if (s == 0.0)
00173 {
00174 this->x = 0.0;
00175 this->y = 0.0;
00176 this->z = 0.0;
00177 this->w = 1.0;
00178 }
00179 else
00180 {
00181 this->x /= s;
00182 this->y /= s;
00183 this->z /= s;
00184 this->w /= s;
00185 }
00186 };
00187 };
00188
00189 class Pose
00190 {
00191 public:
00192 Pose() { this->clear(); };
00193
00194 Vector3 position;
00195 Rotation rotation;
00196
00197 void clear()
00198 {
00199 this->position.clear();
00200 this->rotation.clear();
00201 };
00202 bool initXml(TiXmlElement* xml)
00203 {
00204 this->clear();
00205 if (!xml)
00206 {
00207 ROS_DEBUG("parsing pose: xml empty");
00208 return false;
00209 }
00210 else
00211 {
00212 const char* xyz_str = xml->Attribute("xyz");
00213 if (xyz_str == NULL)
00214 {
00215 ROS_DEBUG("parsing pose: no xyz, using default values.");
00216 return true;
00217 }
00218 else
00219 {
00220 if (!this->position.init(xyz_str))
00221 {
00222 ROS_ERROR("malformed xyz");
00223 this->position.clear();
00224 return false;
00225 }
00226 }
00227
00228 const char* rpy_str = xml->Attribute("rpy");
00229 if (rpy_str == NULL)
00230 {
00231 ROS_DEBUG("parsing pose: no rpy, using default values.");
00232 return true;
00233 }
00234 else
00235 {
00236 if (!this->rotation.init(rpy_str))
00237 {
00238 ROS_ERROR("malformed rpy");
00239 return false;
00240 this->rotation.clear();
00241 }
00242 }
00243
00244 return true;
00245 }
00246 };
00247 };
00248
00249 }
00250
00251 #endif