Refactor to fix API for unit tests and utilities

RethinkRobotics-opensource · May 22, 2018 · 360d0e1 · 360d0e1
1 parent 615164f
commit 360d0e1
Show file tree

Hide file tree

Showing 23 changed files with 69 additions and 35 deletions.
diff --git a/sns_ik_lib/CMakeLists.txt b/sns_ik_lib/CMakeLists.txt
@@ -39,25 +39,32 @@ add_library(sns_ik
             src/fsns_velocity_ik.cpp
             src/osns_sm_velocity_ik.cpp
             src/osns_velocity_ik.cpp
-            src/rng_utilities.cpp
-            src/robot_model.cpp
             src/sns_ik_math_utils.cpp
             src/sns_ik.cpp
             src/sns_position_ik.cpp
-            src/sns_velocity_ik.cpp)
+            src/sns_velocity_ik.cpp
+            test/rng_utilities.cpp
+            test/robot_model.cpp)
 target_link_libraries(sns_ik ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})
 
 install(TARGETS sns_ik LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
 install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
 
 # Unit tests (google framework)
 if (CATKIN_ENABLE_TESTING)
+
+  add_library(sns_ik_test
+            test/rng_utilities.cpp
+            test/robot_model.cpp)
+  target_link_libraries(sns_ik_test sns_ik ${catkin_LIBRARIES})
+
   catkin_add_gtest(rng_utilities_test test/rng_utilities_test.cpp)
+  target_link_libraries(rng_utilities_test sns_ik ${catkin_LIBRARIES})
   catkin_add_gtest(sns_ik_math_utils_test test/sns_ik_math_utils_test.cpp)
+  target_link_libraries(sns_ik_math_utils_test sns_ik ${catkin_LIBRARIES})
   catkin_add_gtest(sns_ik_pos_test test/sns_ik_pos_test.cpp)
+  target_link_libraries(sns_ik_pos_test sns_ik ${catkin_LIBRARIES})
   catkin_add_gtest(sns_ik_vel_test test/sns_ik_vel_test.cpp)
-  target_link_libraries(rng_utilities_test ${catkin_LIBRARIES} sns_ik)
-  target_link_libraries(sns_ik_math_utils_test ${catkin_LIBRARIES} sns_ik)
-  target_link_libraries(sns_ik_pos_test ${catkin_LIBRARIES} sns_ik)
-  target_link_libraries(sns_ik_vel_test ${catkin_LIBRARIES} sns_ik)
+  target_link_libraries(sns_ik_vel_test sns_ik ${catkin_LIBRARIES})
+
 endif()
diff --git a/sns_ik_lib/include/sns_ik/fosns_velocity_ik.hpp b/sns_ik_lib/include/sns_ik/fosns_velocity_ik.hpp
@@ -26,7 +26,6 @@
 #include <Eigen/Dense>
 #include <forward_list>
 
-#include "sns_ik/sns_ik_math_utils.hpp"
 #include "sns_ik/sns_velocity_ik.hpp"
 #include "sns_ik/fsns_velocity_ik.hpp"
 

diff --git a/sns_ik_lib/include/sns_ik/fsns_velocity_ik.hpp b/sns_ik_lib/include/sns_ik/fsns_velocity_ik.hpp
@@ -25,7 +25,6 @@
 
 #include <Eigen/Dense>
 
-#include "sns_ik/sns_ik_math_utils.hpp"
 #include "sns_ik/sns_velocity_ik.hpp"
 
 namespace sns_ik {

diff --git a/sns_ik_lib/include/sns_ik/osns_sm_velocity_ik.hpp b/sns_ik_lib/include/sns_ik/osns_sm_velocity_ik.hpp
@@ -25,7 +25,6 @@
 
 #include <Eigen/Dense>
 
-#include "sns_ik/sns_ik_math_utils.hpp"
 #include "sns_ik/osns_velocity_ik.hpp"
 
 namespace sns_ik {

diff --git a/sns_ik_lib/include/sns_ik/osns_velocity_ik.hpp b/sns_ik_lib/include/sns_ik/osns_velocity_ik.hpp
@@ -25,7 +25,6 @@
 
 #include <Eigen/Dense>
 
-#include "sns_ik/sns_ik_math_utils.hpp"
 #include "sns_ik/sns_velocity_ik.hpp"
 
 namespace sns_ik {

diff --git a/sns_ik_lib/include/sns_ik/sns_position_ik.hpp b/sns_ik_lib/include/sns_ik/sns_position_ik.hpp
@@ -26,8 +26,6 @@
 #include <kdl/chainjnttojacsolver.hpp>
 #include <kdl/chainfksolverpos_recursive.hpp>
 
-#include "sns_ik/sns_ik_math_utils.hpp"
-
 namespace sns_ik {
 
 // Forward declare SNS Velocity Base Class

diff --git a/sns_ik_lib/include/sns_ik/sns_velocity_ik.hpp b/sns_ik_lib/include/sns_ik/sns_velocity_ik.hpp
@@ -25,8 +25,6 @@
 
 #include <Eigen/Dense>
 
-#include "sns_ik/sns_ik_math_utils.hpp"
-
 namespace sns_ik {
 
 /*! \struct Task

diff --git a/sns_ik_lib/src/fosns_velocity_ik.cpp b/sns_ik_lib/src/fosns_velocity_ik.cpp
@@ -24,6 +24,8 @@
 
 #include <ros/console.h>
 
+#include "sns_ik_math_utils.hpp"
+
 namespace sns_ik {
 
 FOSNSVelocityIK::FOSNSVelocityIK(int dof, double loop_period) :

diff --git a/sns_ik_lib/src/fsns_velocity_ik.cpp b/sns_ik_lib/src/fsns_velocity_ik.cpp
@@ -22,6 +22,8 @@
 
 #include <sns_ik/fsns_velocity_ik.hpp>
 
+#include "sns_ik_math_utils.hpp"
+
 namespace sns_ik {
 
 FSNSVelocityIK::FSNSVelocityIK(int dof, double loop_period) :

diff --git a/sns_ik_lib/src/osns_sm_velocity_ik.cpp b/sns_ik_lib/src/osns_sm_velocity_ik.cpp
@@ -22,6 +22,8 @@
 
 #include <sns_ik/osns_sm_velocity_ik.hpp>
 
+#include "sns_ik_math_utils.hpp"
+
 namespace sns_ik {
 
 OSNS_sm_VelocityIK::OSNS_sm_VelocityIK(int dof, double loop_period) :

diff --git a/sns_ik_lib/src/osns_velocity_ik.cpp b/sns_ik_lib/src/osns_velocity_ik.cpp
@@ -22,6 +22,8 @@
 
 #include <sns_ik/osns_velocity_ik.hpp>
 
+#include "sns_ik_math_utils.hpp"
+
 namespace sns_ik {
 
 OSNSVelocityIK::OSNSVelocityIK(int dof, double loop_period) :

diff --git a/sns_ik_lib/src/sns_ik_math_utils.cpp b/sns_ik_lib/src/sns_ik_math_utils.cpp
@@ -21,8 +21,9 @@
  */
 
 #include <ros/console.h>
+#include <limits>
 
-#include <sns_ik/sns_ik_math_utils.hpp>
+#include "sns_ik_math_utils.hpp"
 
 namespace {
   const double EPSQ = 1e-10;
@@ -231,9 +232,16 @@ bool isIdentity(const Eigen::MatrixXd &A) {
 
 /*************************************************************************************************/
 
-void pseudoInverse(const Eigen::MatrixXd& A, double eps, Eigen::MatrixXd* invA,
+bool pseudoInverse(const Eigen::MatrixXd& A, double eps, Eigen::MatrixXd* invA,
                    int* rank, bool* damped)
 {
+  // Input validation  (both rank and damped are allowed to be nullptr)
+  if (!invA) { ROS_ERROR("invA is nullptr!"); return false; }
+  if (eps < std::numeric_limits<double>::epsilon()) {
+    ROS_ERROR("Bad input:  eps (%e) must be positive!", eps);
+    return false;
+  }
+
   // Compute the singular value decomposition:
   Eigen::JacobiSVD<Eigen::MatrixXd> svd_A(A.transpose(), Eigen::ComputeThinU | Eigen::ComputeThinV);
   Eigen::VectorXd sigma = svd_A.singularValues();
@@ -271,6 +279,7 @@ void pseudoInverse(const Eigen::MatrixXd& A, double eps, Eigen::MatrixXd* invA,
   // Optional outputs:
   if (rank) { *rank = rankA; }
   if (damped) { *damped = !fullRank; }
+  return true;
 }
 
 /*************************************************************************************************/

diff --git a/..._lib/include/sns_ik/sns_ik_math_utils.hpp → sns_ik_lib/src/sns_ik_math_utils.hpp b/..._lib/include/sns_ik/sns_ik_math_utils.hpp → sns_ik_lib/src/sns_ik_math_utils.hpp
@@ -26,6 +26,8 @@
 
 #include <Eigen/Dense>
 
+#include "sns_ik_math_utils.hpp"
+
 namespace sns_ik {
 
 static const double INF = std::numeric_limits<double>::max();
@@ -163,8 +165,9 @@ bool isIdentity(const Eigen::MatrixXd &A);
  * @param[out] invA: pseudo-inverse of the matrix
  * @param[out, opt] rank: the rank of matrix A
  * @param[out, opt] damped: true if a damped pseudo-inverse was used
+ * @return: true iff successful
  */
-void pseudoInverse(const Eigen::MatrixXd& A, double eps, Eigen::MatrixXd* invA,
+bool pseudoInverse(const Eigen::MatrixXd& A, double eps, Eigen::MatrixXd* invA,
                    int* rank = nullptr, bool* damped = nullptr);
 
 }  // namespace sns_ik

diff --git a/sns_ik_lib/src/sns_position_ik.cpp b/sns_ik_lib/src/sns_position_ik.cpp
@@ -21,6 +21,8 @@
 #include <sns_ik/sns_velocity_ik.hpp>
 #include <ros/console.h>
 
+#include "sns_ik_math_utils.hpp"
+
 namespace sns_ik {
 
 SNSPositionIK::SNSPositionIK(KDL::Chain chain, std::shared_ptr<SNSVelocityIK> velocity_ik, double eps) :

diff --git a/sns_ik_lib/src/sns_velocity_ik.cpp b/sns_ik_lib/src/sns_velocity_ik.cpp
@@ -24,6 +24,8 @@
 
 #include <ros/console.h>
 
+#include "sns_ik_math_utils.hpp"
+
 namespace sns_ik {
 
 SNSVelocityIK::SNSVelocityIK(int dof, double loop_period) :

diff --git a/sns_ik_lib/src/rng_utilities.cpp → sns_ik_lib/test/rng_utilities.cpp b/sns_ik_lib/src/rng_utilities.cpp → sns_ik_lib/test/rng_utilities.cpp
@@ -13,7 +13,7 @@
  *    limitations under the License.
  */
 
-#include <sns_ik/rng_utilities.hpp>
+#include "rng_utilities.hpp"
 
 #include <random>
 #include <ros/console.h>

diff --git a/sns_ik_lib/include/sns_ik/rng_utilities.hpp → sns_ik_lib/test/rng_utilities.hpp b/sns_ik_lib/include/sns_ik/rng_utilities.hpp → sns_ik_lib/test/rng_utilities.hpp
diff --git a/sns_ik_lib/test/rng_utilities_test.cpp b/sns_ik_lib/test/rng_utilities_test.cpp
@@ -1,5 +1,5 @@
 /*! \file rng_utilities_test.cpp
- * \brief Unit Test: sns_ik_math_utils
+ * \brief Unit Test: rng_utilities
  * \author Matthew Kelly
  *
  * Unit tests for the rng_utilities suite of functions.
@@ -24,7 +24,7 @@
 #include <Eigen/Dense>
 #include <ros/console.h>
 
-#include <sns_ik/rng_utilities.hpp>
+#include "rng_utilities.hpp"
 
 /*************************************************************************************************/
 

diff --git a/sns_ik_lib/src/robot_model.cpp → sns_ik_lib/test/robot_model.cpp b/sns_ik_lib/src/robot_model.cpp → sns_ik_lib/test/robot_model.cpp
@@ -21,7 +21,7 @@
  *    limitations under the License.
  */
 
-#include <sns_ik/robot_model.hpp>
+#include "robot_model.hpp"
 
 #include <ros/console.h>
 

diff --git a/sns_ik_lib/include/sns_ik/robot_model.hpp → sns_ik_lib/test/robot_model.hpp b/sns_ik_lib/include/sns_ik/robot_model.hpp → sns_ik_lib/test/robot_model.hpp
@@ -35,7 +35,7 @@ namespace robot_model {
 * This function returns a KDL chain object that represents the kinematics of a Sawyer Robot arm.
 * Full model description at: https://github.com/RethinkRobotics/sawyer_robot
 * @param[out] jointNames: names of all non-trivial joints
-* @return: kinematic chain betweem right_arm_mount and right_hand for Sawyer.
+* @return: kinematic chain between right_arm_mount and right_hand for Sawyer.
 */
 KDL::Chain getSawyerKdlChain(std::vector<std::string>* jointNames);
 

diff --git a/sns_ik_lib/test/sns_ik_math_utils_test.cpp b/sns_ik_lib/test/sns_ik_math_utils_test.cpp
@@ -22,8 +22,8 @@
 #include <Eigen/Dense>
 #include <ros/console.h>
 
-#include <sns_ik/rng_utilities.hpp>
-#include <sns_ik/sns_ik_math_utils.hpp>
+#include "rng_utilities.hpp"
+#include "../src/sns_ik_math_utils.hpp"
 
 /*************************************************************************************************
  *                               Utilities Functions                                             *
@@ -364,7 +364,7 @@ TEST(sns_ik_math_utils, pseudoInverse_fullRank_test)
     int nRow = sns_ik::rng_util::getRngInt(0, 2, (nCol-1));
     A = sns_ik::rng_util::getRngMatrixXd(seed + 68409, nRow, nCol, low, upp);
     ASSERT_TRUE(sns_ik::pinv_damped_P(A, &X, nullptr, tolSvd, tolSvd));
-    sns_ik::pseudoInverse(A, tolSvd, &invA, &rank, &damped);
+    ASSERT_TRUE(sns_ik::pseudoInverse(A, tolSvd, &invA, &rank, &damped));
     checkPseudoInverse(A, X, tolMat);
     checkPseudoInverse(A, invA, tolMat);
     ASSERT_FALSE(damped);
@@ -399,7 +399,7 @@ TEST(sns_ik_math_utils, pseudoInverse_damped_test)
     int nRank = sns_ik::rng_util::getRngInt(seed + 94846, 1, (nRow-1));
     A = sns_ik::rng_util::getRngMatrixXdRanked(seed + 43203, nRow, nCol, nRank);
     ASSERT_FALSE(sns_ik::pinv_damped_P(A, &X, nullptr, tolSvd, tolSvd));
-    sns_ik::pseudoInverse(A, tolSvd, &invA, &rank, &damped);
+    ASSERT_TRUE(sns_ik::pseudoInverse(A, tolSvd, &invA, &rank, &damped));
     checkPseudoInverse(A, X, tolMat);
     checkPseudoInverse(A, invA, tolMat);
     ASSERT_TRUE(damped);

diff --git a/sns_ik_lib/test/sns_ik_pos_test.cpp b/sns_ik_lib/test/sns_ik_pos_test.cpp
@@ -1,5 +1,5 @@
-/*! \file sns_ik_test.cpp
- * \brief Unit Test: sns_ik_math_utils
+/*! \file sns_ik_pos_test.cpp
+ * \brief Unit Test: sns_ik position solver
  * \author Matthew Kelly
  */
 /*
@@ -29,8 +29,8 @@
 #include <ros/duration.h>
 #include <ros/time.h>
 
-#include <sns_ik/rng_utilities.hpp>
-#include <sns_ik/robot_model.hpp>
+#include "rng_utilities.hpp"
+#include "robot_model.hpp"
 #include <sns_ik/sns_ik.hpp>
 
 /*

diff --git a/sns_ik_lib/test/sns_ik_vel_test.cpp b/sns_ik_lib/test/sns_ik_vel_test.cpp
@@ -1,5 +1,5 @@
-/*! \file sns_ik_test.cpp
- * \brief Unit Test: sns_ik_math_utils
+/*! \file sns_ik_vel_test.cpp
+ * \brief Unit Test: sns_ik velocity solver
  * \author Matthew Kelly
  */
 /*
@@ -23,13 +23,14 @@
 #include <kdl/chain.hpp>
 #include <kdl/chainiksolvervel_pinv.hpp>
 #include <kdl/chainfksolvervel_recursive.hpp>
+#include <kdl/chainiksolvervel_pinv_nso.hpp>
 #include <map>
 #include <ros/console.h>
 #include <ros/duration.h>
 #include <ros/time.h>
 
-#include <sns_ik/rng_utilities.hpp>
-#include <sns_ik/robot_model.hpp>
+#include "rng_utilities.hpp"
+#include "robot_model.hpp"
 #include <sns_ik/sns_ik.hpp>
 
 /*
@@ -225,6 +226,16 @@ void runGeneralVelIkTest(int seed, KDL::ChainFkSolverVel_recursive& fwdKin, IkSo
 /*************************************************************************************************/
 
 void runSnsVelkTest(int seed, sns_ik::VelocitySolveType solverType) {
+
+
+
+ #if EIGEN_VERSION_AT_LEAST(3,3,4)
+    ROS_INFO("Yay!  You have new eigen.");
+  #else
+    ROS_INFO("Boo. You have old eigen.");
+  #endif
+
+
   // Create a sawyer model:
   std::vector<std::string> jointNames;
   KDL::Chain sawyerChain = sns_ik::robot_model::getSawyerKdlChain(&jointNames);