Merge pull request #31 from clearpathrobotics/add_limited_state

Add limited state

Author: Enrique Fernández Perdomo

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h

@@ -180,8 +180,11 @@ namespace diff_drive_controller
     double left_velocity_estimated_average_previous_;
     double right_velocity_estimated_average_previous_;
 
-    double left_velocity_command_previous_;
-    double right_velocity_command_previous_;
+    double left_velocity_desired_previous_;
+    double right_velocity_desired_previous_;
+
+    double left_velocity_limited_previous_;
+    double right_velocity_limited_previous_;
 
     /// Dynamic reconfigure server related:
     typedef dynamic_reconfigure::Server<DiffDriveControllerConfig> ReconfigureServer;

diff_drive_controller/msg/DiffDriveControllerState.msg

@@ -1,10 +1,15 @@
-# This initial part is the same as JointTrajectoryControllerState.msg:
+# This initial part is the same as JointTrajectoryControllerState.msg.
+# Note theat error = desired - actual, as it's generally done in any controller:
 Header header
 string[] joint_names
 trajectory_msgs/JointTrajectoryPoint desired
 trajectory_msgs/JointTrajectoryPoint actual
 trajectory_msgs/JointTrajectoryPoint error  # Redundant, but useful
 
+# Limited state, which might be different from the desired because of the
+# velocity/acceleration/jerk limits enforced, when they're enabled:
+trajectory_msgs/JointTrajectoryPoint limited
+
 # Actual wheel joint trajectory controller state estimated from the positions,
 # i.e. the velocities are NOT the ones directly reported from the joints, but
 # they're differentiated from the positions instead:

diff_drive_controller/src/diff_drive_controller.cpp

@@ -384,6 +384,11 @@ namespace diff_drive_controller
     state_pub_->msg_.actual.accelerations.resize(2 * wheel_joints_size_);
     state_pub_->msg_.actual.effort.resize(2 * wheel_joints_size_);
 
+    state_pub_->msg_.limited.positions.resize(2 * wheel_joints_size_);
+    state_pub_->msg_.limited.velocities.resize(2 * wheel_joints_size_);
+    state_pub_->msg_.limited.accelerations.resize(2 * wheel_joints_size_);
+    state_pub_->msg_.limited.effort.resize(2 * wheel_joints_size_);
+
     state_pub_->msg_.error.positions.resize(2 * wheel_joints_size_);
     state_pub_->msg_.error.velocities.resize(2 * wheel_joints_size_);
     state_pub_->msg_.error.accelerations.resize(2 * wheel_joints_size_);
@@ -446,8 +451,11 @@ namespace diff_drive_controller
     left_velocities_estimated_previous_.resize(wheel_joints_size_, 0.0);
     right_velocities_estimated_previous_.resize(wheel_joints_size_, 0.0);
 
-    left_velocity_command_previous_ = 0.0;
-    right_velocity_command_previous_ = 0.0;
+    left_velocity_desired_previous_ = 0.0;
+    right_velocity_desired_previous_ = 0.0;
+
+    left_velocity_limited_previous_ = 0.0;
+    right_velocity_limited_previous_ = 0.0;
 
     // Get the joint object to use in the realtime loop
     for (int i = 0; i < wheel_joints_size_; ++i)
@@ -664,6 +672,10 @@ namespace diff_drive_controller
       curr_cmd.ang = 0.0;
     }
 
+    // Compute desired (not limited) wheel velocity:
+    const double left_velocity_desired = (curr_cmd.lin - curr_cmd.ang * ws / 2.0)/wrl;
+    const double right_velocity_desired = (curr_cmd.lin + curr_cmd.ang * ws / 2.0)/wrl;
+
     // Limit velocities and accelerations:
     // @todo add an option to limit the velocity considering the actual velocity
     limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, control_period);
@@ -673,14 +685,14 @@ namespace diff_drive_controller
     last0_cmd_ = curr_cmd;
 
     // Compute wheels velocities:
-    const double left_velocity_command  = (curr_cmd.lin - curr_cmd.ang * ws / 2.0)/wrl;
-    const double right_velocity_command = (curr_cmd.lin + curr_cmd.ang * ws / 2.0)/wrr;
+    const double left_velocity_limited  = (curr_cmd.lin - curr_cmd.ang * ws / 2.0)/wrl;
+    const double right_velocity_limited = (curr_cmd.lin + curr_cmd.ang * ws / 2.0)/wrr;
 
     // Set wheels velocities:
     for (size_t i = 0; i < wheel_joints_size_; ++i)
     {
-      left_wheel_joints_[i].setCommand(left_velocity_command);
-      right_wheel_joints_[i].setCommand(right_velocity_command);
+      left_wheel_joints_[i].setCommand(left_velocity_limited);
+      right_wheel_joints_[i].setCommand(right_velocity_limited);
     }
 
     // Publish limited velocity command:
@@ -701,13 +713,13 @@ namespace diff_drive_controller
       {
         state_pub_->msg_.header.stamp = time;
 
-        // Set left wheel joints desired, actual (and actual estimated) state:
+        // Set left wheel joints desired, actual (estimated), limited state:
         for (size_t i = 0; i < wheel_joints_size_; ++i)
         {
           // Desired state:
-          state_pub_->msg_.desired.accelerations[i] = (left_velocity_command - left_velocity_command_previous_) * control_frequency;
-          state_pub_->msg_.desired.velocities[i] = left_velocity_command;
-          state_pub_->msg_.desired.positions[i] += left_velocity_command * control_period;
+          state_pub_->msg_.desired.accelerations[i] = (left_velocity_desired - left_velocity_desired_previous_) * control_frequency;
+          state_pub_->msg_.desired.velocities[i] = left_velocity_desired;
+          state_pub_->msg_.desired.positions[i] += left_velocity_desired * control_period;
           state_pub_->msg_.desired.effort[i] = std::numeric_limits<double>::quiet_NaN();
 
           // Actual state:
@@ -725,15 +737,21 @@ namespace diff_drive_controller
           state_pub_->msg_.actual_estimated.velocities[i] = left_velocities_estimated_[i];
           state_pub_->msg_.actual_estimated.positions[i] = left_positions_estimated_[i];
           state_pub_->msg_.actual_estimated.effort[i] = state_pub_->msg_.actual.effort[i];
+
+          // Limited state:
+          state_pub_->msg_.limited.accelerations[i] = (left_velocity_limited - left_velocity_limited_previous_) * control_frequency;
+          state_pub_->msg_.limited.velocities[i] = left_velocity_limited;
+          state_pub_->msg_.limited.positions[i] += left_velocity_limited * control_period;
+          state_pub_->msg_.limited.effort[i] = std::numeric_limits<double>::quiet_NaN();
         }
 
-        // Set right wheel joints desired, actual (and actual estimated) state:
+        // Set right wheel joints desired, actual (estimated), limited state:
         for (size_t i = 0, j = wheel_joints_size_; i < wheel_joints_size_; ++i, ++j)
         {
           // Desired state:
-          state_pub_->msg_.desired.accelerations[j] = (right_velocity_command - right_velocity_command_previous_) * control_frequency;
-          state_pub_->msg_.desired.velocities[j] = right_velocity_command;
-          state_pub_->msg_.desired.positions[j] += right_velocity_command * control_period;
+          state_pub_->msg_.desired.accelerations[j] = (right_velocity_desired - right_velocity_desired_previous_) * control_frequency;
+          state_pub_->msg_.desired.velocities[j] = right_velocity_desired;
+          state_pub_->msg_.desired.positions[j] += right_velocity_desired * control_period;
           state_pub_->msg_.desired.effort[j] = std::numeric_limits<double>::quiet_NaN();
 
           // Actual state:
@@ -751,6 +769,12 @@ namespace diff_drive_controller
           state_pub_->msg_.actual_estimated.velocities[j] = right_velocities_estimated_[i];
           state_pub_->msg_.actual_estimated.positions[j] = right_positions_estimated_[i];
           state_pub_->msg_.actual_estimated.effort[j] = state_pub_->msg_.actual.effort[j];
+
+          // Limited state:
+          state_pub_->msg_.limited.accelerations[j] = (right_velocity_limited - right_velocity_limited_previous_) * control_frequency;
+          state_pub_->msg_.limited.velocities[j] = right_velocity_limited;
+          state_pub_->msg_.limited.positions[j] += right_velocity_limited * control_period;
+          state_pub_->msg_.limited.effort[j] = std::numeric_limits<double>::quiet_NaN();
         }
 
         // Set left wheel joints actual (and actual estimated) side average
@@ -785,6 +809,7 @@ namespace diff_drive_controller
         state_pub_->msg_.actual_estimated_side_average.positions[1] = right_position_estimated_average;
         state_pub_->msg_.actual_estimated_side_average.effort[1] = state_pub_->msg_.actual.effort[wheel_joints_size_];
 
+        // Set state error:
         for (size_t i = 0; i < 2 * wheel_joints_size_; ++i)
         {
           state_pub_->msg_.error.positions[i] =
@@ -805,14 +830,14 @@ namespace diff_drive_controller
           state_pub_->msg_.error_estimated.effort[i] =
             state_pub_->msg_.desired.effort[i] - state_pub_->msg_.actual_estimated.effort[i];
 
-          state_pub_->msg_.error_estimated.positions[i] =
-            state_pub_->msg_.desired.positions[i] - state_pub_->msg_.actual_estimated.positions[i];
-          state_pub_->msg_.error_estimated.velocities[i] =
-            state_pub_->msg_.desired.velocities[i] - state_pub_->msg_.actual_estimated.velocities[i];
-          state_pub_->msg_.error_estimated.accelerations[i] =
-            state_pub_->msg_.desired.accelerations[i] - state_pub_->msg_.actual_estimated.accelerations[i];
-          state_pub_->msg_.error_estimated.effort[i] =
-            state_pub_->msg_.desired.effort[i] - state_pub_->msg_.actual_estimated.effort[i];
+          state_pub_->msg_.error_side_average.positions[i] =
+            state_pub_->msg_.desired.positions[i] - state_pub_->msg_.actual_side_average.positions[i];
+          state_pub_->msg_.error_side_average.velocities[i] =
+            state_pub_->msg_.desired.velocities[i] - state_pub_->msg_.actual_side_average.velocities[i];
+          state_pub_->msg_.error_side_average.accelerations[i] =
+            state_pub_->msg_.desired.accelerations[i] - state_pub_->msg_.actual_side_average.accelerations[i];
+          state_pub_->msg_.error_side_average.effort[i] =
+            state_pub_->msg_.desired.effort[i] - state_pub_->msg_.actual_side_average.effort[i];
 
           state_pub_->msg_.error_estimated_side_average.positions[i] =
             state_pub_->msg_.desired.positions[i] - state_pub_->msg_.actual_estimated_side_average.positions[i];
@@ -871,8 +896,11 @@ namespace diff_drive_controller
       left_velocity_estimated_average_previous_ = left_velocity_estimated_average;
       right_velocity_estimated_average_previous_ = right_velocity_estimated_average;
 
-      left_velocity_command_previous_ = left_velocity_command;
-      right_velocity_command_previous_ = right_velocity_command;
+      left_velocity_desired_previous_ = left_velocity_desired;
+      right_velocity_desired_previous_ = right_velocity_desired;
+
+      left_velocity_limited_previous_ = left_velocity_limited;
+      right_velocity_limited_previous_ = right_velocity_limited;
     }
 
     // Save wheel joints positions, needed to estimate the velocities: