Eigen3d.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  * General Robotix Inc. 
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     // E + a_hat*sin(q) + a_hat*a_hat*(1-cos(q))
00025     //
00026     //    |  0 -az  ay|
00027     // =E+| az   0 -ax|*s + a_hat*a_hat*v
00028     //    |-ay  ax   0|
00029     //
00030     //    |  0 -az  ay|     |-az*az-ay*ay        ax*ay        az*ax|
00031     // =E+| az   0 -ax|*s + |       ax*ay -az*az-ax*ax        ay*az|*v
00032     //    |-ay  ax   0|     |       az*ax        ay*az -ax*ax-ay*ay|
00033     //
00034     //  |1-az*az*v-ay*ay*v     -az*s+ax*ay*v      ay*s+az*ax*v|
00035     // =|     az*s+ax*ay*v 1-az*az*v-ax*ax*v     -ax*s+ay+az*v|
00036     //  |    -ay*s+az*ax*v      ax*s+ay*az*v 1-ax*ax*v-ay*ay*v|
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) {   //th=0,2PI;
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()) {   //th=PI
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         // cos(p) is nearly = 0
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); // -pi/2< p < pi/2
00112         
00113         roll = atan2(sp*m(0,1)+m(1,2),  // -cp*cp*sr*cy
00114                      sp*m(0,2)-m(1,1)); // -cp*cp*cr*cy
00115         
00116         if (m(0,0)>0.0) { // cy > 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), //sy*sp
00122                         sr*m(0,1)+cr*m(0,2));//cy*sp
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     //return all_elements( m * m.transpose() == Matrix33::Identity() );
00156 }
00157 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53