Merge branches 'joint_pos_vel_startup_bugfix_melodic', 'pid_integrato…

…r_state_melodic' and 'pos_controller_no_redundant_init_melodic' into improve_joint_calibration_controller
PR2 · Aug 10, 2022 · a1a4789 · a1a4789
4 parents 8cf343c + dc586dc + fe9086e + 5548003
commit a1a4789
Show file tree

Hide file tree

Showing 5 changed files with 14 additions and 12 deletions.
diff --git a/pr2_controllers_msgs/msg/JointControllerState.msg b/pr2_controllers_msgs/msg/JointControllerState.msg
@@ -3,6 +3,7 @@ float64 set_point
 float64 process_value
 float64 process_value_dot
 float64 error
+float64 i_error
 float64 time_step
 float64 command
 float64 p

diff --git a/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h b/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h
@@ -96,6 +96,7 @@ class JointPositionController : public pr2_controller_interface::Controller
   virtual void starting() {
     command_ = joint_state_->position_;
     pid_controller_.reset();
+    last_time_ = robot_->getTime();
   }
 
   /*!
@@ -113,7 +114,6 @@ class JointPositionController : public pr2_controller_interface::Controller
 
 private:
   int loop_count_;
-  bool initialized_;
   pr2_mechanism_model::RobotState *robot_;              /**< Pointer to robot structure. */
   control_toolbox::Pid pid_controller_;       /**< Internal PID controller. */
   ros::Time last_time_;                          /**< Last time stamp of update. */

diff --git a/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h b/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h
@@ -105,6 +105,7 @@ class JointVelocityController : public pr2_controller_interface::Controller
   virtual void starting() {
     command_ = 0.0;
     pid_controller_.reset();
+    last_time_ = robot_->getTime();
   }
   virtual void update();
 

diff --git a/robot_mechanism_controllers/src/joint_position_controller.cpp b/robot_mechanism_controllers/src/joint_position_controller.cpp
@@ -44,7 +44,7 @@ namespace controller {
 
 JointPositionController::JointPositionController()
 : joint_state_(NULL), command_(0),
-  loop_count_(0),  initialized_(false), robot_(NULL), last_time_(0)
+  loop_count_(0), robot_(NULL), last_time_(0)
 {
 }
 
@@ -58,7 +58,6 @@ bool JointPositionController::init(pr2_mechanism_model::RobotState *robot, const
 {
   assert(robot);
   robot_ = robot;
-  last_time_ = robot->getTime();
 
   joint_state_ = robot_->getJointState(joint_name);
   if (!joint_state_)
@@ -139,13 +138,7 @@ void JointPositionController::update()
   double error(0);
   ros::Time time = robot_->getTime();
   assert(joint_state_->joint_);
-  dt_= time - last_time_;
-
-  if (!initialized_)
-  {
-    initialized_ = true;
-    command_ = joint_state_->position_;
-  }
+  dt_= time - last_time_;   //will be 0.0 if starting() was called on this iteration
 
   if(joint_state_->joint_->type == urdf::Joint::REVOLUTE)
   {
@@ -175,11 +168,15 @@ void JointPositionController::update()
   {
     if(controller_state_publisher_ && controller_state_publisher_->trylock())
     {
+      double p_error, i_error, d_error;
+      pid_controller_.getCurrentPIDErrors(&p_error, &i_error, &d_error);
+
       controller_state_publisher_->msg_.header.stamp = time;
       controller_state_publisher_->msg_.set_point = command_;
       controller_state_publisher_->msg_.process_value = joint_state_->position_;
       controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_;
       controller_state_publisher_->msg_.error = error;
+      controller_state_publisher_->msg_.i_error = i_error;
       controller_state_publisher_->msg_.time_step = dt_.toSec();
       controller_state_publisher_->msg_.command = commanded_effort;
 

diff --git a/robot_mechanism_controllers/src/joint_velocity_controller.cpp b/robot_mechanism_controllers/src/joint_velocity_controller.cpp
@@ -56,7 +56,6 @@ bool JointVelocityController::init(pr2_mechanism_model::RobotState *robot, const
 {
   assert(robot);
   robot_ = robot;
-  last_time_ = robot->getTime();
 
   joint_state_ = robot_->getJointState(joint_name);
   if (!joint_state_)
@@ -136,18 +135,22 @@ void JointVelocityController::update()
   ros::Time time = robot_->getTime();
 
   double error = command_ - joint_state_->velocity_;
-  dt_ = time - last_time_;
+  dt_ = time - last_time_;  //will be 0.0 if starting() was called on this iteration
   double command = pid_controller_.computeCommand(error, dt_);
   joint_state_->commanded_effort_ += command;
 
   if(loop_count_ % 10 == 0)
   {
     if(controller_state_publisher_ && controller_state_publisher_->trylock())
     {
+      double p_error, i_error, d_error;
+      pid_controller_.getCurrentPIDErrors(&p_error, &i_error, &d_error);
+
       controller_state_publisher_->msg_.header.stamp = time;
       controller_state_publisher_->msg_.set_point = command_;
       controller_state_publisher_->msg_.process_value = joint_state_->velocity_;
       controller_state_publisher_->msg_.error = error;
+      controller_state_publisher_->msg_.i_error = i_error;
       controller_state_publisher_->msg_.time_step = dt_.toSec();
       controller_state_publisher_->msg_.command = command;