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 2005/11/06: Etienne Lachance
00054     - No need to provide a copy constructor and the assignment operator 
00055       (operator=) for Quaternion class. Instead we use the one provide by the
00056       compiler.
00057 
00058 2005/11/13: Etienne Lachance
00059     - operator* and operator/ are now non-member functions when one of the
00060       operand is a real. With these modifications we support q2 = c * q1 and
00061       q2 = q1 * c
00062 ------------------------------------------------------------------------------
00063 */
00064 
00065 
00066 #ifndef QUATERNION_H
00067 #define QUATERNION_H
00068 
00074 
00075 static const char header_quat_rcsid[] = "$Id: quaternion.h,v 1.12 2005/11/15 19:25:58 gourdeau Exp $";
00076 
00077 #include "robot.h"
00078 
00079 #ifdef use_namespace
00080 namespace ROBOOP {
00081   using namespace NEWMAT;
00082 #endif
00083 
00084 #define BASE_FRAME 0
00085 #define BODY_FRAME 1
00086 #define EPSILON 0.0000001
00087 
00092 class Quaternion
00093 {
00094 public:
00095    Quaternion();
00096    Quaternion(const Real angle_in_rad, const ColumnVector & axis);
00097    Quaternion(const Real s, const Real v1, const Real v2,
00098               const Real v3);
00099    Quaternion(const Matrix & R);
00100 
00101    Quaternion   operator+(const Quaternion & q)const;
00102    Quaternion   operator-(const Quaternion & q)const;
00103    Quaternion   operator*(const Quaternion & q)const;
00104    Quaternion   operator/(const Quaternion & q)const;
00105    Quaternion   conjugate()const;
00106 
00107 //   Quaternion   i()const { return conjugate(); }
00108    Quaternion   i()const;
00109    Quaternion & unit(); 
00110    Quaternion   exp() const;
00111    Quaternion   power(const Real t) const;
00112    Quaternion   Log() const;
00113 
00114    Quaternion   dot(const ColumnVector & w, const short sign)const;
00115    ReturnMatrix E(const short sign)const;
00116 
00117    Real         norm()const;
00118    Real         dot_prod(const Quaternion & q)const;
00119    Real         s()const { return s_; }        
00120    void         set_s(const Real s){ s_ = s; } 
00121    ReturnMatrix v()const { return v_; }        
00122    void         set_v(const ColumnVector & v); 
00123    ReturnMatrix R()const;
00124    ReturnMatrix T()const;
00125 
00126 private:
00127    Real s_;         
00128    ColumnVector v_; 
00129 };
00130 
00131 // ----------------------------------------------------------------------------
00132 
00133 Quaternion  operator*(const Real c, const Quaternion & rhs);
00134 Quaternion  operator*(const Quaternion & lhs, const Real c);
00135 Quaternion  operator/(const Real c, const Quaternion & rhs);
00136 Quaternion  operator/(const Quaternion & lhs, const Real c);
00137 
00138 ReturnMatrix Omega(const Quaternion & q, const Quaternion & q_dot);
00139 
00140 short Integ_quat(Quaternion & dquat_present, Quaternion & dquat_past,
00141                  Quaternion & quat, const Real dt);
00142 Real Integ_Trap_quat_s(const Quaternion & present, Quaternion & past,
00143                        const Real dt);
00144 ReturnMatrix Integ_Trap_quat_v(const Quaternion & present, Quaternion & past,
00145                                const Real dt);
00146 
00147 Quaternion Slerp(const Quaternion & q0, const Quaternion & q1, const Real t);
00148 Quaternion Slerp_prime(const Quaternion & q0, const Quaternion & q1, const Real t);
00149 
00150 Quaternion Squad(const Quaternion & p, const Quaternion & a, const Quaternion & b,
00151                  const Quaternion & q, const Real t);
00152 Quaternion Squad_prime(const Quaternion & p, const Quaternion & a, const Quaternion & b,
00153                        const Quaternion & q, const Real t);
00154 
00155 #ifdef use_namespace
00156 }
00157 #endif
00158 
00159 #endif


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:33