fix invalid integration (#320)

(cherry picked from commit b3a290d)

Author: mhubii

lbr_description/ros2_control/lbr_controllers.yaml

@@ -100,8 +100,8 @@
   ros__parameters:
     robot_name: lbr
     admittance:
-      mass: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
-      damping: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+      mass: [0.05, 0.05, 0.05, 0.005, 0.005, 0.005]
+      damping: [1., 1., 1., 0.1, 0.1, 0.1]
       stiffness: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
     inv_jac_ctrl:
       chain_root: lbr_link_0

lbr_ros2_control/src/controllers/admittance_controller.cpp

@@ -131,7 +131,8 @@ AdmittanceController::update(const rclcpp::Time & /*time*/, const rclcpp::Durati
   admittance_impl_ptr_->compute(f_ext_, delta_x_, dx_, ddx_);
 
   // integrate ddx_ to command velocity
-  twist_command_ = ddx_ * dt;
+  dx_ += ddx_ * dt;
+  twist_command_ = dx_;
 
   if (!inv_jac_ctrl_impl_ptr_) {
     RCLCPP_ERROR(this->get_node()->get_logger(), "Inverse Jacobian controller not initialized.");