From 2f661b5d6a8046eea53334fb8836168535f65012 Mon Sep 17 00:00:00 2001 From: Marco Esposito Date: Tue, 4 Apr 2017 16:34:43 +0200 Subject: [PATCH] Do not initialize position with torque in impedance controller Since the impedance controller is an EffortController, joint_handles are torque handles; joint_set_point_handles are the position handles. Initializing the position with the torque (at start probably all zeros) could explain a lot of FRI interpolation errors at startup. --- lwr_controllers/src/joint_impedance_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lwr_controllers/src/joint_impedance_controller.cpp b/lwr_controllers/src/joint_impedance_controller.cpp index bf6997d..0ecc064 100644 --- a/lwr_controllers/src/joint_impedance_controller.cpp +++ b/lwr_controllers/src/joint_impedance_controller.cpp @@ -59,7 +59,7 @@ void JointImpedanceController::starting(const ros::Time& time) // Initializing stiffness, damping, ext_torque and set point values for (size_t i = 0; i < joint_handles_.size(); i++) { tau_des_(i) = 0.0; - q_des_(i) = joint_handles_[i].getPosition(); + q_des_(i) = joint_set_point_handles_[i].getPosition(); }