Rename pid_adapter variable and update gains with every new trajectory

Author: christophfroehlich

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -162,7 +162,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   bool has_effort_command_interface_ = false;
 
   /// If true, a velocity feedforward term plus corrective PID term is used
-  bool use_closed_loop_pid_adapter_ = false;
+  bool use_closed_loop_control_law_ = false;
   // class loader for actual trajectory controller
   std::shared_ptr<
     pluginlib::ClassLoader<joint_trajectory_controller_plugins::TrajectoryControllerBase>>

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -284,7 +284,7 @@ controller_interface::return_type JointTrajectoryController::update(
       // set values for next hardware write() if tolerance is met
       if (!tolerance_violated_while_moving && within_goal_time)
       {
-        if (use_closed_loop_pid_adapter_)
+        if (use_closed_loop_control_law_)
         {
           traj_contr_->computeCommands(
             tmp_command_, state_current_, state_error_, state_desired_, time, period);
@@ -297,7 +297,7 @@ controller_interface::return_type JointTrajectoryController::update(
         }
         if (has_velocity_command_interface_)
         {
-          if (use_closed_loop_pid_adapter_)
+          if (use_closed_loop_control_law_)
           {
             assign_interface_from_point(joint_command_interface_[1], tmp_command_);
           }
@@ -705,13 +705,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_EFFORT);
 
   // if there is only velocity or if there is effort command interface
-  // then use also PID adapter
-  use_closed_loop_pid_adapter_ =
+  // then use also closed loop controller
+  use_closed_loop_control_law_ =
     (has_velocity_command_interface_ && params_.command_interfaces.size() == 1 &&
      !params_.open_loop_control) ||
     has_effort_command_interface_;
 
-  if (use_closed_loop_pid_adapter_)
+  if (use_closed_loop_control_law_)
   {
     try
     {
@@ -994,14 +994,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
     cmd_timeout_ = 0.0;
   }
 
-  // update gains of PID controller
-  if (use_closed_loop_pid_adapter_)
-  {
-    trajectory_msgs::msg::JointTrajectory traj;
-    traj.points.push_back(state_current_);
-    traj_contr_->computeGains(traj);
-  }
-
   return CallbackReturn::SUCCESS;
 }
 
@@ -1468,6 +1460,12 @@ void JointTrajectoryController::add_new_trajectory_msg(
   const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg)
 {
   traj_msg_external_point_ptr_.writeFromNonRT(traj_msg);
+
+  // update gains of controller
+  if (use_closed_loop_control_law_)
+  {
+    traj_contr_->computeGains(traj_msg);
+  }
 }
 
 void JointTrajectoryController::preempt_active_goal()

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -847,7 +847,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid)
      !traj_controller_->is_open_loop()) ||
     traj_controller_->has_effort_command_interface())
   {
-    EXPECT_TRUE(traj_controller_->use_closed_loop_pid_adapter());
+    EXPECT_TRUE(traj_controller_->use_closed_loop_control_law());
   }
 }
 
@@ -904,7 +904,7 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error)
   }
   // is the velocity error correct?
   if (
-    traj_controller_->use_closed_loop_pid_adapter()  // always needed for PID controller
+    traj_controller_->use_closed_loop_control_law()  // always needed for PID controller
     || (traj_controller_->has_velocity_state_interface() &&
         traj_controller_->has_velocity_command_interface()))
   {
@@ -969,8 +969,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order)
 
   if (traj_controller_->has_velocity_command_interface())
   {
-    // if use_closed_loop_pid_adapter_==false: we expect desired velocities from direct sampling
-    // if use_closed_loop_pid_adapter_==true: we expect desired velocities, because we use PID with
+    // if use_closed_loop_control_law_==false: we expect desired velocities from direct sampling
+    // if use_closed_loop_control_law_==true: we expect desired velocities, because we use PID with
     // feedforward term only
     EXPECT_GT(0.0, joint_vel_[0]);
     EXPECT_GT(0.0, joint_vel_[1]);

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -126,7 +126,7 @@ class TestableJointTrajectoryController
 
   bool has_effort_command_interface() const { return has_effort_command_interface_; }
 
-  bool use_closed_loop_pid_adapter() const { return use_closed_loop_pid_adapter_; }
+  bool use_closed_loop_control_law() const { return use_closed_loop_control_law_; }
 
   bool is_open_loop() const { return params_.open_loop_control; }
 
@@ -275,7 +275,7 @@ class TrajectoryControllerTest : public ::testing::Test
 
     // set pid parameters before activate. The PID plugin has to be loaded already, otherwise
     // parameters are not declared yet
-    if (traj_controller_->use_closed_loop_pid_adapter())
+    if (traj_controller_->use_closed_loop_control_law())
     {
       SetPidParameters(k_p, ff);
     }
@@ -545,7 +545,7 @@ class TrajectoryControllerTest : public ::testing::Test
     // i.e., active but trivial trajectory (one point only)
     EXPECT_TRUE(traj_controller_->has_trivial_traj());
 
-    if (traj_controller_->use_closed_loop_pid_adapter() == false)
+    if (traj_controller_->use_closed_loop_control_law() == false)
     {
       if (traj_controller_->has_position_command_interface())
       {

joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp

@@ -37,7 +37,8 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase
     rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::vector<std::string> command_joint_names) override;
 
-  bool computeGains(const trajectory_msgs::msg::JointTrajectory trajectory) override;
+  bool computeGains(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) override;
 
   void computeCommands(
     std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,

joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp

@@ -15,6 +15,7 @@
 #ifndef JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_
 #define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_
 
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -47,7 +48,8 @@ class TrajectoryControllerBase
   /**
    */
   JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
-  virtual bool computeGains(const trajectory_msgs::msg::JointTrajectory trajectory) = 0;
+  virtual bool computeGains(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
 
   /**
    */

joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp

@@ -50,7 +50,8 @@ bool PidTrajectoryPlugin::initialize(
   return true;
 }
 
-bool PidTrajectoryPlugin::computeGains(const trajectory_msgs::msg::JointTrajectory /*trajectory*/)
+bool PidTrajectoryPlugin::computeGains(
+  const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/)
 {
   if (param_listener_->is_old(params_))
   {

joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp

@@ -39,7 +39,7 @@ TEST_F(PidTrajectoryTest, TestSingleJoint)
   traj_contr->trigger_declare_parameters();
   node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0));
 
-  ASSERT_TRUE(traj_contr->computeGains(trajectory_msgs::msg::JointTrajectory()));
+  ASSERT_TRUE(traj_contr->computeGains(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
 
   trajectory_msgs::msg::JointTrajectoryPoint traj_msg;
   traj_msg.positions.push_back(0.0);
@@ -71,7 +71,7 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints)
   node_->set_parameter(rclcpp::Parameter("gains.joint2.p", 1.0));
   node_->set_parameter(rclcpp::Parameter("gains.joint3.p", 1.0));
 
-  ASSERT_TRUE(traj_contr->computeGains(trajectory_msgs::msg::JointTrajectory()));
+  ASSERT_TRUE(traj_contr->computeGains(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
 
   trajectory_msgs::msg::JointTrajectoryPoint traj_msg;
   traj_msg.positions.push_back(0.0);