diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h index 90580701f..c69f2368d 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h +++ b/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 ReconfigureServer; diff --git a/diff_drive_controller/msg/DiffDriveControllerState.msg b/diff_drive_controller/msg/DiffDriveControllerState.msg index d7067f16c..8c055ef68 100644 --- a/diff_drive_controller/msg/DiffDriveControllerState.msg +++ b/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 --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index c462d5e72..5b14eff6b 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/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::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::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::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::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: