GCOP  1.0
pose.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Wim Meeussen */
00036 
00037 #ifndef URDF_INTERFACE_POSE_H
00038 #define URDF_INTERFACE_POSE_H
00039 
00040 #include <string>
00041 #include <vector>
00042 #include <math.h>
00043 #include <boost/algorithm/string.hpp>
00044 #include <boost/lexical_cast.hpp>
00045 
00046 #include <tinyxml.h> // FIXME: remove parser from here
00047 //#include <ros/console.h>
00048 
00049 namespace gcop_urdf{
00050 
00051 class Vector3
00052 {
00053 public:
00054   Vector3(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;};
00055   Vector3() {this->clear();};
00056   double x;
00057   double y;
00058   double z;
00059 
00060   void clear() {this->x=this->y=this->z=0.0;};
00061   bool init(const std::string &vector_str)
00062   {
00063     this->clear();
00064     std::vector<std::string> pieces;
00065     std::vector<double> xyz;
00066     boost::split( pieces, vector_str, boost::is_any_of(" "));
00067     for (unsigned int i = 0; i < pieces.size(); ++i){
00068       if (pieces[i] != ""){
00069         try
00070         {
00071           xyz.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
00072         }
00073         catch (boost::bad_lexical_cast &e)
00074         {
00075           printf("Vector3 xyz element (%s) is not a valid float",pieces[i].c_str());
00076           return false;
00077         }
00078       }
00079     }
00080 
00081     if (xyz.size() != 3) {
00082   printf("Vector contains %i elements instead of 3 elements", (int)xyz.size()); 
00083       return false;
00084     }
00085 
00086     this->x = xyz[0];
00087     this->y = xyz[1];
00088     this->z = xyz[2];
00089 
00090     return true;
00091   };
00092   Vector3 operator+(Vector3 vec)
00093   {
00094     return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z);
00095   };
00096   Vector3 operator-()
00097   {
00098     return Vector3(-this->x,-this->y,-this->z);
00099   };
00100 };
00101 
00102 class Rotation
00103 {
00104 public:
00105   Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;};
00106   Rotation() {this->clear();};
00107   void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const
00108   {
00109     quat_x = this->x;
00110     quat_y = this->y;
00111     quat_z = this->z;
00112     quat_w = this->w;
00113   };
00114   void getRPY(double &roll,double &pitch,double &yaw) const
00115   {
00116     double sqw;
00117     double sqx;
00118     double sqy;
00119     double sqz;
00120 
00121     sqx = this->x * this->x;
00122     sqy = this->y * this->y;
00123     sqz = this->z * this->z;
00124     sqw = this->w * this->w;
00125 
00126     roll  = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz);
00127     double sarg = -2 * (this->x*this->z - this->w*this->y);
00128     pitch = sarg <= -1.0 ? -0.5*M_PI : (sarg >= 1.0 ? 0.5*M_PI : asin(sarg));
00129     yaw   = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz);
00130 
00131   };
00132   void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w)
00133   {
00134     this->x = quat_x;
00135     this->y = quat_y;
00136     this->z = quat_z;
00137     this->w = quat_w;
00138     this->normalize();
00139   };
00140   void setFromRPY(double roll, double pitch, double yaw)
00141   {
00142     double phi, the, psi;
00143 
00144     phi = roll / 2.0;
00145     the = pitch / 2.0;
00146     psi = yaw / 2.0;
00147 
00148     this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi);
00149     this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi);
00150     this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi);
00151     this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi);
00152 
00153     this->normalize();
00154   };
00155 
00156   double x,y,z,w;
00157 
00158   bool init(const std::string &rotation_str)
00159   {
00160     this->clear();
00161 
00162     Vector3 rpy;
00163     
00164     if (!rpy.init(rotation_str))
00165       return false;
00166     else
00167     {
00168       this->setFromRPY(rpy.x,rpy.y,rpy.z);
00169       return true;
00170     }
00171       
00172   };
00173 
00174   void clear() { this->x=this->y=this->z=0.0;this->w=1.0; }
00175 
00176   void normalize()
00177   {
00178     double s = sqrt(this->x * this->x +
00179                     this->y * this->y +
00180                     this->z * this->z +
00181                     this->w * this->w);
00182     if (s == 0.0)
00183     {
00184       this->x = 0.0;
00185       this->y = 0.0;
00186       this->z = 0.0;
00187       this->w = 1.0;
00188     }
00189     else
00190     {
00191       this->x /= s;
00192       this->y /= s;
00193       this->z /= s;
00194       this->w /= s;
00195     }
00196   };
00197 
00198   // Multiplication operator (copied from gazebo)
00199   Rotation operator*( const Rotation &qt ) const
00200   {
00201     Rotation c;
00202 
00203     c.x = this->w * qt.x + this->x * qt.w + this->y * qt.z - this->z * qt.y;
00204     c.y = this->w * qt.y - this->x * qt.z + this->y * qt.w + this->z * qt.x;
00205     c.z = this->w * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.w;
00206     c.w = this->w * qt.w - this->x * qt.x - this->y * qt.y - this->z * qt.z;
00207 
00208     return c;
00209   };
00211   Vector3 operator*(Vector3 vec) const
00212   {
00213     Rotation tmp;
00214     Vector3 result;
00215 
00216     tmp.w = 0.0;
00217     tmp.x = vec.x;
00218     tmp.y = vec.y;
00219     tmp.z = vec.z;
00220 
00221     tmp = (*this) * (tmp * this->GetInverse());
00222 
00223     result.x = tmp.x;
00224     result.y = tmp.y;
00225     result.z = tmp.z;
00226 
00227     return result;
00228   };
00229   // Get the inverse of this quaternion
00230   Rotation GetInverse() const 
00231   {
00232     Rotation q;
00233 
00234     double norm = this->w*this->w+this->x*this->x+this->y*this->y+this->z*this->z;
00235 
00236     if (norm > 0.0)
00237     {
00238       q.w = this->w / norm;
00239       q.x = -this->x / norm;
00240       q.y = -this->y / norm;
00241       q.z = -this->z / norm;
00242     }
00243 
00244     return q;
00245   };
00246 
00247 
00248 };
00249 
00250 class Pose
00251 {
00252   public:
00253     Pose() { this->clear(); };
00254 
00255     Vector3  position;
00256     Rotation rotation;
00257 
00258     void clear()
00259     {
00260       this->position.clear();
00261       this->rotation.clear();
00262     };
00263     bool initXml(TiXmlElement* xml)
00264     {
00265       this->clear();
00266       if (!xml)
00267       {
00268         printf("parsing pose: xml empty");
00269         return false;
00270       }
00271       else
00272       {
00273         const char* xyz_str = xml->Attribute("xyz");
00274         if (xyz_str == NULL)
00275         {
00276           printf("parsing pose: no xyz, using default values.");
00277           return true;
00278         }
00279         else
00280         {
00281           if (!this->position.init(xyz_str))
00282           {
00283             printf("malformed xyz");
00284             this->position.clear();
00285             return false;
00286           }
00287         }
00288 
00289         const char* rpy_str = xml->Attribute("rpy");
00290         if (rpy_str == NULL)
00291         {
00292           printf("parsing pose: no rpy, using default values.");
00293           return true;
00294         }
00295         else
00296         {
00297           if (!this->rotation.init(rpy_str))
00298           {
00299             printf("malformed rpy");
00300             return false;
00301             this->rotation.clear();
00302           }
00303         }
00304 
00305         return true;
00306       }
00307     };
00308     Pose GetInverse() const
00309     {
00310       Pose res;
00311       res.rotation = this->rotation.GetInverse();
00312       res.position = -(res.rotation*this->position);
00313       return res;
00314     }
00315     Pose operator*( const Pose &p2 ) const
00316     {
00317       Pose res;
00318       res.rotation = this->rotation * p2.rotation;
00319       res.position = this->rotation * p2.position + this->position; 
00320       return res;
00321     }
00322     friend std::ostream& operator<<(std::ostream& out, const Pose& vec) // output
00323     {
00324       out << vec.rotation.w << " "<<vec.rotation.x << " "<<vec.rotation.y << " "<<vec.rotation.z << " "<<vec.position.x << " "<<vec.position.y << " "<<vec.position.z << " "<<std::endl;
00325       return out;
00326     }
00327 };
00328 
00329 }
00330 
00331 #endif