RatsMatrix.cpp
Go to the documentation of this file.
1 #include "RatsMatrix.h"
2 
3 namespace rats
4 {
6  hrp::Vector3 mlog;
7  double q0, th;
8  hrp::Vector3 q;
9  double norm;
10 
11  Eigen::Quaternion<double> eiq(m);
12  q0 = eiq.w();
13  q = eiq.vec();
14  norm = q.norm();
15  if (norm > 0) {
16  if ((q0 > 1.0e-10) || (q0 < -1.0e-10)) {
17  th = 2 * std::atan(norm / q0);
18  } else if (q0 > 0) {
19  th = M_PI / 2;
20  } else {
21  th = -M_PI / 2;
22  }
23  mlog = (th / norm) * q ;
24  } else {
25  mlog = hrp::Vector3::Zero();
26  }
27  return mlog;
28  }
29 
30  // matrix product using quaternion normalization
31  void rotm3times (hrp::Matrix33& m12, const hrp::Matrix33& m1, const hrp::Matrix33& m2) {
32  Eigen::Quaternion<double> eiq1(m1);
33  Eigen::Quaternion<double> eiq2(m2);
34  Eigen::Quaternion<double> eiq3;
35  eiq3 = eiq1 * eiq2;
36  eiq3.normalize();
37  m12 = eiq3.toRotationMatrix();
38  }
39 
40  void difference_rotation(hrp::Vector3& ret_dif_rot, const hrp::Matrix33& self_rot, const hrp::Matrix33& target_rot)
41  {
42  //ret_dif_rot = self_rot * hrp::omegaFromRot(self_rot.transpose() * target_rot);
43  ret_dif_rot = self_rot * hrp::Vector3(rats::matrix_log(hrp::Matrix33(self_rot.transpose() * target_rot)));
44  }
45 
46  void mid_rot(hrp::Matrix33& mid_rot, const double p, const hrp::Matrix33& rot1, const hrp::Matrix33& rot2, const double eps) {
47  hrp::Matrix33 r(rot1.transpose() * rot2);
48  hrp::Vector3 omega(matrix_log(r));
49  if (eps_eq(omega.norm(),0.0, eps)) { // c1.rot and c2.rot are same
50  mid_rot = rot1;
51  } else {
52  hrp::calcRodrigues(r, omega.normalized(), omega.norm()*p);
53  //mid_rot = c1.rot * r;
54  rotm3times(mid_rot, rot1, r);
55  }
56  };
57 
58  void mid_coords(coordinates& mid_coords, const double p, const coordinates& c1, const coordinates& c2, const double eps) {
59  mid_coords.pos = (1 - p) * c1.pos + p * c2.pos;
60  mid_rot(mid_coords.rot, p, c1.rot, c2.rot, eps);
61  };
62 
63 }
void difference_rotation(hrp::Vector3 &ret_dif_rot, const hrp::Matrix33 &self_rot, const hrp::Matrix33 &target_rot)
Definition: RatsMatrix.cpp:40
hrp::Matrix33 rot
Definition: RatsMatrix.h:20
Eigen::Vector3d Vector3
Eigen::Matrix3d Matrix33
void rotm3times(hrp::Matrix33 &m12, const hrp::Matrix33 &m1, const hrp::Matrix33 &m2)
Definition: RatsMatrix.cpp:31
HRP_UTIL_EXPORT void calcRodrigues(Matrix44 &out_R, const Vector3 &axis, double q)
def norm(a)
#define M_PI
bool eps_eq(const double a, const double b, const double eps=0.001)
Definition: RatsMatrix.h:8
hrp::Vector3 matrix_log(const hrp::Matrix33 &m)
Definition: RatsMatrix.cpp:5
hrp::Vector3 pos
Definition: RatsMatrix.h:19
void mid_rot(hrp::Matrix33 &mid_rot, const double p, const hrp::Matrix33 &rot1, const hrp::Matrix33 &rot2, const double eps)
Definition: RatsMatrix.cpp:46
void mid_coords(coordinates &mid_coords, const double p, const coordinates &c1, const coordinates &c2, const double eps)
Definition: RatsMatrix.cpp:58


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20