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 index 70490451..7d8a8c0f 100644 --- 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 b/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp similarity index 81% rename from lbr_fri_ros2/include/lbr_fri_ros2/lstsq.hpp rename to lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp index 16019a04..d7e57aad 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/lstsq.hpp +++ b/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 @@ -9,8 +9,8 @@ namespace lbr_fri_ros2 { template Eigen::Matrix -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_