Added gtest for AxisAng6, MatrixLog6; Fix a bug in Matrixlog3

Geonhee-LEE · Feb 17, 2019 · 0aa3a94 · 0aa3a94
1 parent 0102485
commit 0aa3a94
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 2 deletions.
diff --git a/src/lib_test.cpp b/src/lib_test.cpp
@@ -175,3 +175,31 @@ TEST(MRTest, FKInSpaceTest)
 
  ASSERT_TRUE(FKCal.isApprox(result, 4));
 }
+
+TEST(MRTest, AxisAng6Test) {
+ Eigen::VectorXd input(6);
+ Eigen::VectorXd result(7);
+ input << 1.0, 0.0, 0.0, 1.0, 1.0, 2.0, 3.0;
+ result << 1.0, 0.0, 0.0, 1.0, 1.0, 2.0, 3.0, 1.0;
+
+ Eigen::VectorXd output = mr::AxisAng6(input);
+ ASSERT_TRUE(output.isApprox(result, 4));
+}
+
+TEST(MRTest, MatrixLog6Test) {
+ Eigen::MatrixXd Tinput(4, 4);
+ Eigen::MatrixXd result(4, 4);
+ Tinput << 1, 0, 0, 0,
+ 0, 0, -1, 0,
+ 0, 1, 0, 3,
+ 0, 0, 0, 1;
+
+ result << 0, 0, 0, 0,
+ 0, 0, -1.57079633, 2.35619449,
+ 0, 1.57079633, 0, 2.35619449,
+ 0, 0, 0, 0;
+
+ Eigen::MatrixXd Toutput = mr::MatrixLog6(Tinput);
+ ASSERT_TRUE(Toutput.isApprox(result, 4));
+}
+
diff --git a/src/modern_robotics.cpp b/src/modern_robotics.cpp
@@ -122,7 +122,7 @@ namespace mr {
  else if (!NearZero(1 + R(1, 1)))
  omg = (1.0 / std::sqrt(2 * (1 + R(1, 1))))*Eigen::Vector3d(R(0, 1), 1 + R(1, 1), R(2, 1));
  else
- omg = (1.0 / std::sqrt(2 * (1 + R(0, 0))))*Eigen::Vector3cd(1 + R(0, 0), R(1, 0), R(2, 0));
+ omg = (1.0 / std::sqrt(2 * (1 + R(0, 0))))*Eigen::Vector3d(1 + R(0, 0), R(1, 0), R(2, 0));
  m_ret = VecToso3(M_PI * omg);
  return m_ret;
  }
@@ -362,7 +362,7 @@ namespace mr {
  }
 
  Eigen::VectorXd AxisAng6(const Eigen::VectorXd& expc6) {
- Eigen::VectorXd v_ret;
+ Eigen::VectorXd v_ret(7);
  double theta = Eigen::Vector3d(expc6(0), expc6(1), expc6(2)).norm();
  if (NearZero(theta))
  theta = Eigen::Vector3d(expc6(3), expc6(4), expc6(5)).norm();