Skip to content

Add limited state #22

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
wants to merge 2 commits into from
Closed

Add limited state #22

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
279e145
Merge pull request #30 from clearpathrobotics/add_timing_to_state
Feb 1, 2016
4c30982
Add limited state
Jan 29, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 5 additions & 2 deletions diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
7 changes: 6 additions & 1 deletion diff_drive_controller/msg/DiffDriveControllerState.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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:
Expand Down
76 changes: 52 additions & 24 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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);
Expand All @@ -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:
Expand All @@ -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:
Expand All @@ -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:
Expand All @@ -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
Expand Down Expand Up @@ -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] =
Expand All @@ -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] =
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This was a bug from #12

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];
Expand Down Expand Up @@ -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:
Expand Down