diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp index 24dbcb7f69c..73379fb1e87 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp @@ -29,6 +29,7 @@ struct Trajectories Eigen::ArrayXXf x; Eigen::ArrayXXf y; Eigen::ArrayXXf yaws; + Eigen::ArrayXf costs; /** * @brief Reset state data diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index c10684b0c79..3659efcb6bb 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -89,13 +89,15 @@ class MotionModel 0).select(state.vx.col(i - 1) + max_delta_vx, state.vx.col(i - 1) - min_delta_vx); - state.vx.col(i) = state.cvx.col(i - 1) + state.cvx.col(i - 1) = state.cvx.col(i - 1) .cwiseMax(lower_bound_vx) .cwiseMin(upper_bound_vx); + state.vx.col(i) = state.cvx.col(i - 1); - state.wz.col(i) = state.cwz.col(i - 1) + state.cwz.col(i - 1) = state.cwz.col(i - 1) .cwiseMax(state.wz.col(i - 1) - max_delta_wz) .cwiseMin(state.wz.col(i - 1) + max_delta_wz); + state.wz.col(i) = state.cwz.col(i - 1); if (is_holo) { auto lower_bound_vy = (state.vy.col(i - 1) > @@ -105,9 +107,10 @@ class MotionModel 0).select(state.vy.col(i - 1) + max_delta_vy, state.vy.col(i - 1) - min_delta_vy); - state.vy.col(i) = state.cvy.col(i - 1) + state.cvy.col(i - 1) = state.cvy.col(i - 1) .cwiseMax(lower_bound_vy) .cwiseMin(upper_bound_vy); + state.vy.col(i) = state.cvy.col(i - 1); } } } @@ -161,14 +164,10 @@ class AckermannMotionModel : public MotionModel */ void applyConstraints(models::ControlSequence & control_sequence) override { - const auto vx_ptr = control_sequence.vx.data(); - auto wz_ptr = control_sequence.wz.data(); - int steps = control_sequence.vx.size(); - for(int i = 0; i < steps; i++) { - float wz_constrained = fabs(*(vx_ptr + i) / min_turning_r_); - float & wz_curr = *(wz_ptr + i); - wz_curr = utils::clamp(-1 * wz_constrained, wz_constrained, wz_curr); - } + const auto wz_constrained = control_sequence.vx.abs() / min_turning_r_; + control_sequence.wz = control_sequence.wz + .max((-wz_constrained)) + .min(wz_constrained); } /** diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index e70cfcab786..17d74c974c8 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -190,13 +190,13 @@ class Optimizer * @brief Update velocities in state * @param state fill state with velocities on each step */ - void updateStateVelocities(models::State & state) const; + void updateStateVelocities(models::State & state); /** * @brief Update initial velocity in state * @param state fill state */ - void updateInitialStateVelocities(models::State & state) const; + void updateInitialStateVelocities(models::State & state); /** * @brief predict velocities in state using model @@ -280,6 +280,7 @@ class Optimizer models::Path path_; geometry_msgs::msg::Pose goal_; Eigen::ArrayXf costs_; + Eigen::Array3f initial_velocities_; CriticData critics_data_ = { state_, generated_trajectories_, path_, goal_, diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index bce8bdc7fca..882216b5e9e 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -82,8 +82,12 @@ void ConstraintCritic::score(CriticData & data) if (acker != nullptr) { auto & vx = data.state.vx; auto & wz = data.state.wz; - float min_turning_rad = acker->getMinTurningRadius(); - auto out_of_turning_rad_motion = (min_turning_rad - (vx.abs() / wz.abs())).max(0.0f); + const float min_turning_rad = acker->getMinTurningRadius(); + + const float epsilon = 1e-6f; + auto wz_safe = wz.abs().max(epsilon); // Replace small wz values to avoid division by 0 + auto out_of_turning_rad_motion = (min_turning_rad - (vx.abs() / wz_safe)).max(0.0f); + if (power_ > 1u) { data.costs += ((((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) + out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 97a16249814..a50a2b8cbd9 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -210,7 +210,6 @@ std::tuple Optimizer::evalCon } } while (fallback(critics_data_.fail_flag || !trajectory_valid)); - utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); auto control = getControlFromSequenceAsTwist(plan.header.stamp); if (settings_.shift_control_sequence) { @@ -226,6 +225,7 @@ void Optimizer::optimize() generateNoisedTrajectories(); critic_manager_.evalTrajectoriesScores(critics_data_); updateControlSequence(); + generated_trajectories_.costs = costs_; } } @@ -258,7 +258,7 @@ void Optimizer::prepare( state_.pose = robot_pose; state_.speed = robot_speed; path_ = utils::toTensor(plan); - costs_.setZero(); + costs_.setZero(settings_.batch_size); goal_ = goal; critics_data_.fail_flag = false; @@ -299,17 +299,17 @@ void Optimizer::applyControlSequenceConstraints() float max_delta_vy = s.model_dt * s.constraints.ay_max; float min_delta_vy = s.model_dt * s.constraints.ay_min; float max_delta_wz = s.model_dt * s.constraints.az_max; - float vx_last = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, control_sequence_.vx(0)); - float wz_last = utils::clamp(-s.constraints.wz, s.constraints.wz, control_sequence_.wz(0)); - control_sequence_.vx(0) = vx_last; - control_sequence_.wz(0) = wz_last; + + // limit acceleration between current initial_velocities_ and first control in the sequence + float vx_last = initial_velocities_(0); + float wz_last = initial_velocities_(2); + float vy_last = 0; if (isHolonomic()) { - vy_last = utils::clamp(-s.constraints.vy, s.constraints.vy, control_sequence_.vy(0)); - control_sequence_.vy(0) = vy_last; + vy_last = initial_velocities_(1); } - for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) { + for (unsigned int i = 0; i != control_sequence_.vx.size(); i++) { float & vx_curr = control_sequence_.vx(i); vx_curr = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, vx_curr); if(vx_last > 0) { @@ -340,21 +340,25 @@ void Optimizer::applyControlSequenceConstraints() } void Optimizer::updateStateVelocities( - models::State & state) const + models::State & state) { updateInitialStateVelocities(state); propagateStateVelocitiesFromInitials(state); } void Optimizer::updateInitialStateVelocities( - models::State & state) const + models::State & state) { - state.vx.col(0) = static_cast(state.speed.linear.x); - state.wz.col(0) = static_cast(state.speed.angular.z); + state.vx.col(0) = control_sequence_.vx(0); + state.wz.col(0) = control_sequence_.wz(0); if (isHolonomic()) { - state.vy.col(0) = static_cast(state.speed.linear.y); + state.vy.col(0) = control_sequence_.vy(0); } + + initial_velocities_(0) = control_sequence_.vx(0); + initial_velocities_(1) = control_sequence_.vy(0); + initial_velocities_(2) = control_sequence_.wz(0); } void Optimizer::propagateStateVelocitiesFromInitials( @@ -514,19 +518,19 @@ void Optimizer::updateControlSequence() control_sequence_.vy = state_.cvy.transpose().matrix() * softmax_mat; } + utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); + applyControlSequenceConstraints(); } geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist( const builtin_interfaces::msg::Time & stamp) { - unsigned int offset = settings_.shift_control_sequence ? 1 : 0; - - auto vx = control_sequence_.vx(offset); - auto wz = control_sequence_.wz(offset); + auto vx = control_sequence_.vx(0); + auto wz = control_sequence_.wz(0); if (isHolonomic()) { - auto vy = control_sequence_.vy(offset); + auto vy = control_sequence_.vy(0); return utils::toTwistStamped(vx, vy, wz, stamp, costmap_ros_->getBaseFrameID()); } diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 6960b5676c6..796c36f93a6 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -111,6 +111,23 @@ void TrajectoryVisualizer::add( const float shape_1 = static_cast(n_cols); points_->markers.reserve(floor(n_rows / trajectory_step_) * floor(n_cols * time_step_)); + std::vector> sorted_costs; + sorted_costs.reserve(std::ceil(n_rows / trajectory_step_)); + + for (size_t i = 0; i < n_rows; i += trajectory_step_) { + sorted_costs.emplace_back(i, trajectories.costs(i)); + } + std::sort(sorted_costs.begin(), sorted_costs.end(), + [](const auto & a, const auto & b) { return a.second < b.second; }); + + const float step = 1.0f / static_cast(sorted_costs.size()); + float color_component = 1.0f; + std::map cost_color_map; + for (const auto & pair : sorted_costs) { + cost_color_map[pair.first] = color_component; + color_component -= step; + } + for (size_t i = 0; i < n_rows; i += trajectory_step_) { for (size_t j = 0; j < n_cols; j += time_step_) { const float j_flt = static_cast(j); @@ -119,7 +136,7 @@ void TrajectoryVisualizer::add( auto pose = utils::createPose(trajectories.x(i, j), trajectories.y(i, j), 0.03); auto scale = utils::createScale(0.03, 0.03, 0.03); - auto color = utils::createColor(0, green_component, blue_component, 1); + auto color = utils::createColor(cost_color_map[i], green_component, blue_component, 1); auto marker = utils::createMarker( marker_id_++, pose, scale, color, frame_id_, marker_namespace);