Rename pid_adapter variable and update gains with every new trajectory

christophfroehlich · christophfroehlich · commit aac830600424 · 2023-10-30T20:23:11.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
@@ -164,7 +164,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
@@ -281,7 +281,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);
@@ -294,7 +294,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_);
           }
@@ -718,13 +718,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
     {
@@ -1023,14 +1023,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
   }
   rt_is_holding_.writeFromNonRT(true);
 
-  // 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;
 }
 
@@ -1509,6 +1501,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
@@ -533,8 +533,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized)
     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(
@@ -773,8 +773,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
 
   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(
@@ -801,8 +801,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
 
   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(
@@ -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());
   }
 }
 
@@ -906,7 +906,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()))
   {
@@ -972,8 +972,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; }
 
@@ -248,7 +248,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);
     }
@@ -466,7 +466,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
`@@ -281,7 +281,7 @@ controller_interface::return_type JointTrajectoryController::update(`
`281`	`281`	`// set values for next hardware write() if tolerance is met`
`282`	`282`	`if (!tolerance_violated_while_moving && within_goal_time)`
`283`	`283`	`{`
`284`		`- if (use_closed_loop_pid_adapter_)`
	`284`	`+ if (use_closed_loop_control_law_)`
`285`	`285`	`{`
`286`	`286`	`traj_contr_->computeCommands(`
`287`	`287`	`tmp_command_, state_current_, state_error_, state_desired_, time, period);`
`@@ -294,7 +294,7 @@ controller_interface::return_type JointTrajectoryController::update(`
`294`	`294`	`}`
`295`	`295`	`if (has_velocity_command_interface_)`
`296`	`296`	`{`
`297`		`- if (use_closed_loop_pid_adapter_)`
	`297`	`+ if (use_closed_loop_control_law_)`
`298`	`298`	`{`
`299`	`299`	`assign_interface_from_point(joint_command_interface_[1], tmp_command_);`
`300`	`300`	`}`
`@@ -718,13 +718,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(`
`718`	`718`	`contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_EFFORT);`
`719`	`719`
`720`	`720`	`// if there is only velocity or if there is effort command interface`
`721`		`- // then use also PID adapter`
`722`		`- use_closed_loop_pid_adapter_ =`
	`721`	`+ // then use also closed loop controller`
	`722`	`+ use_closed_loop_control_law_ =`
`723`	`723`	`(has_velocity_command_interface_ && params_.command_interfaces.size() == 1 &&`
`724`	`724`	`!params_.open_loop_control) \|\|`
`725`	`725`	`has_effort_command_interface_;`
`726`	`726`
`727`		`- if (use_closed_loop_pid_adapter_)`
	`727`	`+ if (use_closed_loop_control_law_)`
`728`	`728`	`{`
`729`	`729`	`try`
`730`	`730`	`{`
`@@ -1023,14 +1023,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(`
`1023`	`1023`	`}`
`1024`	`1024`	`rt_is_holding_.writeFromNonRT(true);`
`1025`	`1025`
`1026`		`- // update gains of PID controller`
`1027`		`- if (use_closed_loop_pid_adapter_)`
`1028`		`- {`
`1029`		`- trajectory_msgs::msg::JointTrajectory traj;`
`1030`		`- traj.points.push_back(state_current_);`
`1031`		`- traj_contr_->computeGains(traj);`
`1032`		`- }`
`1033`		`-`
`1034`	`1026`	`return CallbackReturn::SUCCESS;`
`1035`	`1027`	`}`
`1036`	`1028`
`@@ -1509,6 +1501,12 @@ void JointTrajectoryController::add_new_trajectory_msg(`
`1509`	`1501`	`const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg)`
`1510`	`1502`	`{`
`1511`	`1503`	`traj_msg_external_point_ptr_.writeFromNonRT(traj_msg);`
	`1504`	`+`
	`1505`	`+ // update gains of controller`
	`1506`	`+ if (use_closed_loop_control_law_)`
	`1507`	`+ {`
	`1508`	`+ traj_contr_->computeGains(traj_msg);`
	`1509`	`+ }`
`1512`	`1510`	`}`
`1513`	`1511`
`1514`	`1512`	`void JointTrajectoryController::preempt_active_goal()`
Original file line number	Diff line number	Diff line change
`@@ -533,8 +533,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized)`
`533`	`533`	`EXPECT_LT(0.0, state_msg->output.velocities[1]);`
`534`	`534`	`EXPECT_LT(0.0, state_msg->output.velocities[2]);`
`535`	`535`
`536`		`- // use_closed_loop_pid_adapter_`
`537`		`- if (traj_controller_->use_closed_loop_pid_adapter())`
	`536`	`+ // use_closed_loop_control_law_`
	`537`	`+ if (traj_controller_->use_closed_loop_control_law())`
`538`	`538`	`{`
`539`	`539`	`// we expect u = k_p * (s_d-s)`
`540`	`540`	`EXPECT_NEAR(`
`@@ -773,8 +773,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)`
`773`	`773`
`774`	`774`	`if (traj_controller_->has_velocity_command_interface())`
`775`	`775`	`{`
`776`		`- // use_closed_loop_pid_adapter_`
`777`		`- if (traj_controller_->use_closed_loop_pid_adapter())`
	`776`	`+ // use_closed_loop_control_law_`
	`777`	`+ if (traj_controller_->use_closed_loop_control_law())`
`778`	`778`	`{`
`779`	`779`	`// we expect u = k_p * (s_d-s) for positions[0] and positions[1]`
`780`	`780`	`EXPECT_NEAR(`
`@@ -801,8 +801,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)`
`801`	`801`
`802`	`802`	`if (traj_controller_->has_effort_command_interface())`
`803`	`803`	`{`
`804`		`- // use_closed_loop_pid_adapter_`
`805`		`- if (traj_controller_->use_closed_loop_pid_adapter())`
	`804`	`+ // use_closed_loop_control_law_`
	`805`	`+ if (traj_controller_->use_closed_loop_control_law())`
`806`	`806`	`{`
`807`	`807`	`// we expect u = k_p * (s_d-s) for positions[0] and positions[1]`
`808`	`808`	`EXPECT_NEAR(`
`@@ -847,7 +847,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid)`
`847`	`847`	`!traj_controller_->is_open_loop()) \|\|`
`848`	`848`	`traj_controller_->has_effort_command_interface())`
`849`	`849`	`{`
`850`		`- EXPECT_TRUE(traj_controller_->use_closed_loop_pid_adapter());`
	`850`	`+ EXPECT_TRUE(traj_controller_->use_closed_loop_control_law());`
`851`	`851`	`}`
`852`	`852`	`}`
`853`	`853`
`@@ -906,7 +906,7 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error)`
`906`	`906`	`}`
`907`	`907`	`// is the velocity error correct?`
`908`	`908`	`if (`
`909`		`- traj_controller_->use_closed_loop_pid_adapter() // always needed for PID controller`
	`909`	`+ traj_controller_->use_closed_loop_control_law() // always needed for PID controller`
`910`	`910`	`\|\| (traj_controller_->has_velocity_state_interface() &&`
`911`	`911`	`traj_controller_->has_velocity_command_interface()))`
`912`	`912`	`{`
`@@ -972,8 +972,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order)`
`972`	`972`
`973`	`973`	`if (traj_controller_->has_velocity_command_interface())`
`974`	`974`	`{`
`975`		`- // if use_closed_loop_pid_adapter_==false: we expect desired velocities from direct sampling`
`976`		`- // if use_closed_loop_pid_adapter_==true: we expect desired velocities, because we use PID with`
	`975`	`+ // if use_closed_loop_control_law_==false: we expect desired velocities from direct sampling`
	`976`	`+ // if use_closed_loop_control_law_==true: we expect desired velocities, because we use PID with`
`977`	`977`	`// feedforward term only`
`978`	`978`	`EXPECT_GT(0.0, joint_vel_[0]);`
`979`	`979`	`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`
`@@ -248,7 +248,7 @@ class TrajectoryControllerTest : public ::testing::Test`
`248`	`248`
`249`	`249`	`// set pid parameters before activate. The PID plugin has to be loaded already, otherwise`
`250`	`250`	`// parameters are not declared yet`
`251`		`- if (traj_controller_->use_closed_loop_pid_adapter())`
	`251`	`+ if (traj_controller_->use_closed_loop_control_law())`
`252`	`252`	`{`
`253`	`253`	`SetPidParameters(k_p, ff);`
`254`	`254`	`}`
`@@ -466,7 +466,7 @@ class TrajectoryControllerTest : public ::testing::Test`
`466`	`466`	`// i.e., active but trivial trajectory (one point only)`
`467`	`467`	`EXPECT_TRUE(traj_controller_->has_trivial_traj());`
`468`	`468`
`469`		`- if (traj_controller_->use_closed_loop_pid_adapter() == false)`
	`469`	`+ if (traj_controller_->use_closed_loop_control_law() == false)`
`470`	`470`	`{`
`471`	`471`	`if (traj_controller_->has_position_command_interface())`
`472`	`472`	`{`
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`	`{`