RatsMatrix.h
Go to the documentation of this file.
1 #ifndef RATSMATRIX_H
2 #define RATSMATRIX_H
3 #include <hrpUtil/Eigen3d.h>
4 #include <iostream>
5 
6 namespace rats
7 {
8  inline bool eps_eq(const double a, const double b, const double eps = 0.001)
9  {
10  return fabs((a)-(b)) <= eps;
11  };
13 
14  // matrix product using quaternion normalization
15  void rotm3times (hrp::Matrix33& m12, const hrp::Matrix33& m1, const hrp::Matrix33& m2);
16  void difference_rotation(hrp::Vector3& ret_dif_rot, const hrp::Matrix33& self_rot, const hrp::Matrix33& target_rot);
17 
18  struct coordinates {
21  coordinates() : pos(hrp::Vector3::Zero()), rot(hrp::Matrix33::Identity()) {};
22  coordinates(const hrp::Vector3& p, const hrp::Matrix33& r) : pos(p), rot(r) {};
23  coordinates(const hrp::Vector3& p) : pos(p), rot(hrp::Matrix33::Identity()) {};
24  coordinates(const hrp::Matrix33& r) : pos(hrp::Vector3::Zero()), rot(r) {};
25  coordinates(const coordinates& c) : pos(c.pos), rot(c.rot) {};
26  virtual ~coordinates() {
27  }
29  if (this != &c) {
30  pos = c.pos;
31  rot = c.rot;
32  }
33  return *this;
34  }
35  void rotate_with_matrix (const hrp::Matrix33& mat, const std::string& wrt = ":local") {
36  hrp::Matrix33 rot_org(rot);
37  if (wrt == ":local") {
38  // alias(rot) = rot * mat;
39  rotm3times(rot, rot_org, mat);
40  } else if(wrt == ":world") {
41  // alias(rot) = mat * rot;
42  rotm3times(rot, mat, rot_org);
43  } else {
44  std::cerr << "**** invalid wrt! ****" << std::endl;
45  }
46  }
47  void rotate (const double theta, const hrp::Vector3& axis, const std::string& wrt = ":local") {
48  Eigen::AngleAxis<double> tmpr(theta, axis);
49  rotate_with_matrix(tmpr.toRotationMatrix(), wrt);
50  }
51  /* void difference_rotation(hrp::Vector3& dif_rot, const coordinates& c) const { */
52  /* dif_rot = rot * matrix_log(rot.transpose() * c.rot); */
53  /* } */
54  void difference(hrp::Vector3& dif_pos, hrp::Vector3& dif_rot, const coordinates& c) const {
55  dif_pos = c.pos - pos;
56  difference_rotation(dif_rot, rot, c.rot);
57  }
58  //abc
60  inv.rot = rot.transpose();
61  inv.pos = inv.rot*(-1 * pos);
62  }
63  void transformation(coordinates& tc, coordinates c, const std::string& wrt = ":local") const {
64  tc = *this;
66  if (wrt == ":local") {
67  tc.transform(c);
68  } else if(wrt == ":world") {
69  c.transform(tc);
70  tc = c;
71  } else {
72  std::cerr << "**** invalid wrt! ****" << std::endl;
73  }
74  }
75  void transform(const coordinates& c, const std::string& wrt = ":local") {
76  if (wrt == ":local") {
77  pos += rot * c.pos;
78  // alias(rot) = rot * c.rot;
79  hrp::Matrix33 rot_org(rot);
80  rotm3times(rot, rot_org, c.rot);
81  } else if (wrt == ":world") {
82  hrp::Vector3 p(c.pos);
83  hrp::Matrix33 r(c.rot);
84  p += r * pos;
85  rotm3times(r, c.rot, rot);
86  pos = p;
87  rot = r;
88  } else {
89  std::cerr << "**** invalid wrt! ****" << std::endl;
90  }
91  }
92  };
93 
94  void mid_rot(hrp::Matrix33& mid_rot, const double p, const hrp::Matrix33& rot1, const hrp::Matrix33& rot2, const double eps = 0.001);
95  void mid_coords(coordinates& mid_coords, const double p, const coordinates& c1, const coordinates& c2, const double eps = 0.001);
96 };
97 #endif /* RATSMATRIX_H */
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
coordinates(const hrp::Matrix33 &r)
Definition: RatsMatrix.h:24
void transform(const coordinates &c, const std::string &wrt=":local")
Definition: RatsMatrix.h:75
OpenHRP::matrix33 Matrix33
Eigen::Vector3d Vector3
coordinates(const coordinates &c)
Definition: RatsMatrix.h:25
Eigen::Matrix3d Matrix33
OpenHRP::vector3 Vector3
coordinates(const hrp::Vector3 &p, const hrp::Matrix33 &r)
Definition: RatsMatrix.h:22
coordinates & operator=(const coordinates &c)
Definition: RatsMatrix.h:28
void rotm3times(hrp::Matrix33 &m12, const hrp::Matrix33 &m1, const hrp::Matrix33 &m2)
Definition: RatsMatrix.cpp:31
void transformation(coordinates &tc, coordinates c, const std::string &wrt=":local") const
Definition: RatsMatrix.h:63
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
void difference(hrp::Vector3 &dif_pos, hrp::Vector3 &dif_rot, const coordinates &c) const
Definition: RatsMatrix.h:54
hrp::Vector3 pos
Definition: RatsMatrix.h:19
void rotate_with_matrix(const hrp::Matrix33 &mat, const std::string &wrt=":local")
Definition: RatsMatrix.h:35
void rotate(const double theta, const hrp::Vector3 &axis, const std::string &wrt=":local")
Definition: RatsMatrix.h:47
#define eps
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
coordinates(const hrp::Vector3 &p)
Definition: RatsMatrix.h:23
virtual ~coordinates()
Definition: RatsMatrix.h:26
void mid_coords(coordinates &mid_coords, const double p, const coordinates &c1, const coordinates &c2, const double eps)
Definition: RatsMatrix.cpp:58
void inverse_transformation(coordinates &inv) const
Definition: RatsMatrix.h:59


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