Tvmet3d.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  * General Robotix Inc.
9  */
10 
11 #include "Tvmet3d.h"
12 
13 using namespace tvmet;
14 using namespace hrp;
15 
16 
17 static const double PI = 3.14159265358979323846;
18 
19 
20 void hrp::calcRodrigues(Matrix33& out_R, const Vector3& axis, double q)
21 {
22  // E + a_hat*sin(q) + a_hat*a_hat*(1-cos(q))
23  //
24  // | 0 -az ay|
25  // =E+| az 0 -ax|*s + a_hat*a_hat*v
26  // |-ay ax 0|
27  //
28  // | 0 -az ay| |-az*az-ay*ay ax*ay az*ax|
29  // =E+| az 0 -ax|*s + | ax*ay -az*az-ax*ax ay*az|*v
30  // |-ay ax 0| | az*ax ay*az -ax*ax-ay*ay|
31  //
32  // |1-az*az*v-ay*ay*v -az*s+ax*ay*v ay*s+az*ax*v|
33  // =| az*s+ax*ay*v 1-az*az*v-ax*ax*v -ax*s+ay+az*v|
34  // | -ay*s+az*ax*v ax*s+ay*az*v 1-ax*ax*v-ay*ay*v|
35  //
36 
37  const double sth = sin(q);
38  const double vth = 1.0 - cos(q);
39 
40  double ax = axis(0);
41  double ay = axis(1);
42  double az = axis(2);
43 
44  const double axx = ax*ax*vth;
45  const double ayy = ay*ay*vth;
46  const double azz = az*az*vth;
47  const double axy = ax*ay*vth;
48  const double ayz = ay*az*vth;
49  const double azx = az*ax*vth;
50 
51  ax *= sth;
52  ay *= sth;
53  az *= sth;
54 
55  out_R = 1.0 - azz - ayy, -az + axy, ay + azx,
56  az + axy, 1.0 - azz - axx, -ax + ayz,
57  -ay + azx, ax + ayz, 1.0 - ayy - axx;
58 }
59 
60 
62 {
63  using ::std::numeric_limits;
64 
65  double alpha = (r(0,0) + r(1,1) + r(2,2) - 1.0) / 2.0;
66 
67  if(fabs(alpha - 1.0) < 1.0e-6) { //th=0,2PI;
68  return Vector3(0.0);
69 
70  } else {
71  double th = acos(alpha);
72  double s = sin(th);
73 
74  if (s < numeric_limits<double>::epsilon()) { //th=PI
75  return Vector3( sqrt((r(0,0)+1)*0.5)*th, sqrt((r(1,1)+1)*0.5)*th, sqrt((r(2,2)+1)*0.5)*th );
76  }
77 
78  double k = -0.5 * th / s;
79 
80  return Vector3( (r(1,2) - r(2,1)) * k,
81  (r(2,0) - r(0,2)) * k,
82  (r(0,1) - r(1,0)) * k );
83  }
84 }
85 
86 
88 {
89  double roll, pitch, yaw;
90 
91  if ((fabs(m(0,0))<fabs(m(2,0))) && (fabs(m(1,0))<fabs(m(2,0)))) {
92  // cos(p) is nearly = 0
93  double sp = -m(2,0);
94  if (sp < -1.0) {
95  sp = -1;
96  } else if (sp > 1.0) {
97  sp = 1;
98  }
99  pitch = asin(sp); // -pi/2< p < pi/2
100 
101  roll = atan2(sp*m(0,1)+m(1,2), // -cp*cp*sr*cy
102  sp*m(0,2)-m(1,1)); // -cp*cp*cr*cy
103 
104  if (m(0,0)>0.0) { // cy > 0
105  (roll < 0.0) ? (roll += PI) : (roll -= PI);
106  }
107  double sr=sin(roll), cr=cos(roll);
108  if (sp > 0.0) {
109  yaw = atan2(sr*m(1,1)+cr*m(1,2), //sy*sp
110  sr*m(0,1)+cr*m(0,2));//cy*sp
111  } else {
112  yaw = atan2(-sr*m(1,1)-cr*m(1,2),
113  -sr*m(0,1)-cr*m(0,2));
114  }
115  } else {
116  yaw = atan2(m(1,0), m(0,0));
117  const double sa = sin(yaw);
118  const double ca = cos(yaw);
119  pitch = atan2(-m(2,0), ca*m(0,0)+sa*m(1,0));
120  roll = atan2(sa*m(0,2)-ca*m(1,2), -sa*m(0,1)+ca*m(1,1));
121  }
122  return Vector3(roll, pitch, yaw);
123 }
124 
125 
126 void hrp::calcRotFromRpy(Matrix33& out_R, double r, double p, double y)
127 {
128  const double cr = cos(r), sr = sin(r), cp = cos(p), sp = sin(p), cy = cos(y), sy = sin(y);
129  out_R(0,0)= cp*cy;
130  out_R(0,1)= sr*sp*cy - cr*sy;
131  out_R(0,2)= cr*sp*cy + sr*sy;
132  out_R(1,0)= cp*sy;
133  out_R(1,1)= sr*sp*sy + cr*cy;
134  out_R(1,2)= cr*sp*sy - sr*cy;
135  out_R(2,0)= -sp;
136  out_R(2,1)= sr*cp;
137  out_R(2,2)= cr*cp;
138 }
139 
140 
141 void hrp::calcInverse(Matrix33& inv, const Matrix33& m)
142 {
143  double det = m(0,0)*(m(1,1)*m(2,2)-m(1,2)*m(2,1)) - m(0,1)*(m(1,0)*m(2,2)-m(1,2)*m(2,0)) + m(0,2)*(m(1,0)*m(2,1)-m(1,1)*m(2,0));
144 
145  if(fabs(det) < std::numeric_limits<double>::epsilon()){
146  throw std::string("Invrse matrix cannot be calculated.");
147  }
148  else{
149  inv =
150  (m(1,1)*m(2,2)-m(1,2)*m(2,1)) / det, (m(0,2)*m(2,1)-m(0,1)*m(2,2)) / det, (m(0,1)*m(1,2)-m(0,2)*m(1,1)) / det,
151  (m(1,2)*m(2,0)-m(1,0)*m(2,2)) / det, (m(0,0)*m(2,2)-m(0,2)*m(2,0)) / det, (m(0,2)*m(1,0)-m(0,0)*m(1,2)) / det,
152  (m(1,0)*m(2,1)-m(1,1)*m(2,0)) / det, (m(0,1)*m(2,0)-m(0,0)*m(2,1)) / det, (m(0,0)*m(1,1)-m(0,1)*m(1,0)) / det;
153  }
154 }
155 
157  return all_elements( m * trans(m) == tvmet::identity<Matrix33>() );
158 }
HRP_UTIL_EXPORT bool isOrthogonalMatrix(Matrix33 &m)
Definition: Eigen3d.cpp:152
HRP_UTIL_EXPORT void calcRotFromRpy(Matrix33 &out_R, double r, double p, double y)
Definition: Eigen3d.cpp:138
void calcInverse(Matrix33 &inv, const Matrix33 &m)
Definition: Tvmet2Eigen.h:32
png_infop png_bytep * trans
Definition: png.h:2435
double det(const dmatrix &_a)
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
OpenHRP::vector3 Vector3
HRP_UTIL_EXPORT Vector3 rpyFromRot(const Matrix33 &m)
Definition: Eigen3d.cpp:99
fMat inv(const fMat &mat)
Definition: fMatrix.cpp:176
HRP_UTIL_EXPORT void calcRodrigues(Matrix33 &out_R, const Vector3 &axis, double q)
Definition: Eigen3d.cpp:22
HRP_UTIL_EXPORT Vector3 omegaFromRot(const Matrix33 &r)
Definition: Eigen3d.cpp:63
static const double PI
Definition: Tvmet3d.cpp:17
* y
Definition: IceUtils.h:97


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:05