Implement effort feedforward

Author: christophfroehlich

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -328,8 +328,6 @@ controller_interface::return_type JointTrajectoryController::update(
       {
         if (traj_contr_)
         {
-          // TODO(christophfroehlich) add has_effort_command_interface_ ?
-          // command_next_.effort[index_cmd_joint] : 0.0)
           traj_contr_->compute_commands(
             tmp_command_, state_current_, state_error_, command_next_,
             time - current_trajectory_->time_from_start(), period);
@@ -1837,6 +1835,7 @@ void JointTrajectoryController::add_new_trajectory_msg_nonRT(
       RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
     }
   }
+  RCLCPP_INFO(get_node()->get_logger(), "Called add_new_trajectory_msg_nonRT.");
 }
 
 void JointTrajectoryController::add_new_trajectory_msg_RT(
@@ -1856,6 +1855,7 @@ void JointTrajectoryController::add_new_trajectory_msg_RT(
       RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
     }
   }
+  RCLCPP_INFO(get_node()->get_logger(), "Called add_new_trajectory_msg_RT.");
 }
 
 void JointTrajectoryController::preempt_active_goal()

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -698,7 +698,7 @@ class TrajectoryControllerTest : public ::testing::Test
         EXPECT_EQ(effort.at(2), joint_eff_[2]);
       }
     }
-    else  // traj_controller_->use_closed_loop_pid_adapter() == true
+    else  // traj_controller_->use_external_control_law() == true
     {
       // velocity or effort PID?
       // --> set kp > 0.0 in test

joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp

@@ -128,11 +128,14 @@ void PidTrajectoryPlugin::compute_commands(
   const trajectory_msgs::msg::JointTrajectoryPoint desired,
   const rclcpp::Duration & /*duration_since_start*/, const rclcpp::Duration & period)
 {
+  // if effort field is present, otherwise it would have been rejected
+  auto has_effort_command_interface = !desired.effort.empty();
   // Update PIDs
   for (auto i = 0ul; i < num_cmd_joints_; ++i)
   {
     tmp_command[map_cmd_to_joints_[i]] =
       (desired.velocities[map_cmd_to_joints_[i]] * ff_velocity_scale_[i]) +
+      (has_effort_command_interface ? desired.effort[i] : 0.0) +
       pids_[i]->compute_command(
         error.positions[map_cmd_to_joints_[i]], error.velocities[map_cmd_to_joints_[i]], period);
   }