RatsMatrix.h
Go to the documentation of this file.
00001 #ifndef RATSMATRIX_H
00002 #define RATSMATRIX_H
00003 #include <hrpUtil/Eigen3d.h>
00004 #include <iostream>
00005 
00006 namespace rats
00007 {
00008   inline bool eps_eq(const double a, const double b, const double eps = 0.001)
00009   {
00010     return fabs((a)-(b)) <= eps;
00011   };
00012   hrp::Vector3 matrix_log(const hrp::Matrix33& m);
00013 
00014   // matrix product using quaternion normalization
00015   void rotm3times (hrp::Matrix33& m12, const hrp::Matrix33& m1, const hrp::Matrix33& m2);
00016   void difference_rotation(hrp::Vector3& ret_dif_rot, const hrp::Matrix33& self_rot, const hrp::Matrix33& target_rot);
00017 
00018   struct coordinates {
00019     hrp::Vector3 pos;
00020     hrp::Matrix33 rot;
00021     coordinates() : pos(hrp::Vector3::Zero()), rot(hrp::Matrix33::Identity()) {};
00022     coordinates(const hrp::Vector3& p, const hrp::Matrix33& r) : pos(p), rot(r) {};
00023     coordinates(const hrp::Vector3& p) : pos(p), rot(hrp::Matrix33::Identity()) {};
00024     coordinates(const hrp::Matrix33& r) : pos(hrp::Vector3::Zero()), rot(r) {};
00025     coordinates(const coordinates& c) : pos(c.pos), rot(c.rot) {};
00026     virtual ~coordinates() {
00027     }
00028     coordinates& operator=(const coordinates& c) {
00029       if (this != &c) {
00030         pos = c.pos;
00031         rot = c.rot;
00032       }
00033       return *this;
00034     }
00035     void rotate_with_matrix (const hrp::Matrix33& mat, const std::string& wrt = ":local") {
00036       hrp::Matrix33 rot_org(rot);
00037       if (wrt == ":local") {              
00038         //                      alias(rot) = rot * mat;
00039         rotm3times(rot, rot_org, mat);
00040       } else if(wrt == ":world") {
00041         //                      alias(rot) = mat * rot;
00042         rotm3times(rot, mat, rot_org);
00043       } else {
00044         std::cerr << "**** invalid wrt! ****" << std::endl;
00045       }
00046     }
00047     void rotate (const double theta, const hrp::Vector3& axis, const std::string& wrt = ":local") {
00048       Eigen::AngleAxis<double> tmpr(theta, axis);
00049       rotate_with_matrix(tmpr.toRotationMatrix(), wrt);
00050     }
00051     /* void difference_rotation(hrp::Vector3& dif_rot, const coordinates& c) const { */
00052     /*   dif_rot = rot * matrix_log(rot.transpose() * c.rot); */
00053     /* } */
00054     void difference(hrp::Vector3& dif_pos, hrp::Vector3& dif_rot, const coordinates& c) const {
00055       dif_pos = c.pos - pos;
00056       difference_rotation(dif_rot, rot, c.rot);
00057     }
00058     //abc
00059     void inverse_transformation(coordinates& inv) const {
00060       inv.rot = rot.transpose();
00061       inv.pos = inv.rot*(-1 * pos);
00062     }
00063     void transformation(coordinates& tc, coordinates c, const std::string& wrt = ":local") const {
00064       tc = *this;
00065       inverse_transformation(tc);
00066       if (wrt == ":local") {
00067         tc.transform(c);
00068       } else if(wrt == ":world") {
00069         c.transform(tc);
00070         tc = c;
00071       } else {
00072         std::cerr << "**** invalid wrt! ****" << std::endl;
00073       }
00074     }
00075     void transform(const coordinates& c, const std::string& wrt = ":local") {
00076       if (wrt == ":local") {
00077         pos += rot * c.pos;
00078         // alias(rot) = rot * c.rot;
00079         hrp::Matrix33 rot_org(rot);
00080         rotm3times(rot, rot_org, c.rot);
00081       } else if (wrt == ":world") {
00082         hrp::Vector3 p(c.pos);
00083         hrp::Matrix33 r(c.rot);      
00084         p += r * pos; 
00085         rotm3times(r, c.rot, rot);      
00086         pos = p;
00087         rot = r;
00088       } else {
00089         std::cerr << "**** invalid wrt! ****" << std::endl;
00090       }
00091     }
00092   };
00093 
00094   void mid_rot(hrp::Matrix33& mid_rot, const double p, const hrp::Matrix33& rot1, const hrp::Matrix33& rot2, const double eps = 0.001);
00095   void mid_coords(coordinates& mid_coords, const double p, const coordinates& c1, const coordinates& c2, const double eps = 0.001);
00096 };
00097 #endif /* RATSMATRIX_H */


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55