quaternion_operation.cpp
Go to the documentation of this file.
1 
13 
14 geometry_msgs::Quaternion operator+(geometry_msgs::Quaternion quat1,geometry_msgs::Quaternion quat2)
15 {
16  geometry_msgs::Quaternion ret;
17  ret.x = quat1.x + quat2.x;
18  ret.y = quat1.y + quat2.y;
19  ret.z = quat1.z + quat2.z;
20  ret.w = quat1.w + quat2.w;
21  return ret;
22 }
23 
24 geometry_msgs::Quaternion operator*(geometry_msgs::Quaternion quat1,geometry_msgs::Quaternion quat2)
25 {
26  geometry_msgs::Quaternion ret;
27  ret.x = quat1.w*quat2.x - quat1.z*quat2.y + quat1.y*quat2.z + quat1.x*quat2.w;
28  ret.y = quat1.z*quat2.x + quat1.w*quat2.y - quat1.x*quat2.z + quat1.y*quat2.w;
29  ret.z = -quat1.y*quat2.x + quat1.x*quat2.y + quat1.w*quat2.z + quat1.z*quat2.w;
30  ret.w = -quat1.x*quat2.x - quat1.y*quat2.y - quat1.z*quat2.z + quat1.w*quat2.w;
31  return ret;
32 }
33 
34 namespace quaternion_operation
35 {
36  geometry_msgs::Quaternion convertEulerAngleToQuaternion(geometry_msgs::Vector3 euler)
37  {
38  geometry_msgs::Quaternion ret;
39  double roll = euler.x;
40  double pitch = euler.y;
41  double yaw = euler.z;
42  tf2::Quaternion tf_quat;
43  tf_quat.setRPY(roll,pitch,yaw);
44  ret.x = tf_quat.x();
45  ret.y = tf_quat.y();
46  ret.z = tf_quat.z();
47  ret.w = tf_quat.w();
48  /*
49  double roll = euler.x;
50  double pitch = euler.y;
51  double yaw = euler.z;
52  Eigen:: Quaterniond quat = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())
53  * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
54  * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
55  geometry_msgs::Quaternion ret;
56  ret.x = quat.x();
57  ret.y = quat.y();
58  ret.z = quat.z();
59  ret.w = quat.w();
60  */
61  return ret;
62  }
63 
64  Eigen::Matrix3d getRotationMatrix(geometry_msgs::Quaternion quat)
65  {
66  double x = quat.x;
67  double y = quat.y;
68  double z = quat.z;
69  double w = quat.w;
70  Eigen::Matrix3d ret(3,3);
71  ret <<
72  x*x-y*y-z*z+w*w, 2*(x*y-z*w), 2*(z*x+w*y),
73  2*(x*y+z*w), -x*x+y*y-z*z+w*w, 2*(y*z-x*w),
74  2*(z*x-w*y), 2*(y*z+w*x), -x*x-y*y+z*z+w*w;
75  return ret;
76  }
77 
78  geometry_msgs::Vector3 convertQuaternionToEulerAngle(geometry_msgs::Quaternion quat)
79  {
80  geometry_msgs::Vector3 ret;
81  tf2::Quaternion tf_quat(quat.x,quat.y,quat.z,quat.w);
82  tf2::Matrix3x3 mat(tf_quat);
83  double roll,pitch,yaw;
84  mat.getRPY(roll, pitch, yaw);
85  ret.x = roll;
86  ret.y = pitch;
87  ret.z = yaw;
88  /*
89  Eigen::Matrix3d m = getRotationMatrix(quat);
90  Eigen::Vector3d ea = m.eulerAngles(0, 1, 2);
91  ret.x = ea(0);
92  ret.y = ea(1);
93  ret.z = ea(2);
94  */
95  return ret;
96  }
97 
98  bool equals(double a,double b)
99  {
100  if (fabs(a - b) < DBL_EPSILON)
101  {
102  return true;
103  }
104  return false;
105  }
106 
107  bool equals(geometry_msgs::Quaternion quat1,geometry_msgs::Quaternion quat2)
108  {
109  if(equals(quat1.x,quat2.x) && equals(quat1.y,quat2.y) && equals(quat1.z,quat2.z) && equals(quat1.w,quat2.w))
110  {
111  return true;
112  }
113  return false;
114  }
115 
116  Eigen::MatrixXd convertToEigenMatrix(geometry_msgs::Quaternion quat)
117  {
118  Eigen::MatrixXd ret(4,1);
119  ret << quat.x,quat.y,quat.z,quat.w;
120  return ret;
121  }
122 
123  geometry_msgs::Quaternion conjugate(geometry_msgs::Quaternion quat1)
124  {
125  quat1.x = quat1.x * -1;
126  quat1.y = quat1.y * -1;
127  quat1.z = quat1.z * -1;
128  //quat1.w = quat1.w * -1;
129  return quat1;
130  }
131 
132  geometry_msgs::Quaternion rotation(geometry_msgs::Quaternion from,geometry_msgs::Quaternion rotation)
133  {
134  return from*rotation;
135  }
136 
137  geometry_msgs::Quaternion getRotation(geometry_msgs::Quaternion from,geometry_msgs::Quaternion to)
138  {
139  geometry_msgs::Quaternion ans;
140  ans = conjugate(from) * to;
141  return ans;
142  }
143 
144  geometry_msgs::Quaternion slerp(geometry_msgs::Quaternion quat1,geometry_msgs::Quaternion quat2,double t)
145  {
146  geometry_msgs::Quaternion q;
147  double qr = quat1.w * quat2.w + quat1.x * quat2.x + quat1.y * quat2.y + quat1.z * quat2.z;
148  double ss = (double)1.0 - qr * qr;
149  if (ss == (double)0.0)
150  {
151  q.w = quat1.w;
152  q.x = quat1.x;
153  q.y = quat1.y;
154  q.z = quat1.z;
155  return q;
156  }
157  else
158  {
159  double sp = std::sqrt(ss);
160  double ph = std::acos(qr);
161  double pt = ph * t;
162  double t1 = std::sin(pt) / sp;
163  double t0 = std::sin(ph - pt) / sp;
164 
165  q.w = quat1.w * t0 + quat2.w * t1;
166  q.x = quat1.x * t0 + quat2.x * t1;
167  q.y = quat1.y * t0 + quat2.y * t1;
168  q.z = quat1.z * t0 + quat2.z * t1;
169  return q;
170  }
171  }
172 }
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
geometry_msgs::Quaternion operator*(geometry_msgs::Quaternion quat1, geometry_msgs::Quaternion quat2)
Operator overload for geometry_msgs::Quaternion (Multiplication)
geometry_msgs::Quaternion convertEulerAngleToQuaternion(geometry_msgs::Vector3 euler)
convert Euler angles to Quaternion
Eigen::MatrixXd convertToEigenMatrix(geometry_msgs::Quaternion quat)
convert geometry_msgs::Quaternion to Eigen::MatrixXd
geometry_msgs::Quaternion getRotation(geometry_msgs::Quaternion from, geometry_msgs::Quaternion to)
Get the Rotation from 2 Quaternions.
geometry_msgs::Quaternion conjugate(geometry_msgs::Quaternion quat1)
get conjugate Quaternion
definitions of quaternion operation
namespace of quaternion_operation ROS package
geometry_msgs::Quaternion operator+(geometry_msgs::Quaternion quat1, geometry_msgs::Quaternion quat2)
Operator overload for geometry_msgs::Quaternion (Addition)
geometry_msgs::Quaternion rotation(geometry_msgs::Quaternion from, geometry_msgs::Quaternion rotation)
rotate Quaternion
geometry_msgs::Vector3 convertQuaternionToEulerAngle(geometry_msgs::Quaternion quat)
convert Quaternion to the Euler angle
bool equals(double a, double b)
checke 2 double values are equal or not
geometry_msgs::Quaternion slerp(geometry_msgs::Quaternion quat1, geometry_msgs::Quaternion quat2, double t)
Spherical linear interpolation function for geometry_msgs::Quaternion.
void getRPY(tf2Scalar &roll, tf2Scalar &pitch, tf2Scalar &yaw, unsigned int solution_number=1) const
Eigen::Matrix3d getRotationMatrix(geometry_msgs::Quaternion quat)
Get the Rotation Matrix from geometry_msgs::Quaternion.


quaternion_operation
Author(s):
autogenerated on Mon Feb 28 2022 23:20:03