Minor code clean-up

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

Hide file tree

Showing 4 changed files with 30 additions and 33 deletions.
diff --git a/sns_ik_lib/src/robot_model.cpp b/sns_ik_lib/src/robot_model.cpp
@@ -96,34 +96,34 @@ void getSawyerJointLimits(KDL::JntArray* qLow, KDL::JntArray* qUpp,
   *qUpp = KDL::JntArray(nJnt);
   *vMax = KDL::JntArray(nJnt);
   *aMax = KDL::JntArray(nJnt);
-  (*qLow)(0) = -3.050300;
-  (*qUpp)(0) = 3.050300;
-  (*vMax)(0) = 1.600000;
-  (*aMax)(0) = 8.000000;
-  (*qLow)(1) = -3.809500;
-  (*qUpp)(1) = 2.273600;
-  (*vMax)(1) = 1.300000;
-  (*aMax)(1) = 8.000000;
-  (*qLow)(2) = -3.042600;
-  (*qUpp)(2) = 3.042600;
-  (*vMax)(2) = 1.900000;
-  (*aMax)(2) = 8.000000;
-  (*qLow)(3) = -3.043900;
-  (*qUpp)(3) = 3.043900;
-  (*vMax)(3) = 1.900000;
-  (*aMax)(3) = 8.000000;
-  (*qLow)(4) = -2.976100;
-  (*qUpp)(4) = 2.976100;
-  (*vMax)(4) = 3.400000;
-  (*aMax)(4) = 10.000000;
-  (*qLow)(5) = -2.976100;
-  (*qUpp)(5) = 2.976100;
-  (*vMax)(5) = 3.400000;
-  (*aMax)(5) = 10.000000;
-  (*qLow)(6) = -3.140000;
-  (*qUpp)(6) = 3.140000;
-  (*vMax)(6) = 4.500000;
-  (*aMax)(6) = 10.000000;
+  (*qLow)(0) = -3.0503;
+  (*qUpp)(0) = 3.0503;
+  (*vMax)(0) = 1.6;
+  (*aMax)(0) = 8.0;
+  (*qLow)(1) = -3.8095;
+  (*qUpp)(1) = 2.2736;
+  (*vMax)(1) = 1.3;
+  (*aMax)(1) = 8.0;
+  (*qLow)(2) = -3.0426;
+  (*qUpp)(2) = 3.0426;
+  (*vMax)(2) = 1.9;
+  (*aMax)(2) = 8.0;
+  (*qLow)(3) = -3.0439;
+  (*qUpp)(3) = 3.0439;
+  (*vMax)(3) = 1.9;
+  (*aMax)(3) = 8.0;
+  (*qLow)(4) = -2.9761;
+  (*qUpp)(4) = 2.9761;
+  (*vMax)(4) = 3.4;
+  (*aMax)(4) = 10.0;
+  (*qLow)(5) = -2.9761;
+  (*qUpp)(5) = 2.9761;
+  (*vMax)(5) = 3.4;
+  (*aMax)(5) = 10.0;
+  (*qLow)(6) = -3.14;
+  (*qUpp)(6) = 3.14;
+  (*vMax)(6) = 4.5;
+  (*aMax)(6) = 10.0;
 }
 
 /*************************************************************************************************/

diff --git a/sns_ik_lib/test/rng_utilities_test.cpp b/sns_ik_lib/test/rng_utilities_test.cpp
@@ -102,7 +102,6 @@ TEST(rng_utilities, getRngMatrixXd_test)
     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);
     double low = sns_ik::rng_util::getRngDouble(seed + 30185, -5, 20);
     double upp = low + sns_ik::rng_util::getRngDouble(seed + 45177, 1e-6, 10);
     X = sns_ik::rng_util::getRngMatrixXd(seed + 10883, nRows, nCols, low, upp);
@@ -133,7 +132,7 @@ TEST(rng_utilities, getRngMatrixXdRanked_test)
     // Check the rank:
     nRank = std::min({nRows, nCols, nRank});
     Eigen::ColPivHouseholderQR<Eigen::MatrixXd> decomp(X);
-    ASSERT_LE(decomp.rank(), nRank);
+    ASSERT_EQ(decomp.rank(), nRank);
 
     // Check the size:
     ASSERT_EQ(X.rows(), nRows);

diff --git a/sns_ik_lib/test/sns_ik_math_utils_test.cpp b/sns_ik_lib/test/sns_ik_math_utils_test.cpp
@@ -358,7 +358,7 @@ TEST(sns_ik_math_utils, pseudoInverse_fullRank_test)
   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++) {
+  for (int iTest = 0; iTest < nTest; iTest++) {
     seed++;
     int nCol = sns_ik::rng_util::getRngInt(seed + 63541, 3, 9);
     int nRow = sns_ik::rng_util::getRngInt(0, 2, (nCol-1));
@@ -386,7 +386,6 @@ TEST(sns_ik_math_utils, pseudoInverse_damped_test)
   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
   Eigen::MatrixXd X;  // pinv(A)
   Eigen::MatrixXd invA;  // pseudoInverse(A)
   Eigen::MatrixXd A;

diff --git a/sns_ik_lib/test/sns_ik_vel_test.cpp b/sns_ik_lib/test/sns_ik_vel_test.cpp
@@ -23,7 +23,6 @@
 #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>