updated lstsq

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/damped_least_squares.hpp"
+#include "lbr_fri_ros2/lstsq.hpp"
 
 namespace lbr_fri_ros2 {
 class AdmittanceController {
@@ -50,7 +50,7 @@ class AdmittanceController {
 
     jacobian_solver_->JntToJac(q_, jacobian_);
 
-    jacobian_inv_ = damped_least_squares(jacobian_.data);
+    jacobian_inv_ = lstsq(jacobian_.data);
     f_ext_ = jacobian_inv_.transpose() * tau_ext_;
 
     for (int i = 0; i < 6; i++) {

lbr_fri_ros2/include/lbr_fri_ros2/damped_least_squares.hpp → lbr_fri_ros2/include/lbr_fri_ros2/lstsq.hpp

@@ -1,26 +1,29 @@
-#ifndef LBR_FRI_ROS2__DAMPED_LEAST_SQUARES_HPP_
-#define LBR_FRI_ROS2__DAMPED_LEAST_SQUARES_HPP_
+#ifndef LBR_FRI_ROS2__LSTSQ_HPP_
+#define LBR_FRI_ROS2__LSTSQ_HPP_
+
+#include <algorithm>
 
 #include "eigen3/Eigen/Core"
 #include "eigen3/Eigen/SVD"
 
 namespace lbr_fri_ros2 {
 template <class MatT>
 Eigen::Matrix<typename MatT::Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime>
-damped_least_squares(const MatT &mat, typename MatT::Scalar lambda =
-                                          typename MatT::Scalar{2e-1}) // choose appropriately
+lstsq(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);
   const auto &singularValues = svd.singularValues();
   Eigen::Matrix<Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime> dampedSingularValuesInv(
       mat.cols(), mat.rows());
   dampedSingularValuesInv.setZero();
-  for (unsigned int i = 0; i < singularValues.size(); ++i) {
-    dampedSingularValuesInv(i, i) =
-        singularValues(i) / (singularValues(i) * singularValues(i) + lambda * lambda);
-  }
+  std::for_each(singularValues.data(), singularValues.data() + singularValues.size(),
+                [&, i = 0](Scalar &s)[mutable] {
+                  dampedSingularValuesInv(i, i) = s / (s * s + lambda * lambda);
+                  ++i;
+                });
   return svd.matrixV() * dampedSingularValuesInv * svd.matrixU().adjoint();
 }
 } // end of namespace lbr_fri_ros2
-#endif // LBR_FRI_ROS2__DAMPED_LEAST_SQUARES_HPP_
+#endif // LBR_FRI_ROS2__LSTSQ_HPP_