Rename pid_adapter variable and update gains with every new trajectory

christophfroehlich · christophfroehlich · commit 732dfafc563c · 2023-11-19T13:44:55.000Z
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/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>>
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -283,7 +283,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);
@@ -296,7 +296,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_);
           }
@@ -704,13 +704,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
     {
@@ -1011,14 +1011,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;
 }
 
@@ -1497,6 +1489,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()
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -567,8 +567,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun
     EXPECT_LT(0.0, state_msg->output.velocities[1]);
     EXPECT_LT(0.0, state_msg->output.velocities[2]);
 
-    // use_closed_loop_pid_adapter_
-    if (traj_controller_->use_closed_loop_pid_adapter())
+    // use_closed_loop_control_law_
+    if (traj_controller_->use_closed_loop_control_law())
     {
       // we expect u = k_p * (s_d-s)
       EXPECT_NEAR(
@@ -806,8 +806,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
 
   if (traj_controller_->has_velocity_command_interface())
   {
-    // use_closed_loop_pid_adapter_
-    if (traj_controller_->use_closed_loop_pid_adapter())
+    // use_closed_loop_control_law_
+    if (traj_controller_->use_closed_loop_control_law())
     {
       // we expect u = k_p * (s_d-s) for positions[0] and positions[1]
       EXPECT_NEAR(
@@ -834,8 +834,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
 
   if (traj_controller_->has_effort_command_interface())
   {
-    // use_closed_loop_pid_adapter_
-    if (traj_controller_->use_closed_loop_pid_adapter())
+    // use_closed_loop_control_law_
+    if (traj_controller_->use_closed_loop_control_law())
     {
       // we expect u = k_p * (s_d-s) for positions[0] and positions[1]
       EXPECT_NEAR(
@@ -880,7 +880,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());
   }
 }
 
@@ -939,7 +939,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()))
   {
@@ -1005,8 +1005,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]);
diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/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; }
 
@@ -255,7 +255,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);
     }
@@ -481,7 +481,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())
       {
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/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,
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/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;
 
   /**
    */
diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/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_))
   {
diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp b/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);

Original file line number	Diff line number	Diff line change
`@@ -283,7 +283,7 @@ controller_interface::return_type JointTrajectoryController::update(`
`283`	`283`	`// set values for next hardware write() if tolerance is met`
`284`	`284`	`if (!tolerance_violated_while_moving && within_goal_time)`
`285`	`285`	`{`
`286`		`- if (use_closed_loop_pid_adapter_)`
	`286`	`+ if (use_closed_loop_control_law_)`
`287`	`287`	`{`
`288`	`288`	`traj_contr_->computeCommands(`
`289`	`289`	`tmp_command_, state_current_, state_error_, state_desired_, time, period);`
`@@ -296,7 +296,7 @@ controller_interface::return_type JointTrajectoryController::update(`
`296`	`296`	`}`
`297`	`297`	`if (has_velocity_command_interface_)`
`298`	`298`	`{`
`299`		`- if (use_closed_loop_pid_adapter_)`
	`299`	`+ if (use_closed_loop_control_law_)`
`300`	`300`	`{`
`301`	`301`	`assign_interface_from_point(joint_command_interface_[1], tmp_command_);`
`302`	`302`	`}`
`@@ -704,13 +704,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(`
`704`	`704`	`contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_EFFORT);`
`705`	`705`
`706`	`706`	`// if there is only velocity or if there is effort command interface`
`707`		`- // then use also PID adapter`
`708`		`- use_closed_loop_pid_adapter_ =`
	`707`	`+ // then use also closed loop controller`
	`708`	`+ use_closed_loop_control_law_ =`
`709`	`709`	`(has_velocity_command_interface_ && params_.command_interfaces.size() == 1 &&`
`710`	`710`	`!params_.open_loop_control) \|\|`
`711`	`711`	`has_effort_command_interface_;`
`712`	`712`
`713`		`- if (use_closed_loop_pid_adapter_)`
	`713`	`+ if (use_closed_loop_control_law_)`
`714`	`714`	`{`
`715`	`715`	`try`
`716`	`716`	`{`
`@@ -1011,14 +1011,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(`
`1011`	`1011`	`cmd_timeout_ = 0.0;`
`1012`	`1012`	`}`
`1013`	`1013`
`1014`		`- // update gains of PID controller`
`1015`		`- if (use_closed_loop_pid_adapter_)`
`1016`		`- {`
`1017`		`- trajectory_msgs::msg::JointTrajectory traj;`
`1018`		`- traj.points.push_back(state_current_);`
`1019`		`- traj_contr_->computeGains(traj);`
`1020`		`- }`
`1021`		`-`
`1022`	`1014`	`return CallbackReturn::SUCCESS;`
`1023`	`1015`	`}`
`1024`	`1016`
`@@ -1497,6 +1489,12 @@ void JointTrajectoryController::add_new_trajectory_msg(`
`1497`	`1489`	`const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg)`
`1498`	`1490`	`{`
`1499`	`1491`	`traj_msg_external_point_ptr_.writeFromNonRT(traj_msg);`
	`1492`	`+`
	`1493`	`+ // update gains of controller`
	`1494`	`+ if (use_closed_loop_control_law_)`
	`1495`	`+ {`
	`1496`	`+ traj_contr_->computeGains(traj_msg);`
	`1497`	`+ }`
`1500`	`1498`	`}`
`1501`	`1499`
`1502`	`1500`	`void JointTrajectoryController::preempt_active_goal()`
Original file line number	Diff line number	Diff line change
`@@ -567,8 +567,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun`
`567`	`567`	`EXPECT_LT(0.0, state_msg->output.velocities[1]);`
`568`	`568`	`EXPECT_LT(0.0, state_msg->output.velocities[2]);`
`569`	`569`
`570`		`- // use_closed_loop_pid_adapter_`
`571`		`- if (traj_controller_->use_closed_loop_pid_adapter())`
	`570`	`+ // use_closed_loop_control_law_`
	`571`	`+ if (traj_controller_->use_closed_loop_control_law())`
`572`	`572`	`{`
`573`	`573`	`// we expect u = k_p * (s_d-s)`
`574`	`574`	`EXPECT_NEAR(`
`@@ -806,8 +806,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)`
`806`	`806`
`807`	`807`	`if (traj_controller_->has_velocity_command_interface())`
`808`	`808`	`{`
`809`		`- // use_closed_loop_pid_adapter_`
`810`		`- if (traj_controller_->use_closed_loop_pid_adapter())`
	`809`	`+ // use_closed_loop_control_law_`
	`810`	`+ if (traj_controller_->use_closed_loop_control_law())`
`811`	`811`	`{`
`812`	`812`	`// we expect u = k_p * (s_d-s) for positions[0] and positions[1]`
`813`	`813`	`EXPECT_NEAR(`
`@@ -834,8 +834,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)`
`834`	`834`
`835`	`835`	`if (traj_controller_->has_effort_command_interface())`
`836`	`836`	`{`
`837`		`- // use_closed_loop_pid_adapter_`
`838`		`- if (traj_controller_->use_closed_loop_pid_adapter())`
	`837`	`+ // use_closed_loop_control_law_`
	`838`	`+ if (traj_controller_->use_closed_loop_control_law())`
`839`	`839`	`{`
`840`	`840`	`// we expect u = k_p * (s_d-s) for positions[0] and positions[1]`
`841`	`841`	`EXPECT_NEAR(`
`@@ -880,7 +880,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid)`
`880`	`880`	`!traj_controller_->is_open_loop()) \|\|`
`881`	`881`	`traj_controller_->has_effort_command_interface())`
`882`	`882`	`{`
`883`		`- EXPECT_TRUE(traj_controller_->use_closed_loop_pid_adapter());`
	`883`	`+ EXPECT_TRUE(traj_controller_->use_closed_loop_control_law());`
`884`	`884`	`}`
`885`	`885`	`}`
`886`	`886`
`@@ -939,7 +939,7 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error)`
`939`	`939`	`}`
`940`	`940`	`// is the velocity error correct?`
`941`	`941`	`if (`
`942`		`- traj_controller_->use_closed_loop_pid_adapter() // always needed for PID controller`
	`942`	`+ traj_controller_->use_closed_loop_control_law() // always needed for PID controller`
`943`	`943`	`\|\| (traj_controller_->has_velocity_state_interface() &&`
`944`	`944`	`traj_controller_->has_velocity_command_interface()))`
`945`	`945`	`{`
`@@ -1005,8 +1005,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order)`
`1005`	`1005`
`1006`	`1006`	`if (traj_controller_->has_velocity_command_interface())`
`1007`	`1007`	`{`
`1008`		`- // if use_closed_loop_pid_adapter_==false: we expect desired velocities from direct sampling`
`1009`		`- // if use_closed_loop_pid_adapter_==true: we expect desired velocities, because we use PID with`
	`1008`	`+ // if use_closed_loop_control_law_==false: we expect desired velocities from direct sampling`
	`1009`	`+ // if use_closed_loop_control_law_==true: we expect desired velocities, because we use PID with`
`1010`	`1010`	`// feedforward term only`
`1011`	`1011`	`EXPECT_GT(0.0, joint_vel_[0]);`
`1012`	`1012`	`EXPECT_GT(0.0, joint_vel_[1]);`
Original file line number	Diff line number	Diff line change
`@@ -126,7 +126,7 @@ class TestableJointTrajectoryController`
`126`	`126`
`127`	`127`	`bool has_effort_command_interface() const { return has_effort_command_interface_; }`
`128`	`128`
`129`		`- bool use_closed_loop_pid_adapter() const { return use_closed_loop_pid_adapter_; }`
	`129`	`+ bool use_closed_loop_control_law() const { return use_closed_loop_control_law_; }`
`130`	`130`
`131`	`131`	`bool is_open_loop() const { return params_.open_loop_control; }`
`132`	`132`
`@@ -255,7 +255,7 @@ class TrajectoryControllerTest : public ::testing::Test`
`255`	`255`
`256`	`256`	`// set pid parameters before activate. The PID plugin has to be loaded already, otherwise`
`257`	`257`	`// parameters are not declared yet`
`258`		`- if (traj_controller_->use_closed_loop_pid_adapter())`
	`258`	`+ if (traj_controller_->use_closed_loop_control_law())`
`259`	`259`	`{`
`260`	`260`	`SetPidParameters(k_p, ff);`
`261`	`261`	`}`
`@@ -481,7 +481,7 @@ class TrajectoryControllerTest : public ::testing::Test`
`481`	`481`	`// i.e., active but trivial trajectory (one point only)`
`482`	`482`	`EXPECT_TRUE(traj_controller_->has_trivial_traj());`
`483`	`483`
`484`		`- if (traj_controller_->use_closed_loop_pid_adapter() == false)`
	`484`	`+ if (traj_controller_->use_closed_loop_control_law() == false)`
`485`	`485`	`{`
`486`	`486`	`if (traj_controller_->has_position_command_interface())`
`487`	`487`	`{`
Original file line number	Diff line number	Diff line change
`@@ -50,7 +50,8 @@ bool PidTrajectoryPlugin::initialize(`
`50`	`50`	`return true;`
`51`	`51`	`}`
`52`	`52`
`53`		`-bool PidTrajectoryPlugin::computeGains(const trajectory_msgs::msg::JointTrajectory /trajectory/)`
	`53`	`+bool PidTrajectoryPlugin::computeGains(`
	`54`	`+ const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /trajectory/)`
`54`	`55`	`{`
`55`	`56`	`if (param_listener_->is_old(params_))`
`56`	`57`	`{`