lstsq -> pinv

lbr-stack · Dec 7, 2023 · 2a98076 · 2a98076
1 parent f209973
commit 2a98076
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 7 deletions.
diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp
@@ -14,7 +14,7 @@
 
 #include "lbr_fri_msgs/msg/lbr_position_command.hpp"
 #include "lbr_fri_msgs/msg/lbr_state.hpp"
-#include "lbr_fri_ros2/lstsq.hpp"
+#include "lbr_fri_ros2/pinv.hpp"
 
 namespace lbr_fri_ros2 {
 class AdmittanceController {
@@ -50,7 +50,7 @@ class AdmittanceController {
 
     jacobian_solver_->JntToJac(q_, jacobian_);
 
-    jacobian_inv_ = lstsq(jacobian_.data);
+    jacobian_inv_ = pinv(jacobian_.data);
     f_ext_ = jacobian_inv_.transpose() * tau_ext_;
 
     for (int i = 0; i < 6; i++) {

diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/lstsq.hpp → lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/lstsq.hpp → lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp
@@ -1,5 +1,5 @@
-#ifndef LBR_FRI_ROS2__LSTSQ_HPP_
-#define LBR_FRI_ROS2__LSTSQ_HPP_
+#ifndef LBR_FRI_ROS2__PINV_HPP_
+#define LBR_FRI_ROS2__PINV_HPP_
 
 #include <algorithm>
 
@@ -9,8 +9,8 @@
 namespace lbr_fri_ros2 {
 template <class MatT>
 Eigen::Matrix<typename MatT::Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime>
-lstsq(const MatT &mat,
-      typename MatT::Scalar lambda = typename MatT::Scalar{2e-1}) // choose appropriately
+pinv(const MatT &mat,
+     typename MatT::Scalar lambda = typename MatT::Scalar{2e-1}) // choose appropriately
 {
   typedef typename MatT::Scalar Scalar;
   auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
@@ -26,4 +26,4 @@ lstsq(const MatT &mat,
   return svd.matrixV() * dampedSingularValuesInv * svd.matrixU().adjoint();
 }
 } // end of namespace lbr_fri_ros2
-#endif // LBR_FRI_ROS2__LSTSQ_HPP_
+#endif // LBR_FRI_ROS2__PINV_HPP_