Basic unit tests for SNS position and velocity IK #75

MatthewPeterKelly · 2018-05-22T12:03:30Z

This commit addes unit tests for:

SNS position IK (all versions)
SNS velocity IK (all versions)
KDL position IK (pinv NR only)

It also addes a pair of functions that directly create a KDL chain model
for Sawyer, which allows the unit tests to run without depending on external
repositories or URDF files.

This commit includes code for a cleaned-up version of the pseudo-inverse function, along with a unit test for both the full-rank and rank-deficient cases.

sns_ik_data_utilities --> rng_utilities

This commit also turns on compiler warnings and treats them as errors.

Issues: #74

This commit addes unit tests for: - SNS position IK (all versions) - SNS velocity IK (all versions) - KDL position IK (pinv NR only) It also addes a pair of functions that directly create a KDL chain model for Sawyer, which allows the unit tests to run without depending on external repositories or URDF files. sns_ik_data_utilities --> rng_utilities This commit also turns on compiler warnings and treats them as errors. Issues: RethinkRobotics-opensource#74

MatthewPeterKelly

Two minor things to fix that I missed on my first pass.

Also need to create issues:

bug: in Ubuntu 16.04 with Eigen 3.3.4 SNS-IK crashes the unit test with a segfault.
bug: Optimal SNS velocity IK solver fails unit test
feature: add more details to SNS-IK unit tests, as well as additional comparisons to KDL.

MatthewPeterKelly · 2018-05-22T12:14:44Z

sns_ik_lib/src/robot_model.cpp

+  *qUpp = KDL::JntArray(nJnt);
+  *vMax = KDL::JntArray(nJnt);
+  *aMax = KDL::JntArray(nJnt);
+  (*qLow)(0) = -3.050300;


remove extra trailing zeros here

and on following lines.

MatthewPeterKelly · 2018-05-22T12:17:28Z

sns_ik_lib/test/sns_ik_vel_test.cpp

+#include <kdl/chain.hpp>
+#include <kdl/chainiksolvervel_pinv.hpp>
+#include <kdl/chainfksolvervel_recursive.hpp>
+#include <kdl/chainiksolvervel_pinv_nso.hpp>


this header is not needed

MatthewPeterKelly · 2018-05-22T13:44:48Z

sns_ik_lib/test/rng_utilities_test.cpp

+    seed++;
+    int nRows = sns_ik::rng_util::getRngInt(seed + 20817, 1, 9);
+    int nCols = sns_ik::rng_util::getRngInt(seed + 66461, 1, 9);
+    int nRank = sns_ik::rng_util::getRngInt(seed + 77126, 1, 9);


nRank is unused.

MatthewPeterKelly · 2018-05-22T13:48:11Z

sns_ik_lib/test/rng_utilities_test.cpp

+    // Check the rank:
+    nRank = std::min({nRows, nCols, nRank});
+    Eigen::ColPivHouseholderQR<Eigen::MatrixXd> decomp(X);
+    ASSERT_LE(decomp.rank(), nRank);


ASSERT_EQ rather than ASSERT_LE

MatthewPeterKelly · 2018-05-22T13:52:16Z

sns_ik_lib/test/sns_ik_math_utils_test.cpp

+  Eigen::MatrixXd A;  // test matrix
+  int rank; // rank of invA, computed by pseudoInverse(A)
+  bool damped;  // true iff invA was computed with a damped pseudo-inverse
+  for (int iTest = 0; iTest < 20; iTest++) {


replace 20 with nTest

MatthewPeterKelly · 2018-05-22T13:52:52Z

sns_ik_lib/test/sns_ik_math_utils_test.cpp

+  double tolMat = 1e-6;  // tolerance for matrix equality check
+  int nTest = 15;
+  int seed = 76407;
+  double low = -2.0;  double upp = 2.0;  // bounds on values in the A and P matrix


this line is unused and can be deleted.

IanTheEngineer

I reviewed about 70% of this. After some feedback is incorporated, I'll take another pass.

IanTheEngineer · 2018-05-22T13:54:20Z

sns_ik_lib/CMakeLists.txt

@@ -6,9 +6,9 @@ include(CheckCXXCompilerFlag)
 CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
 CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
 if(COMPILER_SUPPORTS_CXX11)
-    set(CMAKE_CXX_FLAGS "-std=c++11 -O2")
+    set(CMAKE_CXX_FLAGS "-std=c++11 -O2 -Wall -Werror")


Hmm. I'm not sure this is standard practice, to kill the build on every warning. While I don't mind it for our own builds, we need to double check that this is acceptable for the ROS Buildfarm.

Hmmm... Good point. How can I look up the best practices for this? I did a quick google search and made little progress.
Perhaps I should change it to -Wall -Wextra but let builds pass with warnings?

My goal here was to prevent pull requests that added questionable code to the project, assuming that everyone would build their project before making a pull-request.

I'll leave it for now, but I'll defer to you on your next pass through the review.

I removed the error on warning flag, but kept -Wall.

IanTheEngineer · 2018-05-22T13:57:00Z

sns_ik_lib/include/sns_ik/robot_model.hpp

+* 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.


IanTheEngineer · 2018-05-22T13:58:47Z

sns_ik_lib/include/sns_ik/robot_model.hpp

@@ -0,0 +1,56 @@
+/** @file robot_model.hpp


I would prefer a different name than robot_model for this file. There are other robot model header files in the ROS world, and I'd like to avoid any confusion. Additionally, this tester file should not be part of our public API. Either place it inside the src/ directory, or create a separate test directory for all test headers & sources.

What is the best way to enforce a public and private API for a ROS package?

The code for both robot_model and rng_utilities are both used only by the unit test and should be private. I could not figure out how to get the unit test to correctly link to them without either A) putting them alongside the other files or B) putting the implementation in a header file and #including it, which seemed like a bad idea.

This is part of a bigger discussion #69: what should be public and private, and how to enforce? Perhaps we could talk off-line to decide the best plan going forward.

After talking to you I have a much better understanding of how the public vs private API in the sense of the installer (rather than C++ class keywords).

Updates:

robot_model is now sawyer_model

testing code is moved to a private library

utility code is moved to a private library

IanTheEngineer · 2018-05-22T14:00:31Z

sns_ik_lib/CMakeLists.txt

@@ -39,7 +39,8 @@ add_library(sns_ik
            src/fsns_velocity_ik.cpp
            src/osns_sm_velocity_ik.cpp
            src/osns_velocity_ik.cpp
-            src/sns_ik_data_utilities.cpp
+            src/rng_utilities.cpp
+            src/robot_model.cpp


robot_model.cpp shouldn't be built into the SNS library, just used for testing.

Same issue as above - I think that I'm missing something fundamental about how ROS package API works. Let's talk off-line so that I first understand the situation better.

fixed in the new commit.

IanTheEngineer · 2018-05-22T14:02:13Z

sns_ik_lib/CMakeLists.txt

-  target_link_libraries(sns_ik_data_utilities_test ${catkin_LIBRARIES} sns_ik)
+  catkin_add_gtest(sns_ik_pos_test test/sns_ik_pos_test.cpp)
+  catkin_add_gtest(sns_ik_vel_test test/sns_ik_vel_test.cpp)
+  target_link_libraries(rng_utilities_test ${catkin_LIBRARIES} sns_ik)


it's good practice to link against sns_ik first, catkin_LIBRARIES second. That will prevent the unfortunate circumstance of hiding your source library behind the debian installed version of your library.

Good to know! I'll fix it.

IanTheEngineer · 2018-05-22T14:28:30Z

sns_ik_lib/src/sns_ik_math_utils.cpp

@@ -83,7 +85,7 @@ bool pinv_damped_P(const Eigen::MatrixXd &A, Eigen::MatrixXd *invA, Eigen::Matri
      sigma(i) = 1.0 / sigma(i);
    }
    (*invA) = svd_A.matrixU() * sigma.asDiagonal() * svd_A.matrixV().transpose();
-    (*P) = ((*P) - svd_A.matrixU() * svd_A.matrixU().transpose()).eval();
+    if (P){ *P = ((*P) - svd_A.matrixU() * svd_A.matrixU().transpose()).eval(); }
    return true;


If P is null, we still return true? Is that intentional?

This is intentional - I did this so that the unit test can call the old version of the damped pseudo inverse to compare the results to the new implementation.

The return value is not an error flag, it is whether the matrix was full-rank. This is a confusing API and I changed it for the pseudoInverse() function that I added.

Passing *P = nullptr is used when the caller wants to compute the pseudo inverse without also computing the P matrix. An alternative would be to default to setting P to the identity matrix, but this incurs an extra computational cost.

Ultimately we should remove all of these utility functions and replace them with new ones that are better thought-out and implemented.

IanTheEngineer · 2018-05-22T15:04:22Z

sns_ik_lib/src/sns_ik_math_utils.cpp

+  int rankA = 0;
+  int nSigma = sigma.size();
+  for (int i = 0; i < nSigma; i++) {
+    if (sigma(i) > eps) rankA++;  // increment the rank counter


Do we care if eps is negative and therefore sigma(i) == 0?

Good point - we should do input validation on eps to ensure that it is positive. It will also break the code if it is zero.

IanTheEngineer · 2018-05-22T15:19:45Z

sns_ik_lib/test/rng_utilities_test.cpp

@@ -0,0 +1,198 @@
+/*! \file rng_utilities_test.cpp
+ * \brief Unit Test: sns_ik_math_utils


should the name match the file?

Fixed in a few other places as well.

IanTheEngineer · 2018-05-22T15:21:03Z

sns_ik_lib/test/sns_ik_vel_test.cpp

@@ -0,0 +1,290 @@
+/*! \file sns_ik_test.cpp


Looks like these lines need to be updated

IanTheEngineer · 2018-05-22T15:24:05Z

sns_ik_lib/include/sns_ik/sns_ik_math_utils.hpp

 *                   P.rows() == P.cols() = A.cols() is required
 *                   P should be initialized with the identity matrix
 * @param[opt] lambda_max: damping parameter for the damped pseudoinverse
 * @param[opt] eps: singular values smaller than this will be set to zero
 * @return: true if A is full rank, false if A is rank deficient
 */
-bool pinv_damped_P(const Eigen::MatrixXd &A, Eigen::MatrixXd *invA, Eigen::MatrixXd *P,
+bool pinv_damped_P(const Eigen::MatrixXd &A, Eigen::MatrixXd *invA, Eigen::MatrixXd *P = nullptr,


Note for the future: we should be consistent about how we use references and pointers in function signatures. Also, when this is going to 18.04, let's move towards using std::unique_ptr's instead of raw pointers.

We definitely need to clean up the way that the library handles pointers and references (#43). There are really two different issues:

how to handle ownership of objects, especially those with non-trivial constructors

how to handle output arguments

IanTheEngineer · 2018-05-22T19:31:13Z

sns_ik_lib/test/sns_ik_math_utils_test.cpp

-#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"


bah to relative paths, but I can't think of an elegant way around it off the top of my head.

I think its possibly an indicator that we should consider adding a subset of math utilities to the public API?

I think that this is fixed now. I moved the utilities to their own private library, which both the test and main libraries link against. Let me know if I managed the CMakeLists correctly for it.

Changes: - robot_model renamed sawyer_model - testing code moved to private library - utility code moved to private library

IanTheEngineer · 2018-05-30T20:46:28Z

sns_ik_lib/CMakeLists.txt

+# library for private utilities
+add_library(sns_ik_util
+            utilities/sns_ik_math_utils.cpp)
+target_link_libraries(sns_ik_util)


Hmmmm. This linking is missing things to be linked to. You can likely remove line 39.

That is unless you actually should be linking against things like ${catkin_LIBRARIES}...

Looks like everything works fine after deleting line 39.

IanTheEngineer · 2018-05-30T20:49:44Z

sns_ik_lib/CMakeLists.txt


+# install the public API


+1 for the clarification

IanTheEngineer

Looks good! I can merge once the CMakeLists line is updated.

robodreamer approved these changes May 22, 2018

View reviewed changes

MatthewPeterKelly commented May 22, 2018

View reviewed changes

MatthewPeterKelly requested a review from IanTheEngineer May 22, 2018 12:45

MatthewPeterKelly commented May 22, 2018

View reviewed changes

Minor code clean-up

615164f

IanTheEngineer requested changes May 22, 2018

View reviewed changes

Refactor to fix API for unit tests and utilities

6c0e4dd

MatthewPeterKelly force-pushed the mpk_unitTestSnsIk branch from 360d0e1 to 6c0e4dd Compare May 22, 2018 19:23

IanTheEngineer reviewed May 22, 2018

View reviewed changes

Package organization clean-up

2e6f9ea

Changes: - robot_model renamed sawyer_model - testing code moved to private library - utility code moved to private library

IanTheEngineer reviewed May 30, 2018

View reviewed changes

sns_ik_lib/CMakeLists.txt

# install the public API

Copy link

Member

IanTheEngineer May 30, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

+1 for the clarification

IanTheEngineer approved these changes May 31, 2018

View reviewed changes

Removed one unneeded line in CMakeLists

e03267d

IanTheEngineer merged commit d6c9c8a into RethinkRobotics-opensource:master May 31, 2018

MatthewPeterKelly deleted the mpk_unitTestSnsIk branch May 31, 2018 18:34

		@@ -0,0 +1,198 @@
		/*! \file rng_utilities_test.cpp
		* \brief Unit Test: sns_ik_math_utils

Basic unit tests for SNS position and velocity IK #75

Basic unit tests for SNS position and velocity IK #75

Conversation

MatthewPeterKelly commented May 22, 2018

MatthewPeterKelly left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

IanTheEngineer left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

MatthewPeterKelly May 22, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

MatthewPeterKelly May 22, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

IanTheEngineer left a comment

Choose a reason for hiding this comment

MatthewPeterKelly May 22, 2018 •

edited

Loading

MatthewPeterKelly May 22, 2018 •

edited

Loading