00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #include <iostream>
00012 #include <iomanip>
00013 #include "Eigen3d.h"
00014
00015 using namespace std;
00016 using namespace hrp;
00017
00018
00019 static const double PI = 3.14159265358979323846;
00020
00021
00022 void hrp::calcRodrigues(Matrix33& out_R, const Vector3& axis, double q)
00023 {
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 const double sth = sin(q);
00040 const double vth = 1.0 - cos(q);
00041
00042 double ax = axis(0);
00043 double ay = axis(1);
00044 double az = axis(2);
00045
00046 const double axx = ax*ax*vth;
00047 const double ayy = ay*ay*vth;
00048 const double azz = az*az*vth;
00049 const double axy = ax*ay*vth;
00050 const double ayz = ay*az*vth;
00051 const double azx = az*ax*vth;
00052
00053 ax *= sth;
00054 ay *= sth;
00055 az *= sth;
00056
00057 out_R << 1.0 - azz - ayy, -az + axy, ay + azx,
00058 az + axy, 1.0 - azz - axx, -ax + ayz,
00059 -ay + azx, ax + ayz, 1.0 - ayy - axx;
00060 }
00061
00062
00063 Vector3 hrp::omegaFromRot(const Matrix33& r)
00064 {
00065 using ::std::numeric_limits;
00066
00067 double alpha = (r(0,0) + r(1,1) + r(2,2) - 1.0) / 2.0;
00068
00069 if (alpha > 1.0) {
00070 if (alpha > 1.0 + 1.0e-6) {
00071 cerr << scientific << setprecision(16);
00072 cerr << "alpha exceeded the upper limit in omegaFromRot" << endl;
00073 cerr << "alpha=" << alpha << endl;
00074 cerr << "r=" << r << endl;
00075 }
00076 alpha = 1.0;
00077 }
00078
00079 if(fabs(alpha - 1.0) < 1.0e-12) {
00080 return Vector3::Zero();
00081
00082 } else {
00083 double th = acos(alpha);
00084 double s = sin(th);
00085
00086 if (s < numeric_limits<double>::epsilon()) {
00087 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 );
00088 }
00089
00090 double k = -0.5 * th / s;
00091
00092 return Vector3( (r(1,2) - r(2,1)) * k,
00093 (r(2,0) - r(0,2)) * k,
00094 (r(0,1) - r(1,0)) * k );
00095 }
00096 }
00097
00098
00099 Vector3 hrp::rpyFromRot(const Matrix33& m)
00100 {
00101 double roll, pitch, yaw;
00102
00103 if ((fabs(m(0,0))<fabs(m(2,0))) && (fabs(m(1,0))<fabs(m(2,0)))) {
00104
00105 double sp = -m(2,0);
00106 if (sp < -1.0) {
00107 sp = -1;
00108 } else if (sp > 1.0) {
00109 sp = 1;
00110 }
00111 pitch = asin(sp);
00112
00113 roll = atan2(sp*m(0,1)+m(1,2),
00114 sp*m(0,2)-m(1,1));
00115
00116 if (m(0,0)>0.0) {
00117 (roll < 0.0) ? (roll += PI) : (roll -= PI);
00118 }
00119 double sr=sin(roll), cr=cos(roll);
00120 if (sp > 0.0) {
00121 yaw = atan2(sr*m(1,1)+cr*m(1,2),
00122 sr*m(0,1)+cr*m(0,2));
00123 } else {
00124 yaw = atan2(-sr*m(1,1)-cr*m(1,2),
00125 -sr*m(0,1)-cr*m(0,2));
00126 }
00127 } else {
00128 yaw = atan2(m(1,0), m(0,0));
00129 const double sa = sin(yaw);
00130 const double ca = cos(yaw);
00131 pitch = atan2(-m(2,0), ca*m(0,0)+sa*m(1,0));
00132 roll = atan2(sa*m(0,2)-ca*m(1,2), -sa*m(0,1)+ca*m(1,1));
00133 }
00134 return Vector3(roll, pitch, yaw);
00135 }
00136
00137
00138 void hrp::calcRotFromRpy(Matrix33& out_R, double r, double p, double y)
00139 {
00140 const double cr = cos(r), sr = sin(r), cp = cos(p), sp = sin(p), cy = cos(y), sy = sin(y);
00141 out_R(0,0)= cp*cy;
00142 out_R(0,1)= sr*sp*cy - cr*sy;
00143 out_R(0,2)= cr*sp*cy + sr*sy;
00144 out_R(1,0)= cp*sy;
00145 out_R(1,1)= sr*sp*sy + cr*cy;
00146 out_R(1,2)= cr*sp*sy - sr*cy;
00147 out_R(2,0)= -sp;
00148 out_R(2,1)= sr*cp;
00149 out_R(2,2)= cr*cp;
00150 }
00151
00152 bool hrp::isOrthogonalMatrix(Matrix33& m){
00153 Matrix33 w(m * m.transpose() - Matrix33::Identity());
00154 return (abs(w.array())<1.0e-12).all();
00155
00156 }
00157