Flip the "pose error" calculation

ros-controls · Oct 26, 2021 · 1546a58 · 1546a58
1 parent 9dfb76c
commit 1546a58
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp
@@ -235,7 +235,7 @@ controller_interface::return_type AdmittanceRule::update(
   std::array<double, 6> pose_error;
 
   for (auto i = 0u; i < 6; ++i) {
-    pose_error[i] = target_pose_control_frame_arr_[i] - current_pose_control_frame_arr_[i];
+    pose_error[i] = current_pose_control_frame_arr_[i] - target_pose_control_frame_arr_[i];
     if (i >= 3) {
       pose_error[i] = angles::normalize_angle(pose_error[i]);
     }