Compute velocity errors when using an effort command interface (ros-c…

…ontrols#679) (cherry picked from commit 2e0da5d)
christophfroehlich · Jul 17, 2023 · 27641e5 · 27641e5
1 parent d3b7ed2
commit 27641e5
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 3 deletions.
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -133,7 +133,9 @@ controller_interface::return_type JointTrajectoryController::update(
     {
       error.positions[index] = desired.positions[index] - current.positions[index];
     }
-    if (has_velocity_state_interface_ && has_velocity_command_interface_)
+    if (
+      has_velocity_state_interface_ &&
+      (has_velocity_command_interface_ || has_effort_command_interface_))
     {
       error.velocities[index] = desired.velocities[index] - current.velocities[index];
     }

diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp
@@ -304,7 +304,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal
 /**
  * Makes sense with position command interface only,
  * because no integration to position state interface is implemented
-*/
+ */
 TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success)
 {
   // set tolerance parameters
@@ -349,7 +349,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success)
 /**
  * Makes sense with position command interface only,
  * because no integration to position state interface is implemented
-*/
+ */
 TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success)
 {
   // set tolerance parameters