Homepage Demos Overview Downloads Tutorials Reference
Credits

quaternion.h

Go to the documentation of this file.
00001 /*
00002 Copyright (C) 2002-2004  Etienne Lachance
00003 
00004 This library is free software; you can redistribute it and/or modify
00005 it under the terms of the GNU Lesser General Public License as
00006 published by the Free Software Foundation; either version 2.1 of the
00007 License, or (at your option) any later version.
00008 
00009 This library is distributed in the hope that it will be useful,
00010 but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 GNU Lesser General Public License for more details.
00013 
00014 You should have received a copy of the GNU Lesser General Public
00015 License along with this library; if not, write to the Free Software
00016 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00017 
00018 
00019 Report problems and direct all questions to:
00020 
00021 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
00022 
00023 Reference:
00024 
00025 [1] J.C.K. Chou, "Quaternion Kinematic and Dynamic Differential Equations", 
00026     IEEE Transaction on Robotics and Automation, vol 8, p53-64.
00027 
00028 [2] S. Chiaverini, B. Siciliano, "The Unit Quaternion: A Useful Tool for
00029     Inverse Kinematics of Robot Manipulators", Systems Analysis, Modelling
00030     and Simulation, vol. 35, pp.45-60, 1999.
00031 
00032 [3] C. Natale, "Quaternion-Based Representation of Rigid Bodies Orientation",
00033     PRISMA LAB, PRISMA Technical Report no. 97-05, Oct 1997.
00034 
00035 [4] M. Lillholm, E.B. Dam, M. Koch, "Quaternions, Interpolation and Animation",
00036     Technical Report DIKU-TR-98/5, University of Copenhagen, July 1998.
00037 
00038 [5] D. Eberly, "Quaternion Algebra and Calculus", Magic Software Inc.,
00039     http://www.magic-software.com, March 1999.
00040 -------------------------------------------------------------------------------
00041 Revision_history:
00042 
00043 2003/05/28: Etienne Lachance
00044     -Added functions Slerp, Slerp_prime, Squad, Squad_prime.
00045     -Added the following member functions:+=, -=, *=, /=, Exp, d_dt, Ln, Ln_4, E
00046 
00047 2004/05/21: Etienne Lachance
00048    -Added Doxygen comments.
00049 
00050 2004/07/01: Ethan Tira-Thompson
00051     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00052 ------------------------------------------------------------------------------
00053 */
00054 
00055 
00056 #ifndef QUATERNION_H
00057 #define QUATERNION_H
00058 
00059 /*!
00060   @file quaternion.h
00061   @brief Quaternion class.
00062 */
00063 
00064 #include "robot.h"
00065 
00066 #ifdef use_namespace
00067 namespace ROBOOP {
00068   using namespace NEWMAT;
00069 #endif
00070 
00071 //! @brief RCS/CVS version.
00072 static const char header_quat_rcsid[] __UNUSED__ = "$Id: quaternion.h,v 1.5 2005/07/26 03:22:09 ejt Exp $";
00073 
00074 #define BASE_FRAME 0
00075 #define BODY_FRAME 1
00076 #define EPSILON 0.0000001
00077 
00078 /*!
00079   @class Quaternion
00080   @brief Quaternion class definition.
00081 */
00082 class Quaternion
00083 {
00084 public:
00085    Quaternion();
00086    Quaternion(const Quaternion & q);
00087    Quaternion(const Real angle_in_rad, const ColumnVector & axis);
00088    Quaternion(const Real s, const Real v1, const Real v2,
00089               const Real v3);
00090    Quaternion(const Matrix & R);
00091 
00092    Quaternion & operator=(const Quaternion & q);
00093    Quaternion   operator+(const Quaternion & q)const;
00094    Quaternion   operator-(const Quaternion & q)const;
00095    Quaternion   operator*(const Quaternion & q)const;
00096    Quaternion   operator*(const Real c)const;
00097    Quaternion   operator/(const Quaternion & q)const;
00098    Quaternion   operator/(const Real c)const;
00099    Quaternion   conjugate()const;
00100 
00101 //   Quaternion   i()const { return conjugate(); }
00102    Quaternion   i()const;
00103    Quaternion & unit(); 
00104    Quaternion   exp() const;
00105    Quaternion   power(const Real t) const;
00106    Quaternion   Log() const;
00107 
00108    Quaternion   dot(const ColumnVector & w, const short sign)const;
00109    ReturnMatrix E(const short sign)const;
00110 
00111    Real         norm()const;
00112    Real         dot_prod(const Quaternion & q)const;
00113    Real         s()const { return s_; }        //!< Return scalar part.
00114    void         set_s(const Real s){ s_ = s; } //!< Set scalar part.
00115    ReturnMatrix v()const { return v_; }        //!< Return vector part.
00116    void         set_v(const ColumnVector & v); //!< Set vector part.
00117    ReturnMatrix R()const;
00118    ReturnMatrix T()const;
00119 
00120 private:
00121    Real s_;         //!< Quaternion scalar part.
00122    ColumnVector v_; //!< Quaternion vector part.
00123 };
00124 
00125 // ----------------------------------------------------------------------------
00126 
00127 ReturnMatrix Omega(const Quaternion & q, const Quaternion & q_dot);
00128 
00129 short Integ_quat(Quaternion & dquat_present, Quaternion & dquat_past,
00130                  Quaternion & quat, const Real dt);
00131 Real Integ_Trap_quat_s(const Quaternion & present, Quaternion & past,
00132                        const Real dt);
00133 ReturnMatrix Integ_Trap_quat_v(const Quaternion & present, Quaternion & past,
00134                                const Real dt);
00135 
00136 Quaternion Slerp(const Quaternion & q0, const Quaternion & q1, const Real t);
00137 Quaternion Slerp_prime(const Quaternion & q0, const Quaternion & q1, const Real t);
00138 
00139 Quaternion Squad(const Quaternion & p, const Quaternion & a, const Quaternion & b,
00140                  const Quaternion & q, const Real t);
00141 Quaternion Squad_prime(const Quaternion & p, const Quaternion & a, const Quaternion & b,
00142                        const Quaternion & q, const Real t);
00143 
00144 #ifdef use_namespace
00145 }
00146 #endif
00147 
00148 #endif

ROBOOP v1.21a
Generated Tue Aug 16 16:32:15 2005 by Doxygen 1.4.4