Add unit test for calculate_jacobian_inverse

ros-controls · Nov 15, 2024 · eb33383 · eb33383
1 parent 753504a
commit eb33383
Showing 1 changed file with 49 additions and 0 deletions.
diff --git a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp
@@ -108,6 +108,26 @@ TEST_F(TestKDLPlugin, KDL_plugin_function)
   {
     ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
   }
+
+  // calculate jacobian
+  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian = Eigen::Matrix<double, 6, 2>::Zero();
+  ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian));
+
+  // calculate jacobian inverse
+  Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian_inverse =
+    jacobian.completeOrthogonalDecomposition().pseudoInverse();
+  Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian_inverse_est =
+    Eigen::Matrix<double, 2, 6>::Zero();
+  ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est));
+
+  // ensure jacobian inverse math is correct
+  for (size_t i = 0; i < static_cast<size_t>(jacobian_inverse.rows()); ++i)
+  {
+    for (size_t j = 0; j < static_cast<size_t>(jacobian_inverse.cols()); ++j)
+    {
+      ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02);
+    }
+  }
 }
 
 TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector)
@@ -142,6 +162,26 @@ TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector)
   {
     ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
   }
+
+  // calculate jacobian
+  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian = Eigen::Matrix<double, 6, 2>::Zero();
+  ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian));
+
+  // calculate jacobian inverse
+  Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian_inverse =
+    jacobian.completeOrthogonalDecomposition().pseudoInverse();
+  Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian_inverse_est =
+    Eigen::Matrix<double, 2, 6>::Zero();
+  ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est));
+
+  // ensure jacobian inverse math is correct
+  for (size_t i = 0; i < static_cast<size_t>(jacobian_inverse.rows()); ++i)
+  {
+    for (size_t j = 0; j < static_cast<size_t>(jacobian_inverse.cols()); ++j)
+    {
+      ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02);
+    }
+  }
 }
 
 TEST_F(TestKDLPlugin, incorrect_input_sizes)
@@ -161,10 +201,14 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes)
   delta_x[2] = 1;
   Eigen::Matrix<double, Eigen::Dynamic, 1> delta_theta = Eigen::Matrix<double, 2, 1>::Zero();
   Eigen::Matrix<double, 6, 1> delta_x_est;
+  Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian = Eigen::Matrix<double, 2, 6>::Zero();
 
   // wrong size input vector
   Eigen::Matrix<double, Eigen::Dynamic, 1> vec_5 = Eigen::Matrix<double, 5, 1>::Zero();
 
+  // wrong size input jacobian
+  Eigen::Matrix<double, Eigen::Dynamic, 6> mat_5_6 = Eigen::Matrix<double, 5, 6>::Zero();
+
   // calculate transform
   ASSERT_FALSE(ik_->calculate_link_transform(vec_5, end_effector_, end_effector_transform));
   ASSERT_FALSE(ik_->calculate_link_transform(pos, "link_not_in_model", end_effector_transform));
@@ -183,6 +227,11 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes)
     ik_->convert_joint_deltas_to_cartesian_deltas(pos, vec_5, end_effector_, delta_x_est));
   ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas(
     pos, delta_theta, "link_not_in_model", delta_x_est));
+
+  // calculate jacobian inverse
+  ASSERT_FALSE(ik_->calculate_jacobian_inverse(vec_5, end_effector_, jacobian));
+  ASSERT_FALSE(ik_->calculate_jacobian_inverse(pos, end_effector_, mat_5_6));
+  ASSERT_FALSE(ik_->calculate_jacobian_inverse(pos, "link_not_in_model", jacobian));
 }
 
 TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description)