quaternion.h
Go to the documentation of this file.
1 /*
2 Copyright (C) 2002-2004 Etienne Lachance
3 
4 This library is free software; you can redistribute it and/or modify
5 it under the terms of the GNU Lesser General Public License as
6 published by the Free Software Foundation; either version 2.1 of the
7 License, or (at your option) any later version.
8 
9 This library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU Lesser General Public License for more details.
13 
14 You should have received a copy of the GNU Lesser General Public
15 License along with this library; if not, write to the Free Software
16 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
17 
18 
19 Report problems and direct all questions to:
20 
21 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
22 
23 Reference:
24 
25 [1] J.C.K. Chou, "Quaternion Kinematic and Dynamic Differential Equations",
26  IEEE Transaction on Robotics and Automation, vol 8, p53-64.
27 
28 [2] S. Chiaverini, B. Siciliano, "The Unit Quaternion: A Useful Tool for
29  Inverse Kinematics of Robot Manipulators", Systems Analysis, Modelling
30  and Simulation, vol. 35, pp.45-60, 1999.
31 
32 [3] C. Natale, "Quaternion-Based Representation of Rigid Bodies Orientation",
33  PRISMA LAB, PRISMA Technical Report no. 97-05, Oct 1997.
34 
35 [4] M. Lillholm, E.B. Dam, M. Koch, "Quaternions, Interpolation and Animation",
36  Technical Report DIKU-TR-98/5, University of Copenhagen, July 1998.
37 
38 [5] D. Eberly, "Quaternion Algebra and Calculus", Magic Software Inc.,
39  http://www.magic-software.com, March 1999.
40 -------------------------------------------------------------------------------
41 Revision_history:
42 
43 2003/05/28: Etienne Lachance
44  -Added functions Slerp, Slerp_prime, Squad, Squad_prime.
45  -Added the following member functions:+=, -=, *=, /=, Exp, d_dt, Ln, Ln_4, E
46 
47 2004/05/21: Etienne Lachance
48  -Added Doxygen comments.
49 
50 2004/07/01: Ethan Tira-Thompson
51  -Added support for newmat's use_namespace #define, using ROBOOP namespace
52 
53 2005/11/06: Etienne Lachance
54  - No need to provide a copy constructor and the assignment operator
55  (operator=) for Quaternion class. Instead we use the one provide by the
56  compiler.
57 
58 2005/11/13: Etienne Lachance
59  - operator* and operator/ are now non-member functions when one of the
60  operand is a real. With these modifications we support q2 = c * q1 and
61  q2 = q1 * c
62 ------------------------------------------------------------------------------
63 */
64 
65 
66 #ifndef QUATERNION_H
67 #define QUATERNION_H
68 
74 static const char header_quat_rcsid[] = "$Id: quaternion.h,v 1.12 2005/11/15 19:25:58 gourdeau Exp $";
76 
77 #include "robot.h"
78 
79 #ifdef use_namespace
80 namespace ROBOOP {
81  using namespace NEWMAT;
82 #endif
83 
84 #define BASE_FRAME 0
85 #define BODY_FRAME 1
86 #define EPSILON 0.0000001
87 
93 {
94 public:
95  Quaternion();
96  Quaternion(const Real angle_in_rad, const ColumnVector & axis);
97  Quaternion(const Real s, const Real v1, const Real v2,
98  const Real v3);
99  Quaternion(const Matrix & R);
100 
101  Quaternion operator+(const Quaternion & q)const;
102  Quaternion operator-(const Quaternion & q)const;
103  Quaternion operator*(const Quaternion & q)const;
104  Quaternion operator/(const Quaternion & q)const;
105  Quaternion conjugate()const;
106 
107 // Quaternion i()const { return conjugate(); }
108  Quaternion i()const;
109  Quaternion & unit();
110  Quaternion exp() const;
111  Quaternion power(const Real t) const;
112  Quaternion Log() const;
113 
114  Quaternion dot(const ColumnVector & w, const short sign)const;
115  ReturnMatrix E(const short sign)const;
116 
117  Real norm()const;
118  Real dot_prod(const Quaternion & q)const;
119  Real s()const { return s_; }
120  void set_s(const Real s){ s_ = s; }
121  ReturnMatrix v()const { return v_; }
122  void set_v(const ColumnVector & v);
123  ReturnMatrix R()const;
124  ReturnMatrix T()const;
125 
126 private:
129 };
130 
131 // ----------------------------------------------------------------------------
132 
133 Quaternion operator*(const Real c, const Quaternion & rhs);
134 Quaternion operator*(const Quaternion & lhs, const Real c);
135 Quaternion operator/(const Real c, const Quaternion & rhs);
136 Quaternion operator/(const Quaternion & lhs, const Real c);
137 
138 ReturnMatrix Omega(const Quaternion & q, const Quaternion & q_dot);
139 
140 short Integ_quat(Quaternion & dquat_present, Quaternion & dquat_past,
141  Quaternion & quat, const Real dt);
142 Real Integ_Trap_quat_s(const Quaternion & present, Quaternion & past,
143  const Real dt);
144 ReturnMatrix Integ_Trap_quat_v(const Quaternion & present, Quaternion & past,
145  const Real dt);
146 
147 Quaternion Slerp(const Quaternion & q0, const Quaternion & q1, const Real t);
148 Quaternion Slerp_prime(const Quaternion & q0, const Quaternion & q1, const Real t);
149 
150 Quaternion Squad(const Quaternion & p, const Quaternion & a, const Quaternion & b,
151  const Quaternion & q, const Real t);
152 Quaternion Squad_prime(const Quaternion & p, const Quaternion & a, const Quaternion & b,
153  const Quaternion & q, const Real t);
154 
155 #ifdef use_namespace
156 }
157 #endif
158 
159 #endif
Quaternion class definition.
Definition: quaternion.h:92
ReturnMatrix v() const
Return vector part.
Definition: quaternion.h:121
Quaternion Squad(const Quaternion &p, const Quaternion &a, const Quaternion &b, const Quaternion &q, const Real t)
Spherical Cubic Interpolation.
Definition: quaternion.cpp:725
static const char header_quat_rcsid[]
RCS/CVS version.
Definition: quaternion.h:75
NegShiftedMatrix operator-(Real, const BaseMatrix &)
Definition: newmat6.cpp:305
Robots class definitions.
ShiftedMatrix operator+(Real f, const BaseMatrix &BM)
Definition: newmat6.cpp:302
double Real
Definition: include.h:307
short Integ_quat(Quaternion &dquat_present, Quaternion &dquat_past, Quaternion &quat, const Real dt)
Trapezoidal quaternion integration.
Definition: quaternion.cpp:585
Real Integ_Trap_quat_s(const Quaternion &present, Quaternion &past, const Real dt)
Trapezoidal quaternion scalar part integration.
Definition: quaternion.cpp:612
Real s_
Quaternion scalar part.
Definition: quaternion.h:127
Real s() const
Return scalar part.
Definition: quaternion.h:119
Quaternion Slerp_prime(const Quaternion &q0, const Quaternion &q1, const Real t)
Spherical Linear Interpolation derivative.
Definition: quaternion.cpp:692
void set_s(const Real s)
Set scalar part.
Definition: quaternion.h:120
ColumnVector q0
Definition: demo.cpp:229
Quaternion Slerp(const Quaternion &q0, const Quaternion &q1, const Real t)
Spherical Linear Interpolation.
Definition: quaternion.cpp:631
Quaternion Squad_prime(const Quaternion &p, const Quaternion &a, const Quaternion &b, const Quaternion &q, const Real t)
Spherical Cubic Interpolation derivative.
Definition: quaternion.cpp:751
The usual rectangular matrix.
Definition: newmat.h:625
FloatVector FloatVector * a
ColumnVector v_
Quaternion vector part.
Definition: quaternion.h:128
Quaternion operator*(const Real c, const Quaternion &rhs)
Overload * operator, multiplication by a scalar.
Definition: quaternion.cpp:516
ReturnMatrix Integ_Trap_quat_v(const Quaternion &present, Quaternion &past, const Real dt)
Trapezoidal quaternion vector part integration.
Definition: quaternion.cpp:621
Column vector.
Definition: newmat.h:1008
ReturnMatrix Omega(const Quaternion &q, const Quaternion &q_dot)
Return angular velocity from a quaternion and it's time derivative.
Definition: quaternion.cpp:560
Quaternion operator/(const Real c, const Quaternion &rhs)
Overload / operator, division by a scalar.
Definition: quaternion.cpp:542


kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:16