add contigency for rank deficient J (when on purpose due to joint num)

ICube-Robotics · Aug 23, 2024 · e94d0de · e94d0de
1 parent 5921f9a
commit e94d0de
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 2 deletions.
diff --git a/cartesian_vic_controller/src/rules/vanilla_cartesian_admittance_rule.cpp b/cartesian_vic_controller/src/rules/vanilla_cartesian_admittance_rule.cpp
@@ -195,7 +195,12 @@ bool VanillaCartesianAdmittanceRule::compute_controls(
   RCLCPP_DEBUG(logger_, "Computing J_pinv...");
   const Eigen::JacobiSVD<Eigen::MatrixXd> J_svd =
     Eigen::JacobiSVD<Eigen::MatrixXd>(J_, Eigen::ComputeThinU | Eigen::ComputeThinV);
-  double conditioning_J = J_svd.singularValues()(0) / J_svd.singularValues()(dims - 1);
+  double conditioning_J = 1000.0;
+  if (J_.cols() < 6) {
+    conditioning_J = J_svd.singularValues()(0) / J_svd.singularValues()(J_.cols() - 1);
+  } else {
+    conditioning_J = J_svd.singularValues()(0) / J_svd.singularValues()(dims - 1);
+  }
   if (conditioning_J > 30) {
     success = false;
     RCLCPP_ERROR(

diff --git a/cartesian_vic_controller/src/rules/vanilla_cartesian_impedance_rule.cpp b/cartesian_vic_controller/src/rules/vanilla_cartesian_impedance_rule.cpp
@@ -170,7 +170,12 @@ bool VanillaCartesianImpedanceRule::compute_controls(
   RCLCPP_DEBUG(logger_, "Computing J_pinv...");
   const Eigen::JacobiSVD<Eigen::MatrixXd> J_svd =
     Eigen::JacobiSVD<Eigen::MatrixXd>(J_, Eigen::ComputeThinU | Eigen::ComputeThinV);
-  double conditioning_J = J_svd.singularValues()(0) / J_svd.singularValues()(dims - 1);
+  double conditioning_J = 1000.0;
+  if (J_.cols() < 6) {
+    conditioning_J = J_svd.singularValues()(0) / J_svd.singularValues()(J_.cols() - 1);
+  } else {
+    conditioning_J = J_svd.singularValues()(0) / J_svd.singularValues()(dims - 1);
+  }
   if (conditioning_J > 30) {
     success = false;
     RCLCPP_ERROR(