added damping parameter

lbr-stack · Nov 20, 2023 · 091aca5 · 091aca5
1 parent 8b55297
commit 091aca5
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 1 deletion.
diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml
@@ -21,6 +21,10 @@
     forward_position_controller:
       type: position_controllers/JointGroupPositionController
 
+/**/lbr_virtual_ft_broadcaster:
+  ros__parameters:
+    damping: 0.2
+
 /**/position_trajectory_controller:
   ros__parameters:
     joints:

diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_virtual_ft_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_virtual_ft_broadcaster.hpp
@@ -61,6 +61,8 @@ class LBRVirtualFTBroadcaster : public controller_interface::ControllerInterface
       external_joint_torques_;
   Eigen::Matrix<double, 6, 1> virtual_ft_;
 
+  double damping_;
+
   std::string end_effector_link_ = "link_ee";
   std::array<std::string, 7> joint_names_ = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"};
 

diff --git a/lbr_ros2_control/src/lbr_virtual_ft_broadcaster.cpp b/lbr_ros2_control/src/lbr_virtual_ft_broadcaster.cpp
@@ -32,6 +32,10 @@ controller_interface::CallbackReturn LBRVirtualFTBroadcaster::on_init() {
             wrench_stamped_publisher_ptr_);
     kinematics_interface_kdl_.initialize(this->get_node()->get_node_parameters_interface(),
                                          end_effector_link_);
+    if (!this->get_node()->get_parameter_or<double>("damping", damping_, 2e-1)) {
+      RCLCPP_WARN(this->get_node()->get_logger(),
+                  "Failed to get damping parameter, using default value: %f.", damping_);
+    }
     for (auto &state_interface : state_interfaces_) {
       if (state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) {
         joint_position_interfaces_.emplace_back(std::ref(state_interface));
@@ -57,7 +61,7 @@ LBRVirtualFTBroadcaster::update(const rclcpp::Time & /*time*/,
   }
   // compute virtual FT given Jacobian and external joint torques
   kinematics_interface_kdl_.calculate_jacobian(joint_positions_, end_effector_link_, jacobian_);
-  jacobian_pinv_ = damped_least_squares_(jacobian_);
+  jacobian_pinv_ = damped_least_squares_(jacobian_, damping_);
   virtual_ft_ = jacobian_pinv_.transpose() * external_joint_torques_;
   // publish
   if (rt_wrench_stamped_publisher_ptr_->trylock()) {