Skip to content

Commit

Permalink
minor refactoring, increased drs km_lim limit
Browse files Browse the repository at this point in the history
  • Loading branch information
@klaxalk
klaxalk committed Apr 21, 2024
1 parent 4b11bad commit edfccc8
Show file tree
Hide file tree
Showing 4 changed files with 41 additions and 33 deletions.
2 changes: 1 addition & 1 deletion cfg/mpc_controller.cfg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ attitude.add("kq_yaw", double_t, 0, "Attitude constant for intrinsic yaw control
mass = gen.add_group("Mass estimator");

mass.add("km", double_t, 0, "Integral constant for mass", 0.0, 0.0, 2.0)
mass.add("km_lim", double_t, 0, "mass integral limit", 0.0, 0.0, 10.0)
mass.add("km_lim", double_t, 0, "mass integral limit", 0.0, 0.0, 50.0)

output = gen.add_group("Output");

Expand Down
2 changes: 1 addition & 1 deletion cfg/se3_controller.cfg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ attitude.add("kq_yaw", double_t, 0, "Attitude constant for intrinsic yaw control
attitude = gen.add_group("Mass estimator");

attitude.add("km", double_t, 0, "Integral constant for mass", 0.0, 0.0, 2.0)
attitude.add("km_lim", double_t, 0, "Mass integral limit", 0.0, 0.0, 10.0)
attitude.add("km_lim", double_t, 0, "Mass integral limit", 0.0, 0.0, 50.0)

output = gen.add_group("Output");

Expand Down
48 changes: 26 additions & 22 deletions src/mpc_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -344,7 +344,7 @@ bool MpcController::initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_ua

_tilt_angle_failsafe_ = M_PI * (_tilt_angle_failsafe_ / 180.0);

if (_tilt_angle_failsafe_enabled_ && fabs(_tilt_angle_failsafe_) < 1e-3) {
if (_tilt_angle_failsafe_enabled_ && std::abs(_tilt_angle_failsafe_) < 1e-3) {
ROS_ERROR("[%s]: constraints/tilt_angle_failsafe/enabled = 'TRUE' but the limit is too low", name_.c_str());
return false;
}
Expand Down Expand Up @@ -499,7 +499,7 @@ bool MpcController::activate(const ControlOutput &last_control_output) {
rampup_last_time_ = ros::Time::now();
rampup_throttle_ = throttle_last_controller.value();

rampup_duration_ = fabs(throttle_difference) / _rampup_speed_;
rampup_duration_ = std::abs(throttle_difference) / _rampup_speed_;
}

first_iteration_ = true;
Expand Down Expand Up @@ -577,7 +577,7 @@ MpcController::ControlOutput MpcController::updateActive(const mrs_msgs::UavStat

last_update_time_ = uav_state.header.stamp;

if (fabs(dt) < 0.001) {
if (std::abs(dt) < 0.001) {

ROS_DEBUG("[%s]: the last odometry message came too close (%.2f s)!", name_.c_str(), dt);

Expand Down Expand Up @@ -848,22 +848,22 @@ void MpcController::MPC(const mrs_msgs::UavState &uav_state, const mrs_msgs::Tra
double velocity;
double coef = 1.5;

if (fabs(uav_state.acceleration.linear.x) < coef * max_acceleration_horizontal) {
if (std::abs(uav_state.acceleration.linear.x) < coef * max_acceleration_horizontal) {
acceleration = uav_state.acceleration.linear.x;
} else {
acceleration = tracker_command.acceleration.x;

ROS_ERROR_THROTTLE(1.0, "[%s]: odometry x acceleration exceeds constraints (%.2f > %.1f * %.2f m), using reference for initial condition", name_.c_str(),
fabs(uav_state.acceleration.linear.x), coef, max_acceleration_horizontal);
std::abs(uav_state.acceleration.linear.x), coef, max_acceleration_horizontal);
}

if (fabs(uav_state.velocity.linear.x) < coef * max_speed_horizontal) {
if (std::abs(uav_state.velocity.linear.x) < coef * max_speed_horizontal) {
velocity = uav_state.velocity.linear.x;
} else {
velocity = tracker_command.velocity.x;

ROS_ERROR_THROTTLE(1.0, "[%s]: odometry x velocity exceeds constraints (%.2f > %0.1f * %.2f m), using reference for initial condition", name_.c_str(),
fabs(uav_state.velocity.linear.x), coef, max_speed_horizontal);
std::abs(uav_state.velocity.linear.x), coef, max_speed_horizontal);
}

initial_x << uav_state.pose.position.x, velocity, acceleration;
Expand All @@ -878,22 +878,22 @@ void MpcController::MPC(const mrs_msgs::UavState &uav_state, const mrs_msgs::Tra
double velocity;
double coef = 1.5;

if (fabs(uav_state.acceleration.linear.y) < coef * max_acceleration_horizontal) {
if (std::abs(uav_state.acceleration.linear.y) < coef * max_acceleration_horizontal) {
acceleration = uav_state.acceleration.linear.y;
} else {
acceleration = tracker_command.acceleration.y;

ROS_ERROR_THROTTLE(1.0, "[%s]: odometry y acceleration exceeds constraints (%.2f > %.1f * %.2f m), using reference for initial condition", name_.c_str(),
fabs(uav_state.acceleration.linear.y), coef, max_acceleration_horizontal);
std::abs(uav_state.acceleration.linear.y), coef, max_acceleration_horizontal);
}

if (fabs(uav_state.velocity.linear.y) < coef * max_speed_horizontal) {
if (std::abs(uav_state.velocity.linear.y) < coef * max_speed_horizontal) {
velocity = uav_state.velocity.linear.y;
} else {
velocity = tracker_command.velocity.y;

ROS_ERROR_THROTTLE(1.0, "[%s]: odometry y velocity exceeds constraints (%.2f > %0.1f * %.2f m), using reference for initial condition", name_.c_str(),
fabs(uav_state.velocity.linear.y), coef, max_speed_horizontal);
std::abs(uav_state.velocity.linear.y), coef, max_speed_horizontal);
}

initial_y << uav_state.pose.position.y, velocity, acceleration;
Expand All @@ -908,22 +908,22 @@ void MpcController::MPC(const mrs_msgs::UavState &uav_state, const mrs_msgs::Tra
double velocity;
double coef = 1.5;

if (fabs(uav_state.acceleration.linear.z) < coef * max_acceleration_horizontal) {
if (std::abs(uav_state.acceleration.linear.z) < coef * max_acceleration_horizontal) {
acceleration = uav_state.acceleration.linear.z;
} else {
acceleration = tracker_command.acceleration.z;

ROS_ERROR_THROTTLE(1.0, "[%s]: odometry z acceleration exceeds constraints (%.2f > %.1f * %.2f m), using reference for initial condition", name_.c_str(),
fabs(uav_state.acceleration.linear.z), coef, max_acceleration_horizontal);
std::abs(uav_state.acceleration.linear.z), coef, max_acceleration_horizontal);
}

if (fabs(uav_state.velocity.linear.z) < coef * max_speed_vertical) {
if (std::abs(uav_state.velocity.linear.z) < coef * max_speed_vertical) {
velocity = uav_state.velocity.linear.z;
} else {
velocity = tracker_command.velocity.z;

ROS_ERROR_THROTTLE(1.0, "[%s]: odometry z velocity exceeds constraints (%.2f > %0.1f * %.2f m), using reference for initial condition", name_.c_str(),
fabs(uav_state.velocity.linear.z), coef, max_speed_vertical);
std::abs(uav_state.velocity.linear.z), coef, max_speed_vertical);
}

initial_z << uav_state.pose.position.z, velocity, acceleration;
Expand Down Expand Up @@ -1393,7 +1393,7 @@ void MpcController::MPC(const mrs_msgs::UavState &uav_state, const mrs_msgs::Tra
// antiwindup
double temp_gain = gains.km;
if (rampup_active_ ||
(fabs(uav_state.velocity.linear.z) > 0.3 && ((Ep(2) > 0 && uav_state.velocity.linear.z > 0) || (Ep(2) < 0 && uav_state.velocity.linear.z < 0)))) {
(std::abs(uav_state.velocity.linear.z) > 0.3 && ((Ep(2) > 0 && uav_state.velocity.linear.z > 0) || (Ep(2) < 0 && uav_state.velocity.linear.z < 0)))) {
temp_gain = 0;
ROS_DEBUG_THROTTLE(1.0, "[%s]: anti-windup for the mass kicks in", this->name_.c_str());
}
Expand Down Expand Up @@ -1500,7 +1500,7 @@ void MpcController::MPC(const mrs_msgs::UavState &uav_state, const mrs_msgs::Tra
if (rampup_active_) {

// deactivate the rampup when the times up
if (fabs((ros::Time::now() - rampup_start_time_).toSec()) >= rampup_duration_) {
if (std::abs((ros::Time::now() - rampup_start_time_).toSec()) >= rampup_duration_) {

rampup_active_ = false;

Expand Down Expand Up @@ -2004,7 +2004,11 @@ void MpcController::timerGains(const ros::TimerEvent &event) {

mute_gains_ = false;

const double dt = (event.current_real - event.last_real).toSec();
double dt = (event.current_real - event.last_real).toSec();

if (!std::isfinite(dt) || (dt <= 0) || (dt > 5 * (1.0 / _gain_filtering_rate_))) {
return;
}

bool updated = false;

Expand Down Expand Up @@ -2062,29 +2066,29 @@ double MpcController::calculateGainChange(const double dt, const double current_
double change_in_perc;
double saturated_change;

if (fabs(current_value) < 1e-6) {
if (std::abs(current_value) < 1e-6) {
change *= gains_filter_max_change;
} else {

saturated_change = change;

change_in_perc = (current_value + saturated_change) / current_value - 1.0;
change_in_perc = ((current_value + saturated_change) / current_value) - 1.0;

if (change_in_perc > gains_filter_max_change) {
saturated_change = current_value * gains_filter_max_change;
} else if (change_in_perc < -gains_filter_max_change) {
saturated_change = current_value * -gains_filter_max_change;
}

if (fabs(saturated_change) < fabs(change) * gains_filter_min_change) {
if (std::abs(saturated_change) < std::abs(change) * gains_filter_min_change) {
change *= gains_filter_min_change;
} else {
change = saturated_change;
}
}
}

if (fabs(change) > 1e-3) {
if (std::abs(change) > 1e-3) {
ROS_DEBUG("[%s]: changing gain '%s' from %.2f to %.2f", name_.c_str(), name.c_str(), current_value, desired_value);
updated = true;
}
Expand Down
22 changes: 13 additions & 9 deletions src/se3_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -280,7 +280,7 @@ bool Se3Controller::initialize(const ros::NodeHandle& nh, std::shared_ptr<mrs_ua

_tilt_angle_failsafe_ = M_PI * (_tilt_angle_failsafe_ / 180.0);

if (_tilt_angle_failsafe_enabled_ && fabs(_tilt_angle_failsafe_) < 1e-3) {
if (_tilt_angle_failsafe_enabled_ && std::abs(_tilt_angle_failsafe_) < 1e-3) {
ROS_ERROR("[Se3Controller]: constraints/tilt_angle_failsafe/enabled = 'TRUE' but the limit is too low");
return false;
}
Expand Down Expand Up @@ -436,7 +436,7 @@ bool Se3Controller::activate(const ControlOutput& last_control_output) {
rampup_last_time_ = ros::Time::now();
rampup_throttle_ = throttle_last_controller.value();

rampup_duration_ = fabs(throttle_difference) / _rampup_speed_;
rampup_duration_ = std::abs(throttle_difference) / _rampup_speed_;
}

first_iteration_ = true;
Expand Down Expand Up @@ -512,7 +512,7 @@ Se3Controller::ControlOutput Se3Controller::updateActive(const mrs_msgs::UavStat

last_update_time_ = uav_state.header.stamp;

if (fabs(dt) < 0.001) {
if (std::abs(dt) < 0.001) {

ROS_DEBUG("[Se3Controller]: the last odometry message came too close (%.2f s)!", dt);

Expand Down Expand Up @@ -1252,7 +1252,7 @@ void Se3Controller::SE3Controller(const mrs_msgs::UavState& uav_state, const mrs
} else if (rampup_active_) {

// deactivate the rampup when the times up
if (fabs((ros::Time::now() - rampup_start_time_).toSec()) >= rampup_duration_) {
if (std::abs((ros::Time::now() - rampup_start_time_).toSec()) >= rampup_duration_) {

rampup_active_ = false;

Expand Down Expand Up @@ -1707,7 +1707,11 @@ void Se3Controller::timerGains(const ros::TimerEvent& event) {

mute_gains_ = false;

const double dt = (event.current_real - event.last_real).toSec();
double dt = (event.current_real - event.last_real).toSec();

if (!std::isfinite(dt) || (dt <= 0) || (dt > 5 * (1.0 / _gain_filtering_rate_))) {
return;
}

bool updated = false;

Expand Down Expand Up @@ -1777,29 +1781,29 @@ double Se3Controller::calculateGainChange(const double dt, const double current_
double change_in_perc;
double saturated_change;

if (fabs(current_value) < 1e-6) {
if (std::abs(current_value) < 1e-6) {
change *= gains_filter_max_change;
} else {

saturated_change = change;

change_in_perc = (current_value + saturated_change) / current_value - 1.0;
change_in_perc = ((current_value + saturated_change) / current_value) - 1.0;

if (change_in_perc > gains_filter_max_change) {
saturated_change = current_value * gains_filter_max_change;
} else if (change_in_perc < -gains_filter_max_change) {
saturated_change = current_value * -gains_filter_max_change;
}

if (fabs(saturated_change) < fabs(change) * gains_filter_min_change) {
if (std::abs(saturated_change) < std::abs(change) * gains_filter_min_change) {
change *= gains_filter_min_change;
} else {
change = saturated_change;
}
}
}

if (fabs(change) > 1e-3) {
if (std::abs(change) > 1e-3) {
ROS_DEBUG("[Se3Controller]: changing gain '%s' from %.2f to %.2f", name.c_str(), current_value, desired_value);
updated = true;
}
Expand Down

0 comments on commit edfccc8

Please sign in to comment.