From 137f75db538eab0e48cf490ec6330d9b0c3b3dc4 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Thu, 6 Jun 2024 23:47:49 +0530 Subject: [PATCH 01/21] Initial commit Signed-off-by: Ayush1285 --- .../models/control_sequence.hpp | 19 +++---- .../nav2_mppi_controller/models/path.hpp | 19 +++---- .../nav2_mppi_controller/models/state.hpp | 32 +++++------ .../models/trajectories.hpp | 20 +++---- .../nav2_mppi_controller/motion_models.hpp | 14 +---- .../nav2_mppi_controller/optimizer.hpp | 18 ++---- .../tools/noise_generator.hpp | 14 ++--- .../nav2_mppi_controller/tools/utils.hpp | 55 ++++++++----------- 8 files changed, 71 insertions(+), 120 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp index ad797e9afa..779106dcec 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp @@ -15,12 +15,7 @@ #ifndef NAV2_MPPI_CONTROLLER__MODELS__CONTROL_SEQUENCE_HPP_ #define NAV2_MPPI_CONTROLLER__MODELS__CONTROL_SEQUENCE_HPP_ -// xtensor creates warnings that needs to be ignored as we are building with -Werror -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Warray-bounds" -#pragma GCC diagnostic ignored "-Wstringop-overflow" -#include -#pragma GCC diagnostic pop +#include namespace mppi::models { @@ -40,15 +35,15 @@ struct Control */ struct ControlSequence { - xt::xtensor vx; - xt::xtensor vy; - xt::xtensor wz; + Eigen::VectorXf vx; + Eigen::VectorXf vy; + Eigen::VectorXf wz; void reset(unsigned int time_steps) { - vx = xt::zeros({time_steps}); - vy = xt::zeros({time_steps}); - wz = xt::zeros({time_steps}); + vx = Eigen::VectorXf::Zero(time_steps); + vy = Eigen::VectorXf::Zero(time_steps); + wz = Eigen::VectorXf::Zero(time_steps); } }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp index f1bab3f803..c80dae91a9 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp @@ -15,12 +15,7 @@ #ifndef NAV2_MPPI_CONTROLLER__MODELS__PATH_HPP_ #define NAV2_MPPI_CONTROLLER__MODELS__PATH_HPP_ -// xtensor creates warnings that needs to be ignored as we are building with -Werror -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Warray-bounds" -#pragma GCC diagnostic ignored "-Wstringop-overflow" -#include -#pragma GCC diagnostic pop +#include namespace mppi::models { @@ -31,18 +26,18 @@ namespace mppi::models */ struct Path { - xt::xtensor x; - xt::xtensor y; - xt::xtensor yaws; + Eigen::VectorXf x; + Eigen::VectorXf y; + Eigen::VectorXf yaws; /** * @brief Reset path data */ void reset(unsigned int size) { - x = xt::zeros({size}); - y = xt::zeros({size}); - yaws = xt::zeros({size}); + x = Eigen::VectorXf::Zero(size); + y = Eigen::VectorXf::Zero(size); + yaws = Eigen::VectorXf::Zero(size); } }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp index 75f8c7521a..ff7f88c749 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp @@ -15,16 +15,12 @@ #ifndef NAV2_MPPI_CONTROLLER__MODELS__STATE_HPP_ #define NAV2_MPPI_CONTROLLER__MODELS__STATE_HPP_ -// xtensor creates warnings that needs to be ignored as we are building with -Werror -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Warray-bounds" -#pragma GCC diagnostic ignored "-Wstringop-overflow" -#include -#pragma GCC diagnostic pop +#include #include #include + namespace mppi::models { @@ -34,13 +30,13 @@ namespace mppi::models */ struct State { - xt::xtensor vx; - xt::xtensor vy; - xt::xtensor wz; + Eigen::MatrixXf vx; + Eigen::MatrixXf vy; + Eigen::MatrixXf wz; - xt::xtensor cvx; - xt::xtensor cvy; - xt::xtensor cwz; + Eigen::MatrixXf cvx; + Eigen::MatrixXf cvy; + Eigen::MatrixXf cwz; geometry_msgs::msg::PoseStamped pose; geometry_msgs::msg::Twist speed; @@ -50,13 +46,13 @@ struct State */ void reset(unsigned int batch_size, unsigned int time_steps) { - vx = xt::zeros({batch_size, time_steps}); - vy = xt::zeros({batch_size, time_steps}); - wz = xt::zeros({batch_size, time_steps}); + vx = Eigen::MatrixXf::Zero(batch_size, time_steps); + vy = Eigen::MatrixXf::Zero(batch_size, time_steps); + wz = Eigen::MatrixXf::Zero(batch_size, time_steps); - cvx = xt::zeros({batch_size, time_steps}); - cvy = xt::zeros({batch_size, time_steps}); - cwz = xt::zeros({batch_size, time_steps}); + cvx = Eigen::MatrixXf::Zero(batch_size, time_steps); + cvy = Eigen::MatrixXf::Zero(batch_size, time_steps); + cwz = Eigen::MatrixXf::Zero(batch_size, time_steps); } }; } // namespace mppi::models 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 0862c2caed..71f450079e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp @@ -15,13 +15,7 @@ #ifndef NAV2_MPPI_CONTROLLER__MODELS__TRAJECTORIES_HPP_ #define NAV2_MPPI_CONTROLLER__MODELS__TRAJECTORIES_HPP_ -// xtensor creates warnings that needs to be ignored as we are building with -Werror -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Warray-bounds" -#pragma GCC diagnostic ignored "-Wstringop-overflow" -#include -#include -#pragma GCC diagnostic pop +#include namespace mppi::models { @@ -32,18 +26,18 @@ namespace mppi::models */ struct Trajectories { - xt::xtensor x; - xt::xtensor y; - xt::xtensor yaws; + Eigen::MatrixXf x; + Eigen::MatrixXf y; + Eigen::MatrixXf yaws; /** * @brief Reset state data */ void reset(unsigned int batch_size, unsigned int time_steps) { - x = xt::zeros({batch_size, time_steps}); - y = xt::zeros({batch_size, time_steps}); - yaws = xt::zeros({batch_size, time_steps}); + x = Eigen::MatrixXf::Zero(batch_size, time_steps); + y = Eigen::MatrixXf::Zero(batch_size, time_steps); + yaws = Eigen::MatrixXf::Zero(batch_size, time_steps); } }; 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 86d24996f8..6ea5e2a572 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -22,15 +22,7 @@ #include "nav2_mppi_controller/models/state.hpp" #include "nav2_mppi_controller/models/constraints.hpp" -// xtensor creates warnings that needs to be ignored as we are building with -Werror -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Warray-bounds" -#pragma GCC diagnostic ignored "-Wstringop-overflow" -#include -#include -#include -#include -#pragma GCC diagnostic pop +#include #include "nav2_mppi_controller/tools/parameters_handler.hpp" @@ -87,11 +79,11 @@ class MotionModel float min_delta_vx = model_dt_ * control_constraints_.ax_min; float max_delta_vy = model_dt_ * control_constraints_.ay_max; float max_delta_wz = model_dt_ * control_constraints_.az_max; - for (unsigned int i = 0; i != state.vx.shape(0); i++) { + for (unsigned int i = 0; i != state.vx.rows(); i++) { float vx_last = state.vx(i, 0); float vy_last = state.vy(i, 0); float wz_last = state.wz(i, 0); - for (unsigned int j = 1; j != state.vx.shape(1); j++) { + for (unsigned int j = 1; j != state.vx.cols(); j++) { float & cvx_curr = state.cvx(i, j - 1); cvx_curr = std::clamp(cvx_curr, vx_last + min_delta_vx, vx_last + max_delta_vx); state.vx(i, j) = cvx_curr; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 6e1c271d94..1c51e6156e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -18,13 +18,7 @@ #include #include -// xtensor creates warnings that needs to be ignored as we are building with -Werror -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Warray-bounds" -#pragma GCC diagnostic ignored "-Wstringop-overflow" -#include -#include -#pragma GCC diagnostic pop +#include #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -56,7 +50,7 @@ namespace mppi */ class Optimizer { -public: +public: /** * @brief Constructor for mppi::Optimizer */ @@ -108,7 +102,7 @@ class Optimizer * @brief Get the optimal trajectory for a cycle for visualization * @return Optimal trajectory */ - xt::xtensor getOptimizedTrajectory(); + Eigen::MatrixXf getOptimizedTrajectory(); /** * @brief Set the maximum speed based on the speed limits callback @@ -202,8 +196,8 @@ class Optimizer * @param state fill state */ void integrateStateVelocities( - xt::xtensor & trajectories, - const xt::xtensor & state) const; + Eigen::MatrixXf & trajectories, + const Eigen::MatrixXf & state) const; /** * @brief Update control sequence with state controls weighted by costs @@ -256,7 +250,7 @@ class Optimizer std::array control_history_; models::Trajectories generated_trajectories_; models::Path path_; - xt::xtensor costs_; + Eigen::VectorXf costs_; CriticData critics_data_ = {state_, generated_trajectories_, path_, costs_, settings_.model_dt, false, nullptr, nullptr, diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp index f16bb90953..844605173f 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp @@ -21,13 +21,7 @@ #include #include -// xtensor creates warnings that needs to be ignored as we are building with -Werror -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Warray-bounds" -#pragma GCC diagnostic ignored "-Wstringop-overflow" -#include -#include -#pragma GCC diagnostic pop +#include #include "nav2_mppi_controller/models/optimizer_settings.hpp" #include "nav2_mppi_controller/tools/parameters_handler.hpp" @@ -99,9 +93,9 @@ class NoiseGenerator */ void generateNoisedControls(); - xt::xtensor noises_vx_; - xt::xtensor noises_vy_; - xt::xtensor noises_wz_; + Eigen::MatrixXf noises_vx_; + Eigen::MatrixXf noises_vy_; + Eigen::MatrixXf noises_wz_; mppi::models::OptimizerSettings settings_; bool is_holonomic_; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index ddde607765..3072655700 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -23,16 +23,7 @@ #include #include -// xtensor creates warnings that needs to be ignored as we are building with -Werror -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Warray-bounds" -#pragma GCC diagnostic ignored "-Wstringop-overflow" -#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" -#include -#include -#include -#include -#pragma GCC diagnostic pop +#include #include "angles/angles.h" @@ -207,12 +198,12 @@ inline models::Path toTensor(const nav_msgs::msg::Path & path) * @param path Path to retreive goal pose from * @return bool If robot is within goal checker tolerances to the goal */ -inline bool withinPositionGoalTolerance( +/*inline bool withinPositionGoalTolerance( nav2_core::GoalChecker * goal_checker, const geometry_msgs::msg::Pose & robot, const models::Path & path) { - const auto goal_idx = path.x.shape(0) - 1; + const auto goal_idx = path.x.size() - 1; const auto goal_x = path.x(goal_idx); const auto goal_y = path.y(goal_idx); @@ -234,7 +225,7 @@ inline bool withinPositionGoalTolerance( } return false; -} +}*/ /** * @brief Check if the robot pose is within tolerance to the goal @@ -243,12 +234,12 @@ inline bool withinPositionGoalTolerance( * @param path Path to retreive goal pose from * @return bool If robot is within tolerance to the goal */ -inline bool withinPositionGoalTolerance( +/*inline bool withinPositionGoalTolerance( float pose_tolerance, const geometry_msgs::msg::Pose & robot, const models::Path & path) { - const auto goal_idx = path.x.shape(0) - 1; + const auto goal_idx = path.x.size() - 1; const float goal_x = path.x(goal_idx); const float goal_y = path.y(goal_idx); @@ -264,7 +255,7 @@ inline bool withinPositionGoalTolerance( } return false; -} +}*/ /** * @brief normalize @@ -273,12 +264,12 @@ inline bool withinPositionGoalTolerance( * @param angles Angles to normalize * @return normalized angles */ -template +/*template auto normalize_angles(const T & angles) { auto theta = xt::eval(xt::fmod(angles + M_PIF, 2.0f * M_PIF)); return xt::eval(xt::where(theta < 0.0f, theta + M_PIF, theta - M_PIF)); -} +}*/ /** * @brief shortest_angular_distance @@ -293,13 +284,13 @@ auto normalize_angles(const T & angles) * @param to End angle * @return Shortest distance between angles */ -template +/*template auto shortest_angular_distance( const F & from, const T & to) { return normalize_angles(to - from); -} +}*/ /** * @brief Evaluate furthest point idx of data.path which is @@ -307,7 +298,7 @@ auto shortest_angular_distance( * @param data Data to use * @return Idx of furthest path point reached by a set of trajectories */ -inline size_t findPathFurthestReachedPoint(const CriticData & data) +/*inline size_t findPathFurthestReachedPoint(const CriticData & data) { const auto traj_x = xt::view(data.trajectories.x, xt::all(), -1, xt::newaxis()); const auto traj_y = xt::view(data.trajectories.y, xt::all(), -1, xt::newaxis()); @@ -333,24 +324,24 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) max_id_by_trajectories = std::max(max_id_by_trajectories, min_id_by_path); } return max_id_by_trajectories; -} +}*/ /** * @brief evaluate path furthest point if it is not set * @param data Data to use */ -inline void setPathFurthestPointIfNotSet(CriticData & data) +/*inline void setPathFurthestPointIfNotSet(CriticData & data) { if (!data.furthest_reached_path_point) { data.furthest_reached_path_point = findPathFurthestReachedPoint(data); } -} +}*/ /** * @brief evaluate path costs * @param data Data to use */ -inline void findPathCosts( +/*inline void findPathCosts( CriticData & data, std::shared_ptr costmap_ros) { @@ -379,20 +370,20 @@ inline void findPathCosts( (*data.path_pts_valid)[idx] = true; } -} +}*/ /** * @brief evaluate path costs if it is not set * @param data Data to use */ -inline void setPathCostsIfNotSet( +/*inline void setPathCostsIfNotSet( CriticData & data, std::shared_ptr costmap_ros) { if (!data.path_pts_valid) { findPathCosts(data, costmap_ros); } -} +}*/ /** * @brief evaluate angle from pose (have angle) to point (no angle) @@ -402,7 +393,7 @@ inline void setPathCostsIfNotSet( * @param forward_preference If reversing direction is valid * @return Angle between two points */ -inline float posePointAngle( +/*inline float posePointAngle( const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) { float pose_x = pose.position.x; @@ -419,7 +410,7 @@ inline float posePointAngle( } return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); -} +}*/ /** * @brief evaluate angle from pose (have angle) to point (no angle) @@ -429,7 +420,7 @@ inline float posePointAngle( * @param point_yaw Yaw of the point to consider along Z axis * @return Angle between two points */ -inline float posePointAngle( +/*inline float posePointAngle( const geometry_msgs::msg::Pose & pose, double point_x, double point_y, double point_yaw) { @@ -444,7 +435,7 @@ inline float posePointAngle( } return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); -} +}*/ /** * @brief Apply Savisky-Golay filter to optimal trajectory From a2c11e62b07acf3f6e1669ae25f5baa11e841403 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Fri, 7 Jun 2024 01:58:43 +0530 Subject: [PATCH 02/21] Corrected to Eigen Array Signed-off-by: Ayush1285 --- .../models/control_sequence.hpp | 12 +++++----- .../nav2_mppi_controller/models/path.hpp | 12 +++++----- .../nav2_mppi_controller/models/state.hpp | 24 +++++++++---------- .../models/trajectories.hpp | 12 +++++----- .../nav2_mppi_controller/motion_models.hpp | 5 ++-- .../nav2_mppi_controller/optimizer.hpp | 8 +++---- .../tools/noise_generator.hpp | 6 ++--- .../nav2_mppi_controller/tools/utils.hpp | 7 +++--- 8 files changed, 43 insertions(+), 43 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp index 779106dcec..cb34f5b6c2 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp @@ -35,15 +35,15 @@ struct Control */ struct ControlSequence { - Eigen::VectorXf vx; - Eigen::VectorXf vy; - Eigen::VectorXf wz; + Eigen::ArrayXf vx; + Eigen::ArrayXf vy; + Eigen::ArrayXf wz; void reset(unsigned int time_steps) { - vx = Eigen::VectorXf::Zero(time_steps); - vy = Eigen::VectorXf::Zero(time_steps); - wz = Eigen::VectorXf::Zero(time_steps); + vx = Eigen::ArrayXf::Zero(time_steps); + vy = Eigen::ArrayXf::Zero(time_steps); + wz = Eigen::ArrayXf::Zero(time_steps); } }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp index c80dae91a9..27645a50cf 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp @@ -26,18 +26,18 @@ namespace mppi::models */ struct Path { - Eigen::VectorXf x; - Eigen::VectorXf y; - Eigen::VectorXf yaws; + Eigen::ArrayXf x; + Eigen::ArrayXf y; + Eigen::ArrayXf yaws; /** * @brief Reset path data */ void reset(unsigned int size) { - x = Eigen::VectorXf::Zero(size); - y = Eigen::VectorXf::Zero(size); - yaws = Eigen::VectorXf::Zero(size); + x = Eigen::ArrayXf::Zero(size); + y = Eigen::ArrayXf::Zero(size); + yaws = Eigen::ArrayXf::Zero(size); } }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp index ff7f88c749..6a680d814f 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp @@ -30,13 +30,13 @@ namespace mppi::models */ struct State { - Eigen::MatrixXf vx; - Eigen::MatrixXf vy; - Eigen::MatrixXf wz; + Eigen::ArrayXXf vx; + Eigen::ArrayXXf vy; + Eigen::ArrayXXf wz; - Eigen::MatrixXf cvx; - Eigen::MatrixXf cvy; - Eigen::MatrixXf cwz; + Eigen::ArrayXXf cvx; + Eigen::ArrayXXf cvy; + Eigen::ArrayXXf cwz; geometry_msgs::msg::PoseStamped pose; geometry_msgs::msg::Twist speed; @@ -46,13 +46,13 @@ struct State */ void reset(unsigned int batch_size, unsigned int time_steps) { - vx = Eigen::MatrixXf::Zero(batch_size, time_steps); - vy = Eigen::MatrixXf::Zero(batch_size, time_steps); - wz = Eigen::MatrixXf::Zero(batch_size, time_steps); + vx = Eigen::ArrayXXf::Zero(batch_size, time_steps); + vy = Eigen::ArrayXXf::Zero(batch_size, time_steps); + wz = Eigen::ArrayXXf::Zero(batch_size, time_steps); - cvx = Eigen::MatrixXf::Zero(batch_size, time_steps); - cvy = Eigen::MatrixXf::Zero(batch_size, time_steps); - cwz = Eigen::MatrixXf::Zero(batch_size, time_steps); + cvx = Eigen::ArrayXXf::Zero(batch_size, time_steps); + cvy = Eigen::ArrayXXf::Zero(batch_size, time_steps); + cwz = Eigen::ArrayXXf::Zero(batch_size, time_steps); } }; } // namespace mppi::models 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 71f450079e..70c155ba5d 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp @@ -26,18 +26,18 @@ namespace mppi::models */ struct Trajectories { - Eigen::MatrixXf x; - Eigen::MatrixXf y; - Eigen::MatrixXf yaws; + Eigen::ArrayXXf x; + Eigen::ArrayXXf y; + Eigen::ArrayXXf yaws; /** * @brief Reset state data */ void reset(unsigned int batch_size, unsigned int time_steps) { - x = Eigen::MatrixXf::Zero(batch_size, time_steps); - y = Eigen::MatrixXf::Zero(batch_size, time_steps); - yaws = Eigen::MatrixXf::Zero(batch_size, time_steps); + x = Eigen::ArrayXXf::Zero(batch_size, time_steps); + y = Eigen::ArrayXXf::Zero(batch_size, time_steps); + yaws = Eigen::ArrayXXf::Zero(batch_size, time_steps); } }; 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 6ea5e2a572..6dc4dbf88b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -156,8 +156,9 @@ class AckermannMotionModel : public MotionModel auto & vx = control_sequence.vx; auto & wz = control_sequence.wz; - auto view = xt::masked_view(wz, xt::fabs(vx) / xt::fabs(wz) < min_turning_r_); - view = xt::sign(wz) * vx / min_turning_r_; + wz.noalias() = (vx.abs() / wz.abs() < min_turning_r_).select(wz, wz.sign() * vx / min_turning_r_); + //auto view = xt::masked_view(wz, vx.abs() / wz.abs() < min_turning_r_); + //view = xt::sign(wz) * vx / min_turning_r_; } /** diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 1c51e6156e..98e4e87986 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -102,7 +102,7 @@ class Optimizer * @brief Get the optimal trajectory for a cycle for visualization * @return Optimal trajectory */ - Eigen::MatrixXf getOptimizedTrajectory(); + Eigen::ArrayXXf getOptimizedTrajectory(); /** * @brief Set the maximum speed based on the speed limits callback @@ -196,8 +196,8 @@ class Optimizer * @param state fill state */ void integrateStateVelocities( - Eigen::MatrixXf & trajectories, - const Eigen::MatrixXf & state) const; + Eigen::ArrayXXf & trajectories, + const Eigen::ArrayXXf & state) const; /** * @brief Update control sequence with state controls weighted by costs @@ -250,7 +250,7 @@ class Optimizer std::array control_history_; models::Trajectories generated_trajectories_; models::Path path_; - Eigen::VectorXf costs_; + Eigen::ArrayXf costs_; CriticData critics_data_ = {state_, generated_trajectories_, path_, costs_, settings_.model_dt, false, nullptr, nullptr, diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp index 844605173f..c593a0c79a 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp @@ -93,9 +93,9 @@ class NoiseGenerator */ void generateNoisedControls(); - Eigen::MatrixXf noises_vx_; - Eigen::MatrixXf noises_vy_; - Eigen::MatrixXf noises_wz_; + Eigen::ArrayXXf noises_vx_; + Eigen::ArrayXXf noises_vy_; + Eigen::ArrayXXf noises_wz_; mppi::models::OptimizerSettings settings_; bool is_holonomic_; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 3072655700..c4fa851d47 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -51,7 +51,6 @@ namespace mppi::utils { -using xt::evaluation_strategy::immediate; /** * @brief Convert data into pose @@ -449,7 +448,7 @@ inline void savitskyGolayFilter( const models::OptimizerSettings & settings) { // Savitzky-Golay Quadratic, 9-point Coefficients - xt::xarray filter = {-21.0, 14.0, 39.0, 54.0, 59.0, 54.0, 39.0, 14.0, -21.0}; + Eigen::Array filter = {-21.0, 14.0, 39.0, 54.0, 59.0, 54.0, 39.0, 14.0, -21.0}; filter /= 231.0; const unsigned int num_sequences = control_sequence.vx.shape(0) - 1; @@ -459,12 +458,12 @@ inline void savitskyGolayFilter( return; } - auto applyFilter = [&](const xt::xarray & data) -> float { + auto applyFilter = [&](const Eigen::Array & data) -> float { return xt::sum(data * filter, {0}, immediate)(); }; auto applyFilterOverAxis = - [&](xt::xtensor & sequence, + [&](Eigen::ArrayXf & sequence, const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void { unsigned int idx = 0; From 8df67576696d6895815c5ff9eb6fd634b00b0a17 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Sun, 9 Jun 2024 00:00:27 +0530 Subject: [PATCH 03/21] updated motion model with eigen Signed-off-by: Ayush1285 --- .../include/nav2_mppi_controller/motion_models.hpp | 4 +--- nav2_mppi_controller/src/noise_generator.cpp | 6 +++--- 2 files changed, 4 insertions(+), 6 deletions(-) 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 6dc4dbf88b..70e3539a84 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -156,9 +156,7 @@ class AckermannMotionModel : public MotionModel auto & vx = control_sequence.vx; auto & wz = control_sequence.wz; - wz.noalias() = (vx.abs() / wz.abs() < min_turning_r_).select(wz, wz.sign() * vx / min_turning_r_); - //auto view = xt::masked_view(wz, vx.abs() / wz.abs() < min_turning_r_); - //view = xt::sign(wz) * vx / min_turning_r_; + wz = (vx.abs() / wz.abs() < min_turning_r_).select(wz, wz.sign() * vx / min_turning_r_); } /** diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index b7c452aa6a..12a5f72da7 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -68,9 +68,9 @@ void NoiseGenerator::setNoisedControls( { std::unique_lock guard(noise_lock_); - xt::noalias(state.cvx) = control_sequence.vx + noises_vx_; - xt::noalias(state.cvy) = control_sequence.vy + noises_vy_; - xt::noalias(state.cwz) = control_sequence.wz + noises_wz_; + state.cvx = noises_vx_.rowwise() + control_sequence.vx.transpose(); + state.cvy = noises_vy_.rowwise() + control_sequence.vy.transpose(); + state.cwz = noises_wz_.rowwise() + control_sequence.wz.transpose(); } void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_holonomic) From 0f3a0f8eeb49d4a887f802510112fa4262b8bc09 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Tue, 11 Jun 2024 01:20:00 +0530 Subject: [PATCH 04/21] Replaced xtensor with eigen in Optimizer, NoiseGenerator and all Util files Signed-off-by: Ayush1285 --- nav2_mppi_controller/CMakeLists.txt | 26 +--- nav2_mppi_controller/benchmark/CMakeLists.txt | 2 +- .../benchmark/controller_benchmark.cpp | 24 ++- .../benchmark/optimizer_benchmark.cpp | 32 ++-- .../nav2_mppi_controller/controller.hpp | 4 +- .../nav2_mppi_controller/critic_data.hpp | 9 +- .../nav2_mppi_controller/critic_manager.hpp | 7 +- .../tools/noise_generator.hpp | 7 + .../nav2_mppi_controller/tools/utils.hpp | 9 +- nav2_mppi_controller/src/controller.cpp | 20 +-- nav2_mppi_controller/src/noise_generator.cpp | 24 +-- nav2_mppi_controller/src/optimizer.cpp | 141 +++++++++--------- 12 files changed, 136 insertions(+), 169 deletions(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 95eabf6c41..da5342ea27 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -14,9 +14,11 @@ set(XTENSOR_USE_XSIMD 1) find_package(ament_cmake REQUIRED) find_package(xsimd REQUIRED) find_package(xtensor REQUIRED) +find_package(Eigen3 REQUIRED) include_directories( include + ${EIGEN3_INCLUDE_DIR} ) set(dependencies_pkgs @@ -71,27 +73,15 @@ add_library(mppi_controller SHARED src/controller.cpp src/optimizer.cpp src/critic_manager.cpp - src/trajectory_visualizer.cpp + # src/trajectory_visualizer.cpp src/path_handler.cpp src/parameters_handler.cpp src/noise_generator.cpp ) -add_library(mppi_critics SHARED - src/critics/obstacles_critic.cpp - src/critics/cost_critic.cpp - src/critics/goal_critic.cpp - src/critics/goal_angle_critic.cpp - src/critics/path_align_critic.cpp - src/critics/path_follow_critic.cpp - src/critics/path_angle_critic.cpp - src/critics/prefer_forward_critic.cpp - src/critics/twirling_critic.cpp - src/critics/constraint_critic.cpp - src/critics/velocity_deadband_critic.cpp -) +#add_library(mppi_critics SHARED) -set(libraries mppi_controller mppi_critics) +set(libraries mppi_controller) #mppi_critics foreach(lib IN LISTS libraries) target_compile_options(${lib} PUBLIC -fconcepts) @@ -100,7 +90,7 @@ foreach(lib IN LISTS libraries) ament_target_dependencies(${lib} ${dependencies_pkgs}) endforeach() -install(TARGETS mppi_controller mppi_critics +install(TARGETS mppi_controller ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -115,8 +105,8 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() - add_subdirectory(test) - # add_subdirectory(benchmark) + # add_subdirectory(test) + add_subdirectory(benchmark) endif() ament_export_libraries(${libraries}) diff --git a/nav2_mppi_controller/benchmark/CMakeLists.txt b/nav2_mppi_controller/benchmark/CMakeLists.txt index 97db9f4185..cbce8e1cf0 100644 --- a/nav2_mppi_controller/benchmark/CMakeLists.txt +++ b/nav2_mppi_controller/benchmark/CMakeLists.txt @@ -13,7 +13,7 @@ foreach(name IN LISTS BENCHMARK_NAMES) ${dependencies_pkgs} ) target_link_libraries(${name} - mppi_controller mppi_critics benchmark + mppi_controller benchmark ) target_include_directories(${name} PRIVATE diff --git a/nav2_mppi_controller/benchmark/controller_benchmark.cpp b/nav2_mppi_controller/benchmark/controller_benchmark.cpp index 9a2cabb4dc..5306845f3c 100644 --- a/nav2_mppi_controller/benchmark/controller_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/controller_benchmark.cpp @@ -24,9 +24,7 @@ #include #include -#include -#include -#include +#include #include "nav2_mppi_controller/motion_models.hpp" #include "nav2_mppi_controller/controller.hpp" @@ -122,8 +120,7 @@ static void BM_DiffDrivePointFootprint(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "DiffDrive"; - std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, - {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; + std::vector critics = {}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } @@ -132,8 +129,7 @@ static void BM_DiffDrive(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "DiffDrive"; - std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, - {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; + std::vector critics = {}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } @@ -143,8 +139,7 @@ static void BM_Omni(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "Omni"; - std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, - {"TwirlingCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; + std::vector critics = {}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } @@ -153,13 +148,12 @@ static void BM_Ackermann(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "Ackermann"; - std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, - {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; + std::vector critics = {}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } -static void BM_GoalCritic(benchmark::State & state) +/*static void BM_GoalCritic(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "Ackermann"; @@ -220,19 +214,19 @@ static void BM_PathAngleCritic(benchmark::State & state) std::vector critics = {{"PathAngleCritic"}}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); -} +}*/ BENCHMARK(BM_DiffDrivePointFootprint)->Unit(benchmark::kMillisecond); BENCHMARK(BM_DiffDrive)->Unit(benchmark::kMillisecond); BENCHMARK(BM_Omni)->Unit(benchmark::kMillisecond); BENCHMARK(BM_Ackermann)->Unit(benchmark::kMillisecond); -BENCHMARK(BM_GoalCritic)->Unit(benchmark::kMillisecond); +/*BENCHMARK(BM_GoalCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_GoalAngleCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_PathAngleCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_PathFollowCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_ObstaclesCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_ObstaclesCriticPointFootprint)->Unit(benchmark::kMillisecond); -BENCHMARK(BM_TwilringCritic)->Unit(benchmark::kMillisecond); +BENCHMARK(BM_TwilringCritic)->Unit(benchmark::kMillisecond);*/ BENCHMARK_MAIN(); diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index a7216bc80d..f9fd1ffd13 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -25,9 +25,7 @@ #include #include -#include -#include -#include +#include #include "nav2_mppi_controller/optimizer.hpp" #include "nav2_mppi_controller/motion_models.hpp" @@ -49,11 +47,11 @@ void prepareAndRunBenchmark( bool consider_footprint, std::string motion_model, std::vector critics, benchmark::State & state) { - int batch_size = 300; - int time_steps = 12; - unsigned int path_points = 50u; + int batch_size = 2000; + int time_steps = 56; + unsigned int path_points = 150u; int iteration_count = 2; - double lookahead_distance = 10.0; + double lookahead_distance = 40.0; TestCostmapSettings costmap_settings{}; auto costmap_ros = getDummyCostmapRos(costmap_settings); @@ -97,8 +95,7 @@ static void BM_DiffDrivePointFootprint(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "DiffDrive"; - std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, - {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; + std::vector critics = {}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } @@ -107,8 +104,7 @@ static void BM_DiffDrive(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "DiffDrive"; - std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, - {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; + std::vector critics = {}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } @@ -118,8 +114,7 @@ static void BM_Omni(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "Omni"; - std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, - {"TwirlingCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; + std::vector critics = {}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } @@ -128,13 +123,12 @@ static void BM_Ackermann(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "Ackermann"; - std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, - {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; + std::vector critics = {}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } -static void BM_GoalCritic(benchmark::State & state) +/*static void BM_GoalCritic(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "Ackermann"; @@ -195,19 +189,19 @@ static void BM_PathAngleCritic(benchmark::State & state) std::vector critics = {{"PathAngleCritic"}}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); -} +}*/ BENCHMARK(BM_DiffDrivePointFootprint)->Unit(benchmark::kMillisecond); BENCHMARK(BM_DiffDrive)->Unit(benchmark::kMillisecond); BENCHMARK(BM_Omni)->Unit(benchmark::kMillisecond); BENCHMARK(BM_Ackermann)->Unit(benchmark::kMillisecond); -BENCHMARK(BM_GoalCritic)->Unit(benchmark::kMillisecond); +/*BENCHMARK(BM_GoalCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_GoalAngleCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_PathAngleCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_PathFollowCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_ObstaclesCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_ObstaclesCriticPointFootprint)->Unit(benchmark::kMillisecond); -BENCHMARK(BM_TwilringCritic)->Unit(benchmark::kMillisecond); +BENCHMARK(BM_TwilringCritic)->Unit(benchmark::kMillisecond);*/ BENCHMARK_MAIN(); diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index 82b850bc46..893342e733 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -20,7 +20,7 @@ #include "nav2_mppi_controller/tools/path_handler.hpp" #include "nav2_mppi_controller/optimizer.hpp" -#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp" +//#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp" #include "nav2_mppi_controller/models/constraints.hpp" #include "nav2_mppi_controller/tools/utils.hpp" @@ -117,7 +117,7 @@ class MPPIController : public nav2_core::Controller std::unique_ptr parameters_handler_; Optimizer optimizer_; PathHandler path_handler_; - TrajectoryVisualizer trajectory_visualizer_; + //TrajectoryVisualizer trajectory_visualizer_; bool visualize_; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp index 05adb41791..209cec4e2b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp @@ -18,12 +18,7 @@ #include #include -// xtensor creates warnings that needs to be ignored as we are building with -Werror -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Warray-bounds" -#pragma GCC diagnostic ignored "-Wstringop-overflow" -#include -#pragma GCC diagnostic pop +#include #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/goal_checker.hpp" @@ -47,7 +42,7 @@ struct CriticData const models::Trajectories & trajectories; const models::Path & path; - xt::xtensor & costs; + Eigen::ArrayXf & costs; float & model_dt; bool fail_flag; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp index 7e09fcf91c..0f227d5dfd 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp @@ -20,12 +20,7 @@ #include #include -// xtensor creates warnings that needs to be ignored as we are building with -Werror -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Warray-bounds" -#pragma GCC diagnostic ignored "-Wstringop-overflow" -#include -#pragma GCC diagnostic pop + #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp index c593a0c79a..1902777d1a 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include @@ -78,6 +79,10 @@ class NoiseGenerator */ void reset(mppi::models::OptimizerSettings & settings, bool is_holonomic); + inline static std::normal_distribution ndistribution_vx; + inline static std::normal_distribution ndistribution_wz; + inline static std::normal_distribution ndistribution_vy; + protected: /** * @brief Thread to execute noise generation process @@ -97,6 +102,8 @@ class NoiseGenerator Eigen::ArrayXXf noises_vy_; Eigen::ArrayXXf noises_wz_; + inline static std::default_random_engine generator_; + mppi::models::OptimizerSettings settings_; bool is_holonomic_; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index c4fa851d47..0cdaf7db42 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -51,7 +51,6 @@ namespace mppi::utils { - /** * @brief Convert data into pose * @param x X position @@ -451,7 +450,7 @@ inline void savitskyGolayFilter( Eigen::Array filter = {-21.0, 14.0, 39.0, 54.0, 59.0, 54.0, 39.0, 14.0, -21.0}; filter /= 231.0; - const unsigned int num_sequences = control_sequence.vx.shape(0) - 1; + const unsigned int num_sequences = control_sequence.vx.size() - 1; // Too short to smooth meaningfully if (num_sequences < 20) { @@ -459,7 +458,7 @@ inline void savitskyGolayFilter( } auto applyFilter = [&](const Eigen::Array & data) -> float { - return xt::sum(data * filter, {0}, immediate)(); + return (data * filter).sum(); }; auto applyFilterOverAxis = @@ -666,7 +665,7 @@ inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) * @return dist Distance to look for * @return init Starting index to indec from */ -inline unsigned int findClosestPathPt( +/*inline unsigned int findClosestPathPt( const std::vector & vec, const float dist, const unsigned int init = 0u) { float distim1 = init != 0u ? vec[init] : 0.0f; // First is 0, no accumulated distance yet @@ -683,7 +682,7 @@ inline unsigned int findClosestPathPt( distim1 = disti; } return size - 1; -} +}*/ // A struct to hold pose data in floating point resolution struct Pose2D diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 6c83131263..dacdca432b 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -41,9 +41,9 @@ void MPPIController::configure( // Configure composed objects optimizer_.initialize(parent_, name_, costmap_ros_, parameters_handler_.get()); path_handler_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get()); - trajectory_visualizer_.on_configure( - parent_, name_, - costmap_ros_->getGlobalFrameID(), parameters_handler_.get()); + //trajectory_visualizer_.on_configure( + //parent_, name_, + //costmap_ros_->getGlobalFrameID(), parameters_handler_.get()); RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str()); } @@ -51,21 +51,21 @@ void MPPIController::configure( void MPPIController::cleanup() { optimizer_.shutdown(); - trajectory_visualizer_.on_cleanup(); + //trajectory_visualizer_.on_cleanup(); parameters_handler_.reset(); RCLCPP_INFO(logger_, "Cleaned up MPPI Controller: %s", name_.c_str()); } void MPPIController::activate() { - trajectory_visualizer_.on_activate(); + //trajectory_visualizer_.on_activate(); parameters_handler_->start(); RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str()); } void MPPIController::deactivate() { - trajectory_visualizer_.on_deactivate(); + //trajectory_visualizer_.on_deactivate(); RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str()); } @@ -105,11 +105,11 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( return cmd; } -void MPPIController::visualize(nav_msgs::msg::Path transformed_plan) +void MPPIController::visualize(nav_msgs::msg::Path /*transformed_plan*/) { - trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories"); - trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory"); - trajectory_visualizer_.visualize(std::move(transformed_plan)); + //trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories"); + //trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory"); + //trajectory_visualizer_.visualize(std::move(transformed_plan)); } void MPPIController::setPlan(const nav_msgs::msg::Path & path) diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index 12a5f72da7..1c3948095f 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -16,9 +16,6 @@ #include #include -#include -#include -#include namespace mppi { @@ -81,9 +78,9 @@ void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_h // Recompute the noises on reset, initialization, and fallback { std::unique_lock guard(noise_lock_); - xt::noalias(noises_vx_) = xt::zeros({settings_.batch_size, settings_.time_steps}); - xt::noalias(noises_vy_) = xt::zeros({settings_.batch_size, settings_.time_steps}); - xt::noalias(noises_wz_) = xt::zeros({settings_.batch_size, settings_.time_steps}); + noises_vx_ = Eigen::ArrayXXf::Zero(settings_.batch_size, settings_.time_steps); + noises_vy_ = Eigen::ArrayXXf::Zero(settings_.batch_size, settings_.time_steps); + noises_wz_ = Eigen::ArrayXXf::Zero(settings_.batch_size, settings_.time_steps); ready_ = true; } @@ -107,17 +104,10 @@ void NoiseGenerator::noiseThread() void NoiseGenerator::generateNoisedControls() { auto & s = settings_; - - xt::noalias(noises_vx_) = xt::random::randn( - {s.batch_size, s.time_steps}, 0.0f, - s.sampling_std.vx); - xt::noalias(noises_wz_) = xt::random::randn( - {s.batch_size, s.time_steps}, 0.0f, - s.sampling_std.wz); - if (is_holonomic_) { - xt::noalias(noises_vy_) = xt::random::randn( - {s.batch_size, s.time_steps}, 0.0f, - s.sampling_std.vy); + noises_vx_ = Eigen::ArrayXXf::NullaryExpr(s.batch_size, s.time_steps, [&] () {return ndistribution_vx(generator_);}); + noises_wz_ = Eigen::ArrayXXf::NullaryExpr(s.batch_size, s.time_steps, [&] () {return ndistribution_wz(generator_);}); + if(is_holonomic_) { + noises_vy_ = Eigen::ArrayXXf::NullaryExpr(s.batch_size, s.time_steps, [&] () {return ndistribution_vy(generator_);}); } } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index c539ca74ec..e1a22e9b58 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -20,9 +20,6 @@ #include #include #include -#include -#include -#include #include "nav2_core/controller_exceptions.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" @@ -30,8 +27,8 @@ namespace mppi { -using namespace xt::placeholders; // NOLINT -using xt::evaluation_strategy::immediate; +//using namespace xt::placeholders; // NOLINT +//using xt::evaluation_strategy::immediate; void Optimizer::initialize( rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name, @@ -97,6 +94,10 @@ void Optimizer::getParams() getParam(motion_model_name, "motion_model", std::string("DiffDrive")); s.constraints = s.base_constraints; + NoiseGenerator::ndistribution_vx = std::normal_distribution(0.0f, s.sampling_std.vx); + NoiseGenerator::ndistribution_vy = std::normal_distribution(0.0f, s.sampling_std.vy); + NoiseGenerator::ndistribution_wz = std::normal_distribution(0.0f, s.sampling_std.wz); + setMotionModel(motion_model_name); parameters_handler_->addPostCallback([this]() {reset();}); @@ -137,7 +138,7 @@ void Optimizer::reset() settings_.constraints = settings_.base_constraints; - costs_ = xt::zeros({settings_.batch_size}); + costs_ = Eigen::ArrayXf::Zero(settings_.batch_size); generated_trajectories_.reset(settings_.batch_size, settings_.time_steps); noise_generator_.reset(settings_, isHolonomic()); @@ -217,22 +218,17 @@ void Optimizer::prepare( void Optimizer::shiftControlSequence() { - using namespace xt::placeholders; // NOLINT - control_sequence_.vx = xt::roll(control_sequence_.vx, -1); - control_sequence_.wz = xt::roll(control_sequence_.wz, -1); - - - xt::view(control_sequence_.vx, -1) = - xt::view(control_sequence_.vx, -2); - - xt::view(control_sequence_.wz, -1) = - xt::view(control_sequence_.wz, -2); + //using namespace xt::placeholders; // NOLINT + Eigen::ArrayXf temp_var(settings_.time_steps); + temp_var.head(settings_.time_steps - 1) = control_sequence_.vx.tail(settings_.time_steps - 1); + control_sequence_.vx.head(settings_.time_steps - 1) = temp_var.head(settings_.time_steps - 1); + temp_var.head(settings_.time_steps - 1) = control_sequence_.wz.tail(settings_.time_steps - 1); + control_sequence_.wz.head(settings_.time_steps - 1) = temp_var.head(settings_.time_steps - 1); if (isHolonomic()) { - control_sequence_.vy = xt::roll(control_sequence_.vy, -1); - xt::view(control_sequence_.vy, -1) = - xt::view(control_sequence_.vy, -2); + temp_var.head(settings_.time_steps - 1) = control_sequence_.vy.tail(settings_.time_steps - 1); + control_sequence_.vy.head(settings_.time_steps - 1) = temp_var.head(settings_.time_steps - 1); } } @@ -247,32 +243,32 @@ void Optimizer::generateNoisedTrajectories() void Optimizer::applyControlSequenceConstraints() { auto & s = settings_; - - if (isHolonomic()) { - control_sequence_.vy = xt::clip(control_sequence_.vy, -s.constraints.vy, s.constraints.vy); - } - - control_sequence_.vx = xt::clip(control_sequence_.vx, s.constraints.vx_min, s.constraints.vx_max); - control_sequence_.wz = xt::clip(control_sequence_.wz, -s.constraints.wz, s.constraints.wz); + float & vx_min = s.constraints.vx_min; + float & vx_max = s.constraints.vx_max; + float & vy = s.constraints.vy; + float & wz = s.constraints.wz; float max_delta_vx = s.model_dt * s.constraints.ax_max; float min_delta_vx = s.model_dt * s.constraints.ax_min; float max_delta_vy = s.model_dt * s.constraints.ay_max; float max_delta_wz = s.model_dt * s.constraints.az_max; - float vx_last = control_sequence_.vx(0); - float vy_last = control_sequence_.vy(0); - float wz_last = control_sequence_.wz(0); - for (unsigned int i = 1; i != control_sequence_.vx.shape(0); i++) { + float vx_last = std::clamp(control_sequence_.vx(0), vx_min, vx_max); + float vy_last = std::clamp(control_sequence_.vy(0), -vy, vy); + float wz_last = std::clamp(control_sequence_.wz(0), -wz, wz); + for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) { float & vx_curr = control_sequence_.vx(i); + vx_curr = std::clamp(vx_curr, vx_min, vx_max); vx_curr = std::clamp(vx_curr, vx_last + min_delta_vx, vx_last + max_delta_vx); vx_last = vx_curr; float & wz_curr = control_sequence_.wz(i); + wz_curr = std::clamp(wz_curr, -wz, wz); wz_curr = std::clamp(wz_curr, wz_last - max_delta_wz, wz_last + max_delta_wz); wz_last = wz_curr; if (isHolonomic()) { float & vy_curr = control_sequence_.vy(i); + vy_curr = std::clamp(vy_curr, -vy, vy); vy_curr = std::clamp(vy_curr, vy_last - max_delta_vy, vy_last + max_delta_vy); vy_last = vy_curr; } @@ -291,11 +287,11 @@ void Optimizer::updateStateVelocities( void Optimizer::updateInitialStateVelocities( models::State & state) const { - xt::noalias(xt::view(state.vx, xt::all(), 0)) = static_cast(state.speed.linear.x); - xt::noalias(xt::view(state.wz, xt::all(), 0)) = static_cast(state.speed.angular.z); + state.vx.col(0) += static_cast(state.speed.linear.x); + state.wz.col(0) += static_cast(state.speed.angular.z); if (isHolonomic()) { - xt::noalias(xt::view(state.vy, xt::all(), 0)) = static_cast(state.speed.linear.y); + state.vy.col(0) += static_cast(state.speed.linear.y); } } @@ -305,7 +301,7 @@ void Optimizer::propagateStateVelocitiesFromInitials( motion_model_->predict(state); } -void Optimizer::integrateStateVelocities( +/*void Optimizer::integrateStateVelocities( xt::xtensor & trajectory, const xt::xtensor & sequence) const { @@ -336,7 +332,7 @@ void Optimizer::integrateStateVelocities( xt::noalias(traj_x) = state_.pose.pose.position.x + xt::cumsum(dx * settings_.model_dt, 0); xt::noalias(traj_y) = state_.pose.pose.position.y + xt::cumsum(dy * settings_.model_dt, 0); -} +}*/ void Optimizer::integrateStateVelocities( models::Trajectories & trajectories, @@ -344,29 +340,45 @@ void Optimizer::integrateStateVelocities( { const float initial_yaw = static_cast(tf2::getYaw(state.pose.pose.orientation)); - xt::noalias(trajectories.yaws) = - xt::cumsum(state.wz * settings_.model_dt, {1}) + initial_yaw; + trajectories.yaws.col(0) = state.wz.col(0) * settings_.model_dt + initial_yaw; + for(unsigned int i = 1; i != state.wz.cols(); i++) + { + trajectories.yaws.col(i) = trajectories.yaws.col(i - 1) + state.wz.col(i) * settings_.model_dt; + } - auto yaw_cos = xt::roll(xt::eval(xt::cos(trajectories.yaws)), 1, 1); - auto yaw_sin = xt::roll(xt::eval(xt::sin(trajectories.yaws)), 1, 1); - xt::view(yaw_cos, xt::all(), 0) = cosf(initial_yaw); - xt::view(yaw_sin, xt::all(), 0) = sinf(initial_yaw); + auto yaw_cos = trajectories.yaws.cos().eval(); + auto yaw_sin = trajectories.yaws.sin().eval(); + { + Eigen::ArrayXXf temp_var(settings_.batch_size, settings_.time_steps); + temp_var.rightCols(settings_.time_steps - 1) = yaw_cos.leftCols(settings_.time_steps - 1); + yaw_cos.rightCols(settings_.time_steps - 1) = temp_var.rightCols(settings_.time_steps - 1); + yaw_cos.col(0) = cosf(initial_yaw); + + temp_var.rightCols(settings_.time_steps - 1) = yaw_sin.leftCols(settings_.time_steps - 1); + yaw_sin.rightCols(settings_.time_steps - 1) = temp_var.rightCols(settings_.time_steps - 1); + yaw_sin.col(0) = sinf(initial_yaw); + } - auto && dx = xt::eval(state.vx * yaw_cos); - auto && dy = xt::eval(state.vx * yaw_sin); + auto dx = (state.vx * yaw_cos).eval(); + auto dy = (state.vx * yaw_sin).eval(); if (isHolonomic()) { - dx = dx - state.vy * yaw_sin; - dy = dy + state.vy * yaw_cos; + auto && ddx = (state.vy * yaw_sin).eval(); + auto && ddy = (state.vy * yaw_cos).eval(); + dx = dx - ddx; + dy = dy + ddy; } - xt::noalias(trajectories.x) = state.pose.pose.position.x + - xt::cumsum(dx * settings_.model_dt, {1}); - xt::noalias(trajectories.y) = state.pose.pose.position.y + - xt::cumsum(dy * settings_.model_dt, {1}); + trajectories.x.col(0) = dx.col(0) * settings_.model_dt + state.pose.pose.position.x; + trajectories.y.col(0) = dy.col(0) * settings_.model_dt + state.pose.pose.position.y; + for(unsigned int i = 1; i != dx.cols(); i++) + { + trajectories.x.col(i) = trajectories.x.col(i - 1) + dx.col(i) * settings_.model_dt; + trajectories.y.col(i) = trajectories.y.col(i - 1) + dy.col(i) * settings_.model_dt; + } } -xt::xtensor Optimizer::getOptimizedTrajectory() +/*xt::xtensor Optimizer::getOptimizedTrajectory() { const bool is_holo = isHolonomic(); auto && sequence = @@ -382,7 +394,7 @@ xt::xtensor Optimizer::getOptimizedTrajectory() integrateStateVelocities(trajectories, sequence); return std::move(trajectories); -} +}*/ void Optimizer::updateControlSequence() { @@ -390,30 +402,21 @@ void Optimizer::updateControlSequence() auto & s = settings_; auto bounded_noises_vx = state_.cvx - control_sequence_.vx; auto bounded_noises_wz = state_.cwz - control_sequence_.wz; - xt::noalias(costs_) += - s.gamma / powf(s.sampling_std.vx, 2) * xt::sum( - xt::view(control_sequence_.vx, xt::newaxis(), xt::all()) * bounded_noises_vx, 1, immediate); - xt::noalias(costs_) += - s.gamma / powf(s.sampling_std.wz, 2) * xt::sum( - xt::view(control_sequence_.wz, xt::newaxis(), xt::all()) * bounded_noises_wz, 1, immediate); - + costs_ += s.gamma / powf(s.sampling_std.vx, 2) * (bounded_noises_vx.rowwise() * control_sequence_.vx.transpose()).rowwise().sum().eval(); + costs_ += s.gamma / powf(s.sampling_std.wz, 2) * (bounded_noises_wz.rowwise() * control_sequence_.wz.transpose()).rowwise().sum().eval(); if (is_holo) { auto bounded_noises_vy = state_.cvy - control_sequence_.vy; - xt::noalias(costs_) += - s.gamma / powf(s.sampling_std.vy, 2) * xt::sum( - xt::view(control_sequence_.vy, xt::newaxis(), xt::all()) * bounded_noises_vy, - 1, immediate); + costs_ += s.gamma / powf(s.sampling_std.vy, 2) * (bounded_noises_vy.rowwise() * control_sequence_.vy.transpose()).rowwise().sum().eval(); } - auto && costs_normalized = costs_ - xt::amin(costs_, immediate); - auto && exponents = xt::eval(xt::exp(-1 / settings_.temperature * costs_normalized)); - auto && softmaxes = xt::eval(exponents / xt::sum(exponents, immediate)); - auto && softmaxes_extened = xt::eval(xt::view(softmaxes, xt::all(), xt::newaxis())); + auto && costs_normalized = costs_ - costs_.minCoeff(); + auto && exponents = (-1 / settings_.temperature * costs_normalized).exp(); + auto && softmaxes = (exponents / exponents.sum()).eval(); - xt::noalias(control_sequence_.vx) = xt::sum(state_.cvx * softmaxes_extened, 0, immediate); - xt::noalias(control_sequence_.wz) = xt::sum(state_.cwz * softmaxes_extened, 0, immediate); + control_sequence_.vx = (state_.cvx.colwise() * softmaxes).colwise().sum(); + control_sequence_.wz = (state_.cwz.colwise() * softmaxes).colwise().sum(); if (is_holo) { - xt::noalias(control_sequence_.vy) = xt::sum(state_.cvy * softmaxes_extened, 0, immediate); + control_sequence_.vy = (state_.cvy.colwise() * softmaxes).colwise().sum(); } applyControlSequenceConstraints(); From 3e0bcd4d5f321a5efcb23f5af75a668c0972c2b1 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Wed, 19 Jun 2024 01:22:46 +0530 Subject: [PATCH 05/21] updated critics with Eigen Signed-off-by: Ayush1285 --- nav2_mppi_controller/CMakeLists.txt | 16 ++++- .../benchmark/optimizer_benchmark.cpp | 17 +++--- .../nav2_mppi_controller/motion_models.hpp | 9 +-- .../nav2_mppi_controller/optimizer.hpp | 2 +- .../nav2_mppi_controller/tools/utils.hpp | 60 +++++++++---------- .../src/critics/goal_angle_critic.cpp | 9 +-- .../src/critics/goal_critic.cpp | 16 ++--- .../src/critics/obstacles_critic.cpp | 22 +++---- .../src/critics/path_angle_critic.cpp | 47 +++++++-------- .../src/critics/path_follow_critic.cpp | 15 +++-- .../src/critics/prefer_forward_critic.cpp | 10 +--- .../src/critics/twirling_critic.cpp | 5 +- nav2_mppi_controller/src/optimizer.cpp | 30 +++++----- 13 files changed, 125 insertions(+), 133 deletions(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index da5342ea27..d3b4a09043 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -79,9 +79,21 @@ add_library(mppi_controller SHARED src/noise_generator.cpp ) -#add_library(mppi_critics SHARED) +add_library(mppi_critics SHARED + src/critics/obstacles_critic.cpp + #src/critics/cost_critic.cpp + src/critics/goal_critic.cpp + src/critics/goal_angle_critic.cpp + #src/critics/path_align_critic.cpp + src/critics/path_follow_critic.cpp + src/critics/path_angle_critic.cpp + src/critics/prefer_forward_critic.cpp + src/critics/twirling_critic.cpp + #src/critics/constraint_critic.cpp + #src/critics/velocity_deadband_critic.cpp +) -set(libraries mppi_controller) #mppi_critics +set(libraries mppi_controller mppi_critics) # foreach(lib IN LISTS libraries) target_compile_options(${lib} PUBLIC -fconcepts) diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index f9fd1ffd13..8f270c13d5 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -48,7 +48,7 @@ void prepareAndRunBenchmark( std::vector critics, benchmark::State & state) { int batch_size = 2000; - int time_steps = 56; + int time_steps = 70; unsigned int path_points = 150u; int iteration_count = 2; double lookahead_distance = 40.0; @@ -95,7 +95,8 @@ static void BM_DiffDrivePointFootprint(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "DiffDrive"; - std::vector critics = {}; + std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } @@ -104,26 +105,28 @@ static void BM_DiffDrive(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "DiffDrive"; - std::vector critics = {}; + std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } -static void BM_Omni(benchmark::State & state) +/*static void BM_Omni(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "Omni"; std::vector critics = {}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); -} +}*/ static void BM_Ackermann(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "Ackermann"; - std::vector critics = {}; + std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } @@ -193,7 +196,7 @@ static void BM_PathAngleCritic(benchmark::State & state) BENCHMARK(BM_DiffDrivePointFootprint)->Unit(benchmark::kMillisecond); BENCHMARK(BM_DiffDrive)->Unit(benchmark::kMillisecond); -BENCHMARK(BM_Omni)->Unit(benchmark::kMillisecond); +//BENCHMARK(BM_Omni)->Unit(benchmark::kMillisecond); BENCHMARK(BM_Ackermann)->Unit(benchmark::kMillisecond); /*BENCHMARK(BM_GoalCritic)->Unit(benchmark::kMillisecond); 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 70e3539a84..b405f495d7 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -75,10 +75,11 @@ class MotionModel // } const bool is_holo = isHolonomic(); - float max_delta_vx = model_dt_ * control_constraints_.ax_max; - float min_delta_vx = model_dt_ * control_constraints_.ax_min; - float max_delta_vy = model_dt_ * control_constraints_.ay_max; - float max_delta_wz = model_dt_ * control_constraints_.az_max; + float && max_delta_vx = model_dt_ * control_constraints_.ax_max; + float && min_delta_vx = model_dt_ * control_constraints_.ax_min; + float && max_delta_vy = model_dt_ * control_constraints_.ay_max; + float && max_delta_wz = model_dt_ * control_constraints_.az_max; + for (unsigned int i = 0; i != state.vx.rows(); i++) { float vx_last = state.vx(i, 0); float vy_last = state.vy(i, 0); diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 98e4e87986..c7a594afb9 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -188,7 +188,7 @@ class Optimizer */ void integrateStateVelocities( models::Trajectories & trajectories, - const models::State & state) const; + models::State & state) const; /** * @brief Rollout velocities in state to poses diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 0cdaf7db42..ec7b20c2d6 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -196,7 +196,7 @@ inline models::Path toTensor(const nav_msgs::msg::Path & path) * @param path Path to retreive goal pose from * @return bool If robot is within goal checker tolerances to the goal */ -/*inline bool withinPositionGoalTolerance( +inline bool withinPositionGoalTolerance( nav2_core::GoalChecker * goal_checker, const geometry_msgs::msg::Pose & robot, const models::Path & path) @@ -223,7 +223,7 @@ inline models::Path toTensor(const nav_msgs::msg::Path & path) } return false; -}*/ +} /** * @brief Check if the robot pose is within tolerance to the goal @@ -232,7 +232,7 @@ inline models::Path toTensor(const nav_msgs::msg::Path & path) * @param path Path to retreive goal pose from * @return bool If robot is within tolerance to the goal */ -/*inline bool withinPositionGoalTolerance( +inline bool withinPositionGoalTolerance( float pose_tolerance, const geometry_msgs::msg::Pose & robot, const models::Path & path) @@ -253,7 +253,7 @@ inline models::Path toTensor(const nav_msgs::msg::Path & path) } return false; -}*/ +} /** * @brief normalize @@ -262,12 +262,12 @@ inline models::Path toTensor(const nav_msgs::msg::Path & path) * @param angles Angles to normalize * @return normalized angles */ -/*template +template auto normalize_angles(const T & angles) { - auto theta = xt::eval(xt::fmod(angles + M_PIF, 2.0f * M_PIF)); - return xt::eval(xt::where(theta < 0.0f, theta + M_PIF, theta - M_PIF)); -}*/ + auto theta = ((angles + M_PIF).unaryExpr([](const float x){ return std::fmod(x, 2.0f * M_PIF);})).eval(); + return (theta < 0.0f).select(theta + M_PIF, theta - M_PIF).eval(); +} /** * @brief shortest_angular_distance @@ -282,13 +282,13 @@ auto normalize_angles(const T & angles) * @param to End angle * @return Shortest distance between angles */ -/*template +template auto shortest_angular_distance( const F & from, const T & to) { return normalize_angles(to - from); -}*/ +} /** * @brief Evaluate furthest point idx of data.path which is @@ -296,23 +296,23 @@ auto shortest_angular_distance( * @param data Data to use * @return Idx of furthest path point reached by a set of trajectories */ -/*inline size_t findPathFurthestReachedPoint(const CriticData & data) +inline size_t findPathFurthestReachedPoint(const CriticData & data) { - const auto traj_x = xt::view(data.trajectories.x, xt::all(), -1, xt::newaxis()); - const auto traj_y = xt::view(data.trajectories.y, xt::all(), -1, xt::newaxis()); + const auto traj_x = data.trajectories.x.col(-1); + const auto traj_y = data.trajectories.y.col(-1); - const auto dx = data.path.x - traj_x; - const auto dy = data.path.y - traj_y; + const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1) - traj_x; + const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1) - traj_y; const auto dists = dx * dx + dy * dy; - size_t max_id_by_trajectories = 0, min_id_by_path = 0; + int max_id_by_trajectories = 0, min_id_by_path = 0; float min_distance_by_path = std::numeric_limits::max(); - for (size_t i = 0; i < dists.shape(0); i++) { + for (int i = 0; i < dists.rows(); i++) { min_id_by_path = 0; min_distance_by_path = std::numeric_limits::max(); - for (size_t j = max_id_by_trajectories; j < dists.shape(1); j++) { + for (int j = max_id_by_trajectories; j < dists.cols(); j++) { const float cur_dist = dists(i, j); if (cur_dist < min_distance_by_path) { min_distance_by_path = cur_dist; @@ -322,30 +322,30 @@ auto shortest_angular_distance( max_id_by_trajectories = std::max(max_id_by_trajectories, min_id_by_path); } return max_id_by_trajectories; -}*/ +} /** * @brief evaluate path furthest point if it is not set * @param data Data to use */ -/*inline void setPathFurthestPointIfNotSet(CriticData & data) +inline void setPathFurthestPointIfNotSet(CriticData & data) { if (!data.furthest_reached_path_point) { data.furthest_reached_path_point = findPathFurthestReachedPoint(data); } -}*/ +} /** * @brief evaluate path costs * @param data Data to use */ -/*inline void findPathCosts( +inline void findPathCosts( CriticData & data, std::shared_ptr costmap_ros) { auto * costmap = costmap_ros->getCostmap(); unsigned int map_x, map_y; - const size_t path_segments_count = data.path.x.shape(0) - 1; + const size_t path_segments_count = data.path.x.size() - 1; data.path_pts_valid = std::vector(path_segments_count, false); const bool tracking_unknown = costmap_ros->getLayeredCostmap()->isTrackingUnknown(); for (unsigned int idx = 0; idx < path_segments_count; idx++) { @@ -368,20 +368,20 @@ auto shortest_angular_distance( (*data.path_pts_valid)[idx] = true; } -}*/ +} /** * @brief evaluate path costs if it is not set * @param data Data to use */ -/*inline void setPathCostsIfNotSet( +inline void setPathCostsIfNotSet( CriticData & data, std::shared_ptr costmap_ros) { if (!data.path_pts_valid) { findPathCosts(data, costmap_ros); } -}*/ +} /** * @brief evaluate angle from pose (have angle) to point (no angle) @@ -391,7 +391,7 @@ auto shortest_angular_distance( * @param forward_preference If reversing direction is valid * @return Angle between two points */ -/*inline float posePointAngle( +inline float posePointAngle( const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) { float pose_x = pose.position.x; @@ -408,7 +408,7 @@ auto shortest_angular_distance( } return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); -}*/ +} /** * @brief evaluate angle from pose (have angle) to point (no angle) @@ -418,7 +418,7 @@ auto shortest_angular_distance( * @param point_yaw Yaw of the point to consider along Z axis * @return Angle between two points */ -/*inline float posePointAngle( +inline float posePointAngle( const geometry_msgs::msg::Pose & pose, double point_x, double point_y, double point_yaw) { @@ -433,7 +433,7 @@ auto shortest_angular_distance( } return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); -}*/ +} /** * @brief Apply Savisky-Golay filter to optimal trajectory diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 99ad3950de..377af949b3 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -41,16 +41,13 @@ void GoalAngleCritic::score(CriticData & data) return; } - const auto goal_idx = data.path.x.shape(0) - 1; + const auto goal_idx = data.path.x.size() - 1; const float goal_yaw = data.path.yaws(goal_idx); if (power_ > 1u) { - data.costs += xt::pow( - xt::mean(xt::fabs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) * - weight_, power_); + data.costs += Eigen::pow(((Eigen::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw))).rowwise().mean()) * weight_, power_); } else { - data.costs += xt::mean( - xt::fabs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) * weight_; + data.costs += ((Eigen::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw))).rowwise().mean()) * weight_; } } diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 4dbaf063f3..5a9366536f 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -18,7 +18,6 @@ namespace mppi::critics { -using xt::evaluation_strategy::immediate; void GoalCritic::initialize() { @@ -41,23 +40,18 @@ void GoalCritic::score(CriticData & data) return; } - const auto goal_idx = data.path.x.shape(0) - 1; + const auto goal_idx = data.path.x.size() - 1; const auto goal_x = data.path.x(goal_idx); const auto goal_y = data.path.y(goal_idx); - const auto traj_x = xt::view(data.trajectories.x, xt::all(), xt::all()); - const auto traj_y = xt::view(data.trajectories.y, xt::all(), xt::all()); + const auto delta_x = data.trajectories.x - goal_x; + const auto delta_y = data.trajectories.y - goal_y; if (power_ > 1u) { - data.costs += xt::pow( - xt::mean( - xt::hypot(traj_x - goal_x, traj_y - goal_y), - {1}, immediate) * weight_, power_); + data.costs += Eigen::pow((((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean()) * weight_, power_); } else { - data.costs += xt::mean( - xt::hypot(traj_x - goal_x, traj_y - goal_y), - {1}, immediate) * weight_; + data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean()) * weight_; } } diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index e3e49918d7..42d15b92b1 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -104,7 +104,6 @@ float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost) void ObstaclesCritic::score(CriticData & data) { - using xt::evaluation_strategy::immediate; if (!enabled_) { return; } @@ -120,12 +119,12 @@ void ObstaclesCritic::score(CriticData & data) near_goal = true; } - auto && raw_cost = xt::xtensor::from_shape({data.costs.shape(0)}); - auto && repulsive_cost = xt::xtensor::from_shape({data.costs.shape(0)}); + Eigen::ArrayXf raw_cost = Eigen::ArrayXf::Zero(data.costs.size()); + Eigen::ArrayXf repulsive_cost = Eigen::ArrayXf::Zero(data.costs.size()); - const size_t traj_len = data.trajectories.x.shape(1); + const int traj_len = data.trajectories.x.cols(); bool all_trajectories_collide = true; - for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) { + for (int i = 0; i < data.trajectories.x.rows(); ++i) { bool trajectory_collide = false; float traj_cost = 0.0f; const auto & traj = data.trajectories; @@ -133,7 +132,7 @@ void ObstaclesCritic::score(CriticData & data) raw_cost[i] = 0.0f; repulsive_cost[i] = 0.0f; - for (size_t j = 0; j < traj_len; j++) { + for (int j = 0; j < traj_len; j++) { pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j)); if (pose_cost.cost < 1.0f) {continue;} // In free space @@ -166,17 +165,12 @@ void ObstaclesCritic::score(CriticData & data) // Normalize repulsive cost by trajectory length & lowest score to not overweight importance // This is a preferential cost, not collision cost, to be tuned relative to desired behaviors - auto && repulsive_cost_normalized = - (repulsive_cost - xt::amin(repulsive_cost, immediate)) / traj_len; + auto && repulsive_cost_normalized = (repulsive_cost - repulsive_cost.minCoeff()) / traj_len; if (power_ > 1u) { - data.costs += xt::pow( - (critical_weight_ * raw_cost) + - (repulsion_weight_ * repulsive_cost_normalized), - power_); + data.costs += Eigen::pow((critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized), power_); } else { - data.costs += (critical_weight_ * raw_cost) + - (repulsion_weight_ * repulsive_cost_normalized); + data.costs += (critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized); } data.fail_flag = all_trajectories_collide; diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index eee54a6be3..ea6149cd76 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -71,7 +71,7 @@ void PathAngleCritic::score(CriticData & data) utils::setPathFurthestPointIfNotSet(data); auto offseted_idx = std::min( - *data.furthest_reached_path_point + offset_from_furthest_, data.path.x.shape(0) - 1); + *data.furthest_reached_path_point + offset_from_furthest_, (size_t)data.path.x.size() - 1); const float goal_x = data.path.x(offseted_idx); const float goal_y = data.path.y(offseted_idx); @@ -98,19 +98,18 @@ void PathAngleCritic::score(CriticData & data) throw nav2_core::ControllerException("Invalid path angle mode!"); } - auto yaws_between_points = xt::atan2( - goal_y - xt::view(data.trajectories.y, xt::all(), -1), - goal_x - xt::view(data.trajectories.x, xt::all(), -1)); + int && rightmost_idx = data.trajectories.y.cols() - 1; + auto yaws_between_points = ((goal_y - data.trajectories.y.col(rightmost_idx)).binaryExpr( + (goal_x - data.trajectories.x.col(rightmost_idx)), [&](const float & y, const float & x){return std::atan2(y, x);})).eval(); switch (mode_) { case PathAngleMode::FORWARD_PREFERENCE: { - auto yaws = - xt::fabs( - utils::shortest_angular_distance( - xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points)); + const Eigen::ArrayXf& yaws = + Eigen::abs( + utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points)); if (power_ > 1u) { - data.costs += xt::pow(std::move(yaws) * weight_, power_); + data.costs += Eigen::pow(std::move(yaws) * weight_, power_); } else { data.costs += std::move(yaws) * weight_; } @@ -118,18 +117,14 @@ void PathAngleCritic::score(CriticData & data) } case PathAngleMode::NO_DIRECTIONAL_PREFERENCE: { - auto yaws = - xt::fabs( - utils::shortest_angular_distance( - xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points)); - const auto yaws_between_points_corrected = xt::where( - yaws < M_PIF_2, yaws_between_points, - utils::normalize_angles(yaws_between_points + M_PIF)); - const auto corrected_yaws = xt::fabs( - utils::shortest_angular_distance( - xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points_corrected)); + const Eigen::ArrayXf& yaws = + Eigen::abs( + utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points)); + const Eigen::ArrayXf& yaws_between_points_corrected = ((yaws < M_PIF_2).select(yaws_between_points, utils::normalize_angles((yaws_between_points + M_PIF)))).eval(); + const Eigen::ArrayXf& corrected_yaws = Eigen::abs( + utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points_corrected)); if (power_ > 1u) { - data.costs += xt::pow(std::move(corrected_yaws) * weight_, power_); + data.costs += Eigen::pow(std::move(corrected_yaws) * weight_, power_); } else { data.costs += std::move(corrected_yaws) * weight_; } @@ -137,14 +132,12 @@ void PathAngleCritic::score(CriticData & data) } case PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS: { - const auto yaws_between_points_corrected = xt::where( - xt::fabs(utils::shortest_angular_distance(yaws_between_points, goal_yaw)) < M_PIF_2, - yaws_between_points, utils::normalize_angles(yaws_between_points + M_PIF)); - const auto corrected_yaws = xt::fabs( - utils::shortest_angular_distance( - xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points_corrected)); + const Eigen::ArrayXf& yaws_between_points_corrected = ((Eigen::abs(utils::shortest_angular_distance(yaws_between_points, goal_yaw)) < M_PIF_2).select( + yaws_between_points, utils::normalize_angles(yaws_between_points + M_PIF))).eval(); + const Eigen::ArrayXf& corrected_yaws = Eigen::abs( + utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points_corrected)); if (power_ > 1u) { - data.costs += xt::pow(std::move(corrected_yaws) * weight_, power_); + data.costs += Eigen::pow(std::move(corrected_yaws) * weight_, power_); } else { data.costs += std::move(corrected_yaws) * weight_; } diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index fc8f0ff7f6..fd2b7e1298 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -34,7 +34,7 @@ void PathFollowCritic::initialize() void PathFollowCritic::score(CriticData & data) { - if (!enabled_ || data.path.x.shape(0) < 2 || + if (!enabled_ || data.path.x.size() < 2 || utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) { return; @@ -42,7 +42,7 @@ void PathFollowCritic::score(CriticData & data) utils::setPathFurthestPointIfNotSet(data); utils::setPathCostsIfNotSet(data, costmap_ros_); - const size_t path_size = data.path.x.shape(0) - 1; + const size_t path_size = data.path.x.size() - 1; auto offseted_idx = std::min( *data.furthest_reached_path_point + offset_from_furthest_, path_size); @@ -60,13 +60,16 @@ void PathFollowCritic::score(CriticData & data) const auto path_x = data.path.x(offseted_idx); const auto path_y = data.path.y(offseted_idx); - const auto last_x = xt::view(data.trajectories.x, xt::all(), -1); - const auto last_y = xt::view(data.trajectories.y, xt::all(), -1); + const int && rightmost_idx = data.trajectories.x.cols() - 1; + const auto last_x = data.trajectories.x.col(rightmost_idx); + const auto last_y = data.trajectories.y.col(rightmost_idx); + const auto delta_x = last_x - path_x; + const auto delta_y = last_y - path_y; if (power_ > 1u) { - data.costs += xt::pow(weight_ * std::move(xt::hypot(last_x - path_x, last_y - path_y)), power_); + data.costs += Eigen::pow(weight_ * std::move((delta_x.square() + delta_y.square()).sqrt()), power_); } else { - data.costs += weight_ * std::move(xt::hypot(last_x - path_x, last_y - path_y)); + data.costs += weight_ * std::move((delta_x.square() + delta_y.square()).sqrt()); } } diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 885d1fadc9..0223d58f84 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -32,7 +32,6 @@ void PreferForwardCritic::initialize() void PreferForwardCritic::score(CriticData & data) { - using xt::evaluation_strategy::immediate; if (!enabled_ || utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) { @@ -40,13 +39,10 @@ void PreferForwardCritic::score(CriticData & data) } if (power_ > 1u) { - data.costs += xt::pow( - xt::sum( - std::move( - xt::maximum(-data.state.vx, 0)) * data.model_dt, {1}, immediate) * weight_, power_); + data.costs += Eigen::pow( + (std::move(data.state.vx.unaryExpr([&](const float & x){ return std::max(-x, 0.0f);})) * data.model_dt).rowwise().sum() * weight_, power_); } else { - data.costs += xt::sum( - std::move(xt::maximum(-data.state.vx, 0)) * data.model_dt, {1}, immediate) * weight_; + data.costs += (data.state.vx.unaryExpr([&](const float & x){ return std::max(-x, 0.0f);}) * data.model_dt).rowwise().sum() * weight_; } } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index f4ae970dc6..f4e74b6177 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -30,7 +30,6 @@ void TwirlingCritic::initialize() void TwirlingCritic::score(CriticData & data) { - using xt::evaluation_strategy::immediate; if (!enabled_ || utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.path)) { @@ -38,9 +37,9 @@ void TwirlingCritic::score(CriticData & data) } if (power_ > 1u) { - data.costs += xt::pow(xt::mean(xt::fabs(data.state.wz), {1}, immediate) * weight_, power_); + data.costs += Eigen::pow((Eigen::abs(data.state.wz)).rowwise().mean().eval() * weight_, power_); } else { - data.costs += xt::mean(xt::fabs(data.state.wz), {1}, immediate) * weight_; + data.costs += (Eigen::abs(data.state.wz)).rowwise().mean().eval() * weight_; } } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index e1a22e9b58..45f9be8c13 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -248,10 +248,10 @@ void Optimizer::applyControlSequenceConstraints() float & vy = s.constraints.vy; float & wz = s.constraints.wz; - float max_delta_vx = s.model_dt * s.constraints.ax_max; - float min_delta_vx = s.model_dt * s.constraints.ax_min; - float max_delta_vy = s.model_dt * s.constraints.ay_max; - float max_delta_wz = s.model_dt * s.constraints.az_max; + float && max_delta_vx = s.model_dt * s.constraints.ax_max; + float && min_delta_vx = s.model_dt * s.constraints.ax_min; + float && max_delta_vy = s.model_dt * s.constraints.ay_max; + float && max_delta_wz = s.model_dt * s.constraints.az_max; float vx_last = std::clamp(control_sequence_.vx(0), vx_min, vx_max); float vy_last = std::clamp(control_sequence_.vy(0), -vy, vy); float wz_last = std::clamp(control_sequence_.wz(0), -wz, wz); @@ -336,7 +336,7 @@ void Optimizer::propagateStateVelocitiesFromInitials( void Optimizer::integrateStateVelocities( models::Trajectories & trajectories, - const models::State & state) const + models::State & state) const { const float initial_yaw = static_cast(tf2::getYaw(state.pose.pose.orientation)); @@ -363,10 +363,10 @@ void Optimizer::integrateStateVelocities( auto dy = (state.vx * yaw_sin).eval(); if (isHolonomic()) { - auto && ddx = (state.vy * yaw_sin).eval(); - auto && ddy = (state.vy * yaw_cos).eval(); - dx = dx - ddx; - dy = dy + ddy; + //auto && ddx = (state.vy * yaw_sin).eval(); + //auto && ddy = (state.vy * yaw_cos).eval(); + dx = dx - state.vy * yaw_sin; + dy = dy + state.vy * yaw_cos; } trajectories.x.col(0) = dx.col(0) * settings_.model_dt + state.pose.pose.position.x; @@ -400,13 +400,13 @@ void Optimizer::updateControlSequence() { const bool is_holo = isHolonomic(); auto & s = settings_; - auto bounded_noises_vx = state_.cvx - control_sequence_.vx; - auto bounded_noises_wz = state_.cwz - control_sequence_.wz; - costs_ += s.gamma / powf(s.sampling_std.vx, 2) * (bounded_noises_vx.rowwise() * control_sequence_.vx.transpose()).rowwise().sum().eval(); - costs_ += s.gamma / powf(s.sampling_std.wz, 2) * (bounded_noises_wz.rowwise() * control_sequence_.wz.transpose()).rowwise().sum().eval(); + auto && bounded_noises_vx = state_.cvx.rowwise() - control_sequence_.vx.transpose(); + auto && bounded_noises_wz = state_.cwz.rowwise() - control_sequence_.wz.transpose(); + costs_ += s.gamma / powf(s.sampling_std.vx, 2) * (bounded_noises_vx.rowwise() * control_sequence_.vx.transpose()).rowwise().sum(); + costs_ += s.gamma / powf(s.sampling_std.wz, 2) * (bounded_noises_wz.rowwise() * control_sequence_.wz.transpose()).rowwise().sum(); if (is_holo) { - auto bounded_noises_vy = state_.cvy - control_sequence_.vy; - costs_ += s.gamma / powf(s.sampling_std.vy, 2) * (bounded_noises_vy.rowwise() * control_sequence_.vy.transpose()).rowwise().sum().eval(); + auto && bounded_noises_vy = state_.cvy.rowwise() - control_sequence_.vy.transpose(); + costs_ += s.gamma / powf(s.sampling_std.vy, 2) * (bounded_noises_vy.rowwise() * control_sequence_.vy.transpose()).rowwise().sum(); } auto && costs_normalized = costs_ - costs_.minCoeff(); From f244e0a1ab62cf2cf8647f141aa484b857920c7d Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Tue, 13 Aug 2024 00:58:00 +0530 Subject: [PATCH 06/21] optimized Eigen::Array implementation Signed-off-by: Ayush1285 --- .../models/trajectories.hpp | 6 +- .../nav2_mppi_controller/motion_models.hpp | 56 +++++++++--------- .../nav2_mppi_controller/tools/utils.hpp | 50 ++++++++++++++-- .../src/critics/goal_angle_critic.cpp | 6 +- .../src/critics/goal_critic.cpp | 6 +- .../src/critics/obstacles_critic.cpp | 33 +++++++---- .../src/critics/path_angle_critic.cpp | 25 ++++---- .../src/critics/path_follow_critic.cpp | 7 +-- .../src/critics/prefer_forward_critic.cpp | 4 +- .../src/critics/twirling_critic.cpp | 2 + nav2_mppi_controller/src/optimizer.cpp | 59 ++++++++----------- 11 files changed, 145 insertions(+), 109 deletions(-) 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 70c155ba5d..9b09d093a2 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp @@ -35,9 +35,9 @@ struct Trajectories */ void reset(unsigned int batch_size, unsigned int time_steps) { - x = Eigen::ArrayXXf::Zero(batch_size, time_steps); - y = Eigen::ArrayXXf::Zero(batch_size, time_steps); - yaws = Eigen::ArrayXXf::Zero(batch_size, time_steps); + x = Eigen::ArrayXXf::Zero(batch_size, time_steps);; + y = Eigen::ArrayXXf::Zero(batch_size, time_steps);; + yaws = Eigen::ArrayXXf::Zero(batch_size, time_steps);; } }; 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 b405f495d7..499fea4a15 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -63,43 +63,33 @@ class MotionModel */ virtual void predict(models::State & state) { - // Previously completed via tensor views, but found to be 10x slower - // using namespace xt::placeholders; // NOLINT - // xt::noalias(xt::view(state.vx, xt::all(), xt::range(1, _))) = - // xt::noalias(xt::view(state.cvx, xt::all(), xt::range(0, -1))); - // xt::noalias(xt::view(state.wz, xt::all(), xt::range(1, _))) = - // xt::noalias(xt::view(state.cwz, xt::all(), xt::range(0, -1))); - // if (isHolonomic()) { - // xt::noalias(xt::view(state.vy, xt::all(), xt::range(1, _))) = - // xt::noalias(xt::view(state.cvy, xt::all(), xt::range(0, -1))); - // } - const bool is_holo = isHolonomic(); float && max_delta_vx = model_dt_ * control_constraints_.ax_max; float && min_delta_vx = model_dt_ * control_constraints_.ax_min; float && max_delta_vy = model_dt_ * control_constraints_.ay_max; float && max_delta_wz = model_dt_ * control_constraints_.az_max; - for (unsigned int i = 0; i != state.vx.rows(); i++) { - float vx_last = state.vx(i, 0); - float vy_last = state.vy(i, 0); - float wz_last = state.wz(i, 0); - for (unsigned int j = 1; j != state.vx.cols(); j++) { - float & cvx_curr = state.cvx(i, j - 1); - cvx_curr = std::clamp(cvx_curr, vx_last + min_delta_vx, vx_last + max_delta_vx); - state.vx(i, j) = cvx_curr; - vx_last = cvx_curr; + unsigned int n_rows = state.vx.rows(); + unsigned int n_cols = state.vx.cols(); - float & cwz_curr = state.cwz(i, j - 1); + // Default layout in eigen is column-major, hence accessing elements in column-major fashion to utilize L1 cache as much as possible + for (unsigned int i = 1; i != n_cols; i++) { + for (unsigned int j = 0; j != n_rows; j++) { + float & vx_last = state.vx(j, i - 1); + float & cvx_curr = state.cvx(j, i - 1); + cvx_curr = std::clamp(cvx_curr, vx_last + min_delta_vx, vx_last + max_delta_vx); + state.vx(j, i) = cvx_curr; + + float & wz_last = state.wz(j, i - 1); + float & cwz_curr = state.cwz(j, i - 1); cwz_curr = std::clamp(cwz_curr, wz_last - max_delta_wz, wz_last + max_delta_wz); - state.wz(i, j) = cwz_curr; - wz_last = cwz_curr; + state.wz(j, i) = cwz_curr; if (is_holo) { - float & cvy_curr = state.cvy(i, j - 1); + float & vy_last = state.vy(j, i - 1); + float & cvy_curr = state.cvy(j, i - 1); cvy_curr = std::clamp(cvy_curr, vy_last - max_delta_vy, vy_last + max_delta_vy); - state.vy(i, j) = cvy_curr; - vy_last = cvy_curr; + state.vy(j, i) = cvy_curr; } } } @@ -154,10 +144,18 @@ class AckermannMotionModel : public MotionModel */ void applyConstraints(models::ControlSequence & control_sequence) override { - auto & vx = control_sequence.vx; - auto & wz = control_sequence.wz; + 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 = std::abs(*(vx_ptr + i) / min_turning_r_); + float & wz_curr = *(wz_ptr + i); + wz_curr = std::clamp(wz_curr, -1 * wz_constrained, wz_constrained); + } + // Taking more time compared to for loop + //wz = ((vx.abs() / wz.abs() < min_turning_r_).select(wz, wz.sign() * vx / min_turning_r_)).eval(); - wz = (vx.abs() / wz.abs() < min_turning_r_).select(wz, wz.sign() * vx / min_turning_r_); } /** diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index ec7b20c2d6..577782a7ee 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -265,8 +265,8 @@ inline bool withinPositionGoalTolerance( template auto normalize_angles(const T & angles) { - auto theta = ((angles + M_PIF).unaryExpr([](const float x){ return std::fmod(x, 2.0f * M_PIF);})).eval(); - return (theta < 0.0f).select(theta + M_PIF, theta - M_PIF).eval(); + Eigen::ArrayXXf theta = (angles + M_PIF).unaryExpr([](const float x){ float remainder = std::fmod(x, 2.0f * M_PIF); return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF;}); + return ((theta < 0.0f).select(theta + M_PIF, theta - M_PIF)).eval(); } /** @@ -308,11 +308,12 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) int max_id_by_trajectories = 0, min_id_by_path = 0; float min_distance_by_path = std::numeric_limits::max(); - - for (int i = 0; i < dists.rows(); i++) { + int n_rows = dists.rows(); + int n_cols = dists.cols(); + for (int i = 0; i < n_rows; i++) { min_id_by_path = 0; min_distance_by_path = std::numeric_limits::max(); - for (int j = max_id_by_trajectories; j < dists.cols(); j++) { + for (int j = max_id_by_trajectories; j < n_cols; j++) { const float cur_dist = dists(i, j); if (cur_dist < min_distance_by_path) { min_distance_by_path = cur_dist; @@ -690,6 +691,45 @@ struct Pose2D float x, y, theta; }; +template +auto rollColumns(T&& e, std::ptrdiff_t shift) +{ + shift = shift >= 0 ? shift : e.cols() + shift; + auto flat_size = shift * e.rows(); + Eigen::ArrayXXf cpyMatrix(e.rows(), e.cols()); + std::copy(e.data(), e.data() + flat_size, std::copy(e.data() + flat_size, e.data() + e.size(), cpyMatrix.data())); + return cpyMatrix; +} + +inline auto point_corrected_yaws(const Eigen::ArrayXf & yaws, const Eigen::ArrayXf & yaws_between_points) +{ + const auto yaws_ptr = yaws.data(); + const auto yaws_between_points_ptr = yaws_between_points.data(); + int size = yaws.size(); + Eigen::ArrayXf yaws_between_points_corrected(size); + auto yaws_between_points_corrected_ptr = yaws_between_points_corrected.data(); + for(int i = 0; i != size; i++) + { + const float & yaw_between_points = *(yaws_between_points_ptr + i); + *(yaws_between_points_corrected_ptr + i) = *(yaws_ptr + i) < M_PIF_2 ? yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF); + } + return yaws_between_points_corrected; +} + +inline auto point_corrected_yaws(const Eigen::ArrayXf & yaws_between_points, const float & goal_yaw) +{ + const auto yaws_between_points_ptr = yaws_between_points.data(); + int size = yaws_between_points.size(); + Eigen::ArrayXf yaws_between_points_corrected(size); + auto yaws_between_points_corrected_ptr = yaws_between_points_corrected.data(); + for(int i = 0; i != size; i++) + { + const float & yaw_between_points = *(yaws_between_points_ptr + i); + *(yaws_between_points_corrected_ptr + i) = std::abs(angles::normalize_angle(yaw_between_points - goal_yaw)) < M_PIF_2 ? yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF); + } + return yaws_between_points_corrected; +} + } // namespace mppi::utils #endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 377af949b3..edaa41ada0 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -44,10 +44,10 @@ void GoalAngleCritic::score(CriticData & data) const auto goal_idx = data.path.x.size() - 1; const float goal_yaw = data.path.yaws(goal_idx); - if (power_ > 1u) { - data.costs += Eigen::pow(((Eigen::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw))).rowwise().mean()) * weight_, power_); + if( power_ > 1u) { + data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()).rowwise().mean())*weight_).pow(power_); } else { - data.costs += ((Eigen::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw))).rowwise().mean()) * weight_; + data.costs += ((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()).rowwise().mean())*weight_; } } diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 5a9366536f..cf9e9a1f6b 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -48,10 +48,10 @@ void GoalCritic::score(CriticData & data) const auto delta_x = data.trajectories.x - goal_x; const auto delta_y = data.trajectories.y - goal_y; - if (power_ > 1u) { - data.costs += Eigen::pow((((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean()) * weight_, power_); + if(power_ > 1u) { + data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean() * weight_).pow(power_); } else { - data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean()) * weight_; + data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean() * weight_).eval(); } } diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 42d15b92b1..d998d99f29 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -122,23 +122,30 @@ void ObstaclesCritic::score(CriticData & data) Eigen::ArrayXf raw_cost = Eigen::ArrayXf::Zero(data.costs.size()); Eigen::ArrayXf repulsive_cost = Eigen::ArrayXf::Zero(data.costs.size()); - const int traj_len = data.trajectories.x.cols(); + const unsigned int traj_len = data.trajectories.x.cols(); + const unsigned int batch_size = data.trajectories.x.rows(); bool all_trajectories_collide = true; - for (int i = 0; i < data.trajectories.x.rows(); ++i) { + const auto & traj = data.trajectories; + + // Default layout in eigen is column-major, hence accessing elements in column-major fashion to utilize L1 cache as much as possible + for(unsigned int i = 0; i != traj_len; i++) + { bool trajectory_collide = false; - float traj_cost = 0.0f; - const auto & traj = data.trajectories; CollisionCost pose_cost; - raw_cost[i] = 0.0f; - repulsive_cost[i] = 0.0f; + for(unsigned int j = 0; j != batch_size; j++) + { + if(raw_cost[j] == collision_cost_) + { + continue; + } - for (int j = 0; j < traj_len; j++) { - pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j)); + pose_cost = costAtPose(traj.x(j, i), traj.y(j, i), traj.yaws(j, i)); if (pose_cost.cost < 1.0f) {continue;} // In free space if (inCollision(pose_cost.cost)) { trajectory_collide = true; - break; + raw_cost[j] = collision_cost_; + continue; } // Cannot process repulsion if inflation layer does not exist @@ -149,18 +156,20 @@ void ObstaclesCritic::score(CriticData & data) const float dist_to_obj = distanceToObstacle(pose_cost); // Let near-collision trajectory points be punished severely + float traj_cost = 0.0f; if (dist_to_obj < collision_margin_distance_) { - traj_cost += (collision_margin_distance_ - dist_to_obj); + traj_cost = (collision_margin_distance_ - dist_to_obj); } // Generally prefer trajectories further from obstacles if (!near_goal) { - repulsive_cost[i] += inflation_radius_ - dist_to_obj; + repulsive_cost[j] += inflation_radius_ - dist_to_obj; } + raw_cost[j] += traj_cost; } if (!trajectory_collide) {all_trajectories_collide = false;} - raw_cost[i] = trajectory_collide ? collision_cost_ : traj_cost; + } // Normalize repulsive cost by trajectory length & lowest score to not overweight importance diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index ea6149cd76..6bd7a78a3a 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -20,8 +20,6 @@ namespace mppi::critics { -using xt::evaluation_strategy::immediate; - void PathAngleCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); @@ -99,30 +97,30 @@ void PathAngleCritic::score(CriticData & data) } int && rightmost_idx = data.trajectories.y.cols() - 1; - auto yaws_between_points = ((goal_y - data.trajectories.y.col(rightmost_idx)).binaryExpr( - (goal_x - data.trajectories.x.col(rightmost_idx)), [&](const float & y, const float & x){return std::atan2(y, x);})).eval(); + auto yaws_between_points = (goal_y - data.trajectories.y.col(rightmost_idx)).binaryExpr( + (goal_x - data.trajectories.x.col(rightmost_idx)), [&](const float & y, const float & x){return std::atan2(y, x);}).eval(); switch (mode_) { case PathAngleMode::FORWARD_PREFERENCE: { - const Eigen::ArrayXf& yaws = + Eigen::ArrayXf yaws = Eigen::abs( utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points)); if (power_ > 1u) { data.costs += Eigen::pow(std::move(yaws) * weight_, power_); } else { - data.costs += std::move(yaws) * weight_; + data.costs += (std::move(yaws) * weight_).eval(); } return; } case PathAngleMode::NO_DIRECTIONAL_PREFERENCE: { - const Eigen::ArrayXf& yaws = + Eigen::ArrayXf yaws = Eigen::abs( utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points)); - const Eigen::ArrayXf& yaws_between_points_corrected = ((yaws < M_PIF_2).select(yaws_between_points, utils::normalize_angles((yaws_between_points + M_PIF)))).eval(); - const Eigen::ArrayXf& corrected_yaws = Eigen::abs( - utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points_corrected)); + Eigen::ArrayXf yaws_between_points_corrected = utils::point_corrected_yaws(yaws, yaws_between_points); + Eigen::ArrayXf corrected_yaws = Eigen::abs( + utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), std::move(yaws_between_points_corrected))); if (power_ > 1u) { data.costs += Eigen::pow(std::move(corrected_yaws) * weight_, power_); } else { @@ -132,10 +130,9 @@ void PathAngleCritic::score(CriticData & data) } case PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS: { - const Eigen::ArrayXf& yaws_between_points_corrected = ((Eigen::abs(utils::shortest_angular_distance(yaws_between_points, goal_yaw)) < M_PIF_2).select( - yaws_between_points, utils::normalize_angles(yaws_between_points + M_PIF))).eval(); - const Eigen::ArrayXf& corrected_yaws = Eigen::abs( - utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points_corrected)); + Eigen::ArrayXf yaws_between_points_corrected = utils::point_corrected_yaws(yaws_between_points, goal_yaw); + Eigen::ArrayXf corrected_yaws = Eigen::abs( + utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), std::move(yaws_between_points_corrected))); if (power_ > 1u) { data.costs += Eigen::pow(std::move(corrected_yaws) * weight_, power_); } else { diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index fd2b7e1298..5db60bae88 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -14,8 +14,7 @@ #include "nav2_mppi_controller/critics/path_follow_critic.hpp" -#include -#include +#include namespace mppi::critics { @@ -67,9 +66,9 @@ void PathFollowCritic::score(CriticData & data) const auto delta_x = last_x - path_x; const auto delta_y = last_y - path_y; if (power_ > 1u) { - data.costs += Eigen::pow(weight_ * std::move((delta_x.square() + delta_y.square()).sqrt()), power_); + data.costs += (((delta_x.square() + delta_y.square()).sqrt()) * weight_).pow(power_); } else { - data.costs += weight_ * std::move((delta_x.square() + delta_y.square()).sqrt()); + data.costs += ((delta_x.square() + delta_y.square()).sqrt()) * weight_; } } diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 0223d58f84..fb2157de0e 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -14,6 +14,8 @@ #include "nav2_mppi_controller/critics/prefer_forward_critic.hpp" +#include + namespace mppi::critics { @@ -40,7 +42,7 @@ void PreferForwardCritic::score(CriticData & data) if (power_ > 1u) { data.costs += Eigen::pow( - (std::move(data.state.vx.unaryExpr([&](const float & x){ return std::max(-x, 0.0f);})) * data.model_dt).rowwise().sum() * weight_, power_); + (data.state.vx.unaryExpr([&](const float & x){ return std::max(-x, 0.0f);}) * data.model_dt).rowwise().sum() * weight_, power_); } else { data.costs += (data.state.vx.unaryExpr([&](const float & x){ return std::max(-x, 0.0f);}) * data.model_dt).rowwise().sum() * weight_; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index f4e74b6177..0a897626dd 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -14,6 +14,8 @@ #include "nav2_mppi_controller/critics/twirling_critic.hpp" +#include + namespace mppi::critics { diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 45f9be8c13..2057b55a6a 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include "nav2_core/controller_exceptions.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" @@ -174,6 +175,7 @@ geometry_msgs::msg::TwistStamped Optimizer::evalControl( void Optimizer::optimize() { for (size_t i = 0; i < settings_.iteration_count; ++i) { + generateNoisedTrajectories(); critic_manager_.evalTrajectoriesScores(critics_data_); updateControlSequence(); @@ -219,16 +221,11 @@ void Optimizer::prepare( void Optimizer::shiftControlSequence() { //using namespace xt::placeholders; // NOLINT - Eigen::ArrayXf temp_var(settings_.time_steps); - temp_var.head(settings_.time_steps - 1) = control_sequence_.vx.tail(settings_.time_steps - 1); - control_sequence_.vx.head(settings_.time_steps - 1) = temp_var.head(settings_.time_steps - 1); - - temp_var.head(settings_.time_steps - 1) = control_sequence_.wz.tail(settings_.time_steps - 1); - control_sequence_.wz.head(settings_.time_steps - 1) = temp_var.head(settings_.time_steps - 1); + control_sequence_.vx = utils::rollColumns(control_sequence_.vx, 1); + control_sequence_.wz = utils::rollColumns(control_sequence_.wz, 1); if (isHolonomic()) { - temp_var.head(settings_.time_steps - 1) = control_sequence_.vy.tail(settings_.time_steps - 1); - control_sequence_.vy.head(settings_.time_steps - 1) = temp_var.head(settings_.time_steps - 1); + control_sequence_.vy = utils::rollColumns(control_sequence_.vy, 1); } } @@ -287,11 +284,11 @@ void Optimizer::updateStateVelocities( void Optimizer::updateInitialStateVelocities( models::State & state) const { - state.vx.col(0) += static_cast(state.speed.linear.x); - state.wz.col(0) += static_cast(state.speed.angular.z); + state.vx.col(0) = static_cast(state.speed.linear.x); + state.wz.col(0) = static_cast(state.speed.angular.z); if (isHolonomic()) { - state.vy.col(0) += static_cast(state.speed.linear.y); + state.vy.col(0) = static_cast(state.speed.linear.y); } } @@ -339,42 +336,34 @@ void Optimizer::integrateStateVelocities( models::State & state) const { const float initial_yaw = static_cast(tf2::getYaw(state.pose.pose.orientation)); + const unsigned int n_cols = state.wz.cols(); + const float & dt_time = settings_.model_dt; - trajectories.yaws.col(0) = state.wz.col(0) * settings_.model_dt + initial_yaw; - for(unsigned int i = 1; i != state.wz.cols(); i++) + trajectories.yaws.col(0) = state.wz.col(0) * dt_time + initial_yaw; + for(unsigned int i = 1; i != n_cols; i++) { - trajectories.yaws.col(i) = trajectories.yaws.col(i - 1) + state.wz.col(i) * settings_.model_dt; + trajectories.yaws.col(i) = trajectories.yaws.col(i - 1) + state.wz.col(i) * dt_time; } - auto yaw_cos = trajectories.yaws.cos().eval(); - auto yaw_sin = trajectories.yaws.sin().eval(); - { - Eigen::ArrayXXf temp_var(settings_.batch_size, settings_.time_steps); - temp_var.rightCols(settings_.time_steps - 1) = yaw_cos.leftCols(settings_.time_steps - 1); - yaw_cos.rightCols(settings_.time_steps - 1) = temp_var.rightCols(settings_.time_steps - 1); - yaw_cos.col(0) = cosf(initial_yaw); - - temp_var.rightCols(settings_.time_steps - 1) = yaw_sin.leftCols(settings_.time_steps - 1); - yaw_sin.rightCols(settings_.time_steps - 1) = temp_var.rightCols(settings_.time_steps - 1); - yaw_sin.col(0) = sinf(initial_yaw); - } + auto yaw_cos = (utils::rollColumns(trajectories.yaws, -1).cos()).eval(); + auto yaw_sin = (utils::rollColumns(trajectories.yaws, -1).sin()).eval(); + yaw_cos.col(0) = cosf(initial_yaw); + yaw_sin.col(0) = sinf(initial_yaw); auto dx = (state.vx * yaw_cos).eval(); auto dy = (state.vx * yaw_sin).eval(); if (isHolonomic()) { - //auto && ddx = (state.vy * yaw_sin).eval(); - //auto && ddy = (state.vy * yaw_cos).eval(); dx = dx - state.vy * yaw_sin; dy = dy + state.vy * yaw_cos; } - trajectories.x.col(0) = dx.col(0) * settings_.model_dt + state.pose.pose.position.x; - trajectories.y.col(0) = dy.col(0) * settings_.model_dt + state.pose.pose.position.y; - for(unsigned int i = 1; i != dx.cols(); i++) + trajectories.x.col(0) = dx.col(0) * dt_time + state.pose.pose.position.x; + trajectories.y.col(0) = dy.col(0) * dt_time + state.pose.pose.position.y; + for(unsigned int i = 1; i != n_cols; i++) { - trajectories.x.col(i) = trajectories.x.col(i - 1) + dx.col(i) * settings_.model_dt; - trajectories.y.col(i) = trajectories.y.col(i - 1) + dy.col(i) * settings_.model_dt; + trajectories.x.col(i) = trajectories.x.col(i - 1) + dx.col(i) * dt_time; + trajectories.y.col(i) = trajectories.y.col(i - 1) + dy.col(i) * dt_time; } } @@ -405,12 +394,12 @@ void Optimizer::updateControlSequence() costs_ += s.gamma / powf(s.sampling_std.vx, 2) * (bounded_noises_vx.rowwise() * control_sequence_.vx.transpose()).rowwise().sum(); costs_ += s.gamma / powf(s.sampling_std.wz, 2) * (bounded_noises_wz.rowwise() * control_sequence_.wz.transpose()).rowwise().sum(); if (is_holo) { - auto && bounded_noises_vy = state_.cvy.rowwise() - control_sequence_.vy.transpose(); + auto bounded_noises_vy = state_.cvy.rowwise() - control_sequence_.vy.transpose(); costs_ += s.gamma / powf(s.sampling_std.vy, 2) * (bounded_noises_vy.rowwise() * control_sequence_.vy.transpose()).rowwise().sum(); } auto && costs_normalized = costs_ - costs_.minCoeff(); - auto && exponents = (-1 / settings_.temperature * costs_normalized).exp(); + auto && exponents = ((-1 / settings_.temperature * costs_normalized).exp()).eval(); auto && softmaxes = (exponents / exponents.sum()).eval(); control_sequence_.vx = (state_.cvx.colwise() * softmaxes).colwise().sum(); From 96b14d148e05afb300975811a1ebb4c43f60d2d9 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Thu, 15 Aug 2024 01:40:50 +0530 Subject: [PATCH 07/21] added comment Signed-off-by: Ayush1285 --- .../include/nav2_mppi_controller/motion_models.hpp | 2 +- .../include/nav2_mppi_controller/tools/utils.hpp | 9 +++++++-- nav2_mppi_controller/src/optimizer.cpp | 4 ---- 3 files changed, 8 insertions(+), 7 deletions(-) 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 499fea4a15..17fdc84555 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -153,7 +153,7 @@ class AckermannMotionModel : public MotionModel float & wz_curr = *(wz_ptr + i); wz_curr = std::clamp(wz_curr, -1 * wz_constrained, wz_constrained); } - // Taking more time compared to for loop + // Taking more time compared to for loop on raw ointer //wz = ((vx.abs() / wz.abs() < min_turning_r_).select(wz, wz.sign() * vx / min_turning_r_)).eval(); } diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 577782a7ee..5bca01e86e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -666,7 +666,7 @@ inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) * @return dist Distance to look for * @return init Starting index to indec from */ -/*inline unsigned int findClosestPathPt( +inline unsigned int findClosestPathPt( const std::vector & vec, const float dist, const unsigned int init = 0u) { float distim1 = init != 0u ? vec[init] : 0.0f; // First is 0, no accumulated distance yet @@ -683,7 +683,7 @@ inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) distim1 = disti; } return size - 1; -}*/ +} // A struct to hold pose data in floating point resolution struct Pose2D @@ -691,6 +691,11 @@ struct Pose2D float x, y, theta; }; +/** + * @brief Shift columns of a Eigen Array + * @param e input Eigen Array or expression + * @return a column wise shifted Eigen Array + */ template auto rollColumns(T&& e, std::ptrdiff_t shift) { diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 2057b55a6a..3483c064fc 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -28,9 +28,6 @@ namespace mppi { -//using namespace xt::placeholders; // NOLINT -//using xt::evaluation_strategy::immediate; - void Optimizer::initialize( rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name, std::shared_ptr costmap_ros, @@ -220,7 +217,6 @@ void Optimizer::prepare( void Optimizer::shiftControlSequence() { - //using namespace xt::placeholders; // NOLINT control_sequence_.vx = utils::rollColumns(control_sequence_.vx, 1); control_sequence_.wz = utils::rollColumns(control_sequence_.wz, 1); From 62eb959b028bff1a9a1e4feec1d2a16cd4940787 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Sat, 17 Aug 2024 01:56:24 +0530 Subject: [PATCH 08/21] Updated path align critic and velocity deadband critic with Eigen Signed-off-by: Ayush1285 --- nav2_mppi_controller/CMakeLists.txt | 4 +- nav2_mppi_controller/package.xml | 2 + .../src/critics/path_align_critic.cpp | 33 +++++------- .../src/critics/velocity_deadband_critic.cpp | 51 +++++-------------- 4 files changed, 30 insertions(+), 60 deletions(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index d3b4a09043..62c6b9193c 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -84,13 +84,13 @@ add_library(mppi_critics SHARED #src/critics/cost_critic.cpp src/critics/goal_critic.cpp src/critics/goal_angle_critic.cpp - #src/critics/path_align_critic.cpp + src/critics/path_align_critic.cpp src/critics/path_follow_critic.cpp src/critics/path_angle_critic.cpp src/critics/prefer_forward_critic.cpp src/critics/twirling_critic.cpp #src/critics/constraint_critic.cpp - #src/critics/velocity_deadband_critic.cpp + src/critics/velocity_deadband_critic.cpp ) set(libraries mppi_controller mppi_critics) # diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 2bfdfd945e..77e79672ca 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -29,6 +29,8 @@ libomp-dev benchmark xsimd + eigen3_cmake_module + eigen ament_lint_auto ament_lint_common diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index bd0245f824..a3d0552be4 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -17,9 +17,6 @@ namespace mppi::critics { -using namespace xt::placeholders; // NOLINT -using xt::evaluation_strategy::immediate; - void PathAlignCritic::initialize() { auto getParam = parameters_handler_->getParamGetter(name_); @@ -69,8 +66,8 @@ void PathAlignCritic::score(CriticData & data) } } - const size_t batch_size = data.trajectories.x.shape(0); - auto && cost = xt::xtensor::from_shape({data.costs.shape(0)}); + const size_t batch_size = data.trajectories.x.cols(); + Eigen::ArrayXf cost(data.costs.rows()); // Find integrated distance in the path std::vector path_integrated_distances(path_segments_count, 0.0f); @@ -98,18 +95,16 @@ void PathAlignCritic::score(CriticData & data) unsigned int path_pt = 0u; float traj_integrated_distance = 0.0f; + int strided_traj_rows = data.trajectories.x.rows(); + int strided_traj_cols = data.trajectories.x.cols()/trajectory_point_step_ + 1; + int outer_stride = strided_traj_rows * trajectory_point_step_; // Get strided trajectory information - const auto T_x = xt::view( - data.trajectories.x, xt::all(), - xt::range(0, _, trajectory_point_step_)); - const auto T_y = xt::view( - data.trajectories.y, xt::all(), - xt::range(0, _, trajectory_point_step_)); - const auto T_yaw = xt::view( - data.trajectories.yaws, xt::all(), - xt::range(0, _, trajectory_point_step_)); - const auto traj_sampled_size = T_x.shape(1); + const auto T_x = Eigen::Map>(data.trajectories.x.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto T_y = Eigen::Map>(data.trajectories.y.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto T_yaw = Eigen::Map>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto traj_sampled_size = T_x.cols(); + // TODO: check performance for both row-major and column-major looping for (size_t t = 0; t < batch_size; ++t) { summed_path_dist = 0.0f; num_samples = 0u; @@ -117,7 +112,7 @@ void PathAlignCritic::score(CriticData & data) path_pt = 0u; float Tx_m1 = T_x(t, 0); float Ty_m1 = T_y(t, 0); - for (size_t p = 1; p < traj_sampled_size; p++) { + for (int p = 1; p < traj_sampled_size; p++) { const float Tx = T_x(t, p); const float Ty = T_y(t, p); dx = Tx - Tx_m1; @@ -144,14 +139,14 @@ void PathAlignCritic::score(CriticData & data) } } if (num_samples > 0u) { - cost[t] = summed_path_dist / static_cast(num_samples); + cost(t) = summed_path_dist / static_cast(num_samples); } else { - cost[t] = 0.0f; + cost(t) = 0.0f; } } if (power_ > 1u) { - data.costs += xt::pow(std::move(cost) * weight_, power_); + data.costs += (std::move(cost) * weight_).pow(power_); } else { data.costs += std::move(cost) * weight_; } diff --git a/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp index 84d3aba303..a553391495 100644 --- a/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp +++ b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp @@ -40,59 +40,32 @@ void VelocityDeadbandCritic::initialize() void VelocityDeadbandCritic::score(CriticData & data) { - using xt::evaluation_strategy::immediate; if (!enabled_) { return; } - auto & vx = data.state.vx; - auto & wz = data.state.wz; - if (data.motion_model->isHolonomic()) { - auto & vy = data.state.vy; if (power_ > 1u) { - data.costs += xt::pow( - xt::sum( - std::move( - xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + - xt::maximum(fabs(deadband_velocities_.at(1)) - xt::fabs(vy), 0) + - xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0)) * - data.model_dt, - {1}, immediate) * - weight_, - power_); + data.costs += ((((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + + (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + + (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * data.model_dt).rowwise().sum() * weight_).pow(power_); } else { - data.costs += xt::sum( - (std::move( - xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + - xt::maximum(fabs(deadband_velocities_.at(1)) - xt::fabs(vy), 0) + - xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0))) * - data.model_dt, - {1}, immediate) * - weight_; + data.costs += (((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + + (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + + (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * data.model_dt).rowwise().sum() * weight_; } return; } if (power_ > 1u) { - data.costs += xt::pow( - xt::sum( - std::move( - xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + - xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0)) * - data.model_dt, - {1}, immediate) * - weight_, - power_); + data.costs += ((((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + + (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + + (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * data.model_dt).rowwise().sum() * weight_).pow(power_); } else { - data.costs += xt::sum( - (std::move( - xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + - xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0))) * - data.model_dt, - {1}, immediate) * - weight_; + data.costs += (((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + + (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + + (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * data.model_dt).rowwise().sum() * weight_; } return; } From 3d8f987f2f8a5c7177fbbce30556d805d9a3935e Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Mon, 19 Aug 2024 22:53:06 +0530 Subject: [PATCH 09/21] Updated cost critic and constraint critic with eigen Signed-off-by: Ayush1285 --- nav2_mppi_controller/CMakeLists.txt | 4 +- .../src/critics/constraint_critic.cpp | 62 +++++++------------ .../src/critics/cost_critic.cpp | 30 ++++----- 3 files changed, 40 insertions(+), 56 deletions(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 62c6b9193c..e8dd0f2217 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -81,7 +81,7 @@ add_library(mppi_controller SHARED add_library(mppi_critics SHARED src/critics/obstacles_critic.cpp - #src/critics/cost_critic.cpp + src/critics/cost_critic.cpp src/critics/goal_critic.cpp src/critics/goal_angle_critic.cpp src/critics/path_align_critic.cpp @@ -89,7 +89,7 @@ add_library(mppi_critics SHARED src/critics/path_angle_critic.cpp src/critics/prefer_forward_critic.cpp src/critics/twirling_critic.cpp - #src/critics/constraint_critic.cpp + src/critics/constraint_critic.cpp src/critics/velocity_deadband_critic.cpp ) diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index c359be2b34..ae744f191e 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -40,8 +40,6 @@ void ConstraintCritic::initialize() void ConstraintCritic::score(CriticData & data) { - using xt::evaluation_strategy::immediate; - if (!enabled_) { return; } @@ -50,18 +48,9 @@ void ConstraintCritic::score(CriticData & data) auto diff = dynamic_cast(data.motion_model.get()); if (diff != nullptr) { if (power_ > 1u) { - data.costs += xt::pow( - xt::sum( - (std::move( - xt::maximum(data.state.vx - max_vel_, 0.0f) + - xt::maximum(min_vel_ - data.state.vx, 0.0f))) * - data.model_dt, {1}, immediate) * weight_, power_); + data.costs += (((((data.state.vx - max_vel_).max(0.0f) + (min_vel_ - data.state.vx).max(0.0f)) * data.model_dt).rowwise().sum().eval()) * weight_).pow(power_); } else { - data.costs += xt::sum( - (std::move( - xt::maximum(data.state.vx - max_vel_, 0.0f) + - xt::maximum(min_vel_ - data.state.vx, 0.0f))) * - data.model_dt, {1}, immediate) * weight_; + data.costs += ((((data.state.vx - max_vel_).max(0.0f) + (min_vel_ - data.state.vx).max(0.0f)) * data.model_dt).rowwise().sum().eval()) * weight_; } return; } @@ -69,21 +58,23 @@ void ConstraintCritic::score(CriticData & data) // Omnidirectional motion model auto omni = dynamic_cast(data.motion_model.get()); if (omni != nullptr) { - auto sgn = xt::eval(xt::where(data.state.vx > 0.0f, 1.0f, -1.0f)); - auto vel_total = sgn * xt::hypot(data.state.vx, data.state.vy); + int n_rows = data.state.vx.rows(); + int n_cols = data.state.vx.cols(); + Eigen::ArrayXXf sgn(n_rows, n_cols); + auto vx_ptr = data.state.vx.data(); + for(int i = 0; i != n_cols; i++) + { + for(int j = 0; j != n_rows; j++) + { + sgn(j, i) = *(vx_ptr + i * n_rows + j) > 0.0f ? 1.0f : -1.0f; + } + } + + auto vel_total = (data.state.vx.square() + data.state.vy.square()).sqrt() * sgn; if (power_ > 1u) { - data.costs += xt::pow( - xt::sum( - (std::move( - xt::maximum(vel_total - max_vel_, 0.0f) + - xt::maximum(min_vel_ - vel_total, 0.0f))) * - data.model_dt, {1}, immediate) * weight_, power_); + data.costs += ((std::move((vel_total - max_vel_).max(0.0f) + (min_vel_ - vel_total).max(0.0f)) * data.model_dt).rowwise().sum().eval() * weight_).pow(power_); } else { - data.costs += xt::sum( - (std::move( - xt::maximum(vel_total - max_vel_, 0.0f) + - xt::maximum(min_vel_ - vel_total, 0.0f))) * - data.model_dt, {1}, immediate) * weight_; + data.costs += (std::move((vel_total - max_vel_).max(0.0f) + (min_vel_ - vel_total).max(0.0f)) * data.model_dt).rowwise().sum().eval() * weight_; } return; } @@ -93,21 +84,14 @@ void ConstraintCritic::score(CriticData & data) if (acker != nullptr) { auto & vx = data.state.vx; auto & wz = data.state.wz; - auto out_of_turning_rad_motion = xt::maximum( - acker->getMinTurningRadius() - (xt::fabs(vx) / xt::fabs(wz)), 0.0f); + float min_turning_rad = acker->getMinTurningRadius(); + auto out_of_turning_rad_motion = (min_turning_rad - (vx.abs()/wz.abs())).max(0.0f); if (power_ > 1u) { - data.costs += xt::pow( - xt::sum( - (std::move( - xt::maximum(data.state.vx - max_vel_, 0.0f) + - xt::maximum(min_vel_ - data.state.vx, 0.0f) + out_of_turning_rad_motion)) * - data.model_dt, {1}, immediate) * weight_, power_); + data.costs += ((std::move((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) + out_of_turning_rad_motion) + * data.model_dt).rowwise().sum().eval() * weight_).pow(power_); } else { - data.costs += xt::sum( - (std::move( - xt::maximum(data.state.vx - max_vel_, 0.0f) + - xt::maximum(min_vel_ - data.state.vx, 0.0f) + out_of_turning_rad_motion)) * - data.model_dt, {1}, immediate) * weight_; + data.costs += (std::move((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) + out_of_turning_rad_motion) + * data.model_dt).rowwise().sum().eval() * weight_; } return; } diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index f348d7780a..e92d61f115 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -96,8 +96,6 @@ float CostCritic::findCircumscribedCost( void CostCritic::score(CriticData & data) { - using xt::evaluation_strategy::immediate; - using xt::placeholders::_; if (!enabled_) { return; } @@ -122,24 +120,27 @@ void CostCritic::score(CriticData & data) near_goal = true; } - auto && repulsive_cost = xt::xtensor::from_shape({data.costs.shape(0)}); + Eigen::ArrayXf repulsive_cost(data.costs.rows()); bool all_trajectories_collide = true; - const size_t traj_len = floor(data.trajectories.x.shape(1) / trajectory_point_step_); - const auto traj_x = - xt::view(data.trajectories.x, xt::all(), xt::range(0, _, trajectory_point_step_)); - const auto traj_y = - xt::view(data.trajectories.y, xt::all(), xt::range(0, _, trajectory_point_step_)); - const auto traj_yaw = xt::view( - data.trajectories.yaws, xt::all(), xt::range(0, _, trajectory_point_step_)); + int strided_traj_cols = data.trajectories.x.cols()/trajectory_point_step_ + 1; + int strided_traj_rows = data.trajectories.x.rows(); + int outer_stride = strided_traj_rows * trajectory_point_step_; - for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) { + const auto traj_x = Eigen::Map> + (data.trajectories.x.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto traj_y = Eigen::Map> + (data.trajectories.y.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto traj_yaw = Eigen::Map> + (data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + + for (int i = 0; i < strided_traj_rows; ++i) { bool trajectory_collide = false; float pose_cost = 0.0f; float & traj_cost = repulsive_cost[i]; traj_cost = 0.0f; - for (size_t j = 0; j < traj_len; j++) { + for (int j = 0; j < strided_traj_cols; j++) { float Tx = traj_x(i, j); float Ty = traj_y(i, j); unsigned int x_i = 0u, y_i = 0u; @@ -182,10 +183,9 @@ void CostCritic::score(CriticData & data) } if (power_ > 1u) { - data.costs += xt::pow( - (std::move(repulsive_cost) * (weight_ / static_cast(traj_len))), power_); + data.costs += (std::move(repulsive_cost) * (weight_/static_cast(strided_traj_cols))).pow(power_); } else { - data.costs += std::move(repulsive_cost) * (weight_ / static_cast(traj_len)); + data.costs += std::move(repulsive_cost) * (weight_/static_cast(strided_traj_cols)); } data.fail_flag = all_trajectories_collide; From bc5b377b78e7d9d1c56645e3ebd3c22297f36917 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Mon, 19 Aug 2024 23:50:12 +0530 Subject: [PATCH 10/21] Updated utils test with Eigen Signed-off-by: Ayush1285 --- nav2_mppi_controller/CMakeLists.txt | 2 +- nav2_mppi_controller/test/CMakeLists.txt | 32 +++++++------- nav2_mppi_controller/test/utils_test.cpp | 54 ++++++++++++------------ 3 files changed, 43 insertions(+), 45 deletions(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index e8dd0f2217..1f34e4bd7d 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -117,7 +117,7 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() - # add_subdirectory(test) + add_subdirectory(test) add_subdirectory(benchmark) endif() diff --git a/nav2_mppi_controller/test/CMakeLists.txt b/nav2_mppi_controller/test/CMakeLists.txt index 6305d4e31a..7923f99706 100644 --- a/nav2_mppi_controller/test/CMakeLists.txt +++ b/nav2_mppi_controller/test/CMakeLists.txt @@ -1,15 +1,15 @@ set(TEST_NAMES - optimizer_smoke_test - controller_state_transition_test - models_test - noise_generator_test - parameter_handler_test - motion_model_tests - trajectory_visualizer_tests + # optimizer_smoke_test + # controller_state_transition_test + # models_test + # noise_generator_test + # parameter_handler_test + # motion_model_tests + # trajectory_visualizer_tests utils_test - path_handler_test - critic_manager_test - optimizer_unit_tests + # path_handler_test + # critic_manager_test + # optimizer_unit_tests ) foreach(name IN LISTS TEST_NAMES) @@ -32,9 +32,9 @@ foreach(name IN LISTS TEST_NAMES) endforeach() # This is a special case requiring linking against the critics library -ament_add_gtest(critics_tests critics_tests.cpp) -ament_target_dependencies(critics_tests ${dependencies_pkgs}) -target_link_libraries(critics_tests mppi_controller mppi_critics) -if(${TEST_DEBUG_INFO}) - target_compile_definitions(critics_tests PUBLIC -DTEST_DEBUG_INFO) -endif() +# ament_add_gtest(critics_tests critics_tests.cpp) +# ament_target_dependencies(critics_tests ${dependencies_pkgs}) +# target_link_libraries(critics_tests mppi_controller mppi_critics) +# if(${TEST_DEBUG_INFO}) +# target_compile_definitions(critics_tests PUBLIC -DTEST_DEBUG_INFO) +# endif() diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 3603d4f3cf..1787bc7e33 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -15,11 +15,6 @@ #include #include -#pragma GCC diagnostic ignored "-Warray-bounds" -#pragma GCC diagnostic ignored "-Wstringop-overflow" -#include -#pragma GCC diagnostic pop - #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "nav2_mppi_controller/tools/utils.hpp" @@ -120,9 +115,9 @@ TEST(UtilsTests, ConversionTests) models::Path path_t = toTensor(path); // Check population is correct - EXPECT_EQ(path_t.x.shape(0), 5u); - EXPECT_EQ(path_t.y.shape(0), 5u); - EXPECT_EQ(path_t.yaws.shape(0), 5u); + EXPECT_EQ(path_t.x.rows(), 5u); + EXPECT_EQ(path_t.y.rows(), 5u); + EXPECT_EQ(path_t.yaws.rows(), 5u); EXPECT_EQ(path_t.x(2), 5); EXPECT_EQ(path_t.y(2), 50); EXPECT_NEAR(path_t.yaws(2), 0.0, 1e-6); @@ -172,9 +167,10 @@ TEST(UtilsTests, WithTolTests) TEST(UtilsTests, AnglesTests) { // Test angle normalization by creating insane angles - xt::xtensor angles, zero_angles; - angles = xt::ones({100}); - for (unsigned int i = 0; i != angles.shape(0); i++) { + Eigen::ArrayXf angles(100); + angles.setConstant(1.0f); + + for (unsigned int i = 0; i != angles.size(); i++) { angles(i) = i * i; if (i % 2 == 0) { angles(i) *= -1; @@ -182,15 +178,17 @@ TEST(UtilsTests, AnglesTests) } auto norm_ang = normalize_angles(angles); - for (unsigned int i = 0; i != norm_ang.shape(0); i++) { - EXPECT_TRUE((norm_ang(i) >= -M_PI) && (norm_ang(i) <= M_PI)); + for (unsigned int i = 0; i != norm_ang.size(); i++) { + //Sstd::cout << "norm_ang(" << i << ") = " << norm_ang(i) << std::endl; + EXPECT_TRUE((norm_ang(i) >= -M_PIF) && (norm_ang(i) <= M_PIF)); } // Test shortest angular distance - zero_angles = xt::zeros({100}); + Eigen::ArrayXf zero_angles(100); + zero_angles.setZero(); auto ang_dist = shortest_angular_distance(angles, zero_angles); - for (unsigned int i = 0; i != ang_dist.shape(0); i++) { - EXPECT_TRUE((ang_dist(i) >= -M_PI) && (ang_dist(i) <= M_PI)); + for (unsigned int i = 0; i != ang_dist.size(); i++) { + EXPECT_TRUE((ang_dist(i) >= -M_PIF) && (ang_dist(i) <= M_PIF)); } // Test point-pose angle @@ -225,7 +223,7 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) models::State state; models::Trajectories generated_trajectories; models::Path path; - xt::xtensor costs; + Eigen::ArrayXf costs; float model_dt = 0.1; CriticData data = @@ -245,9 +243,9 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) EXPECT_EQ(data2.furthest_reached_path_point, 0); // Test the actual computation of the path point reached - generated_trajectories.x = xt::ones({100, 2}); - generated_trajectories.y = xt::zeros({100, 2}); - generated_trajectories.yaws = xt::zeros({100, 2}); + generated_trajectories.x = Eigen::ArrayXXf::Ones(100, 2); + generated_trajectories.y = Eigen::ArrayXXf::Zero(100, 2); + generated_trajectories.yaws = Eigen::ArrayXXf::Zero(100, 2); nav_msgs::msg::Path plan; plan.poses.resize(10); @@ -260,7 +258,7 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) CriticData data3 = {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references - EXPECT_EQ(findPathFurthestReachedPoint(data3), 5u); + EXPECT_EQ(findPathFurthestReachedPoint(data3), 5); } TEST(UtilsTests, findPathCosts) @@ -268,7 +266,7 @@ TEST(UtilsTests, findPathCosts) models::State state; models::Trajectories generated_trajectories; models::Path path; - xt::xtensor costs; + Eigen::ArrayXf costs; float model_dt = 0.1; CriticData data = @@ -316,7 +314,7 @@ TEST(UtilsTests, findPathCosts) // This should be evaluated and have real outputs now setPathCostsIfNotSet(data3, costmap_ros); EXPECT_TRUE(data3.path_pts_valid.has_value()); - for (unsigned int i = 0; i != path.x.shape(0) - 1; i++) { + for (unsigned int i = 0; i != path.x.size() - 1; i++) { if (i == 1 || i == 10) { EXPECT_FALSE((*data3.path_pts_valid)[i]); } else { @@ -328,12 +326,12 @@ TEST(UtilsTests, findPathCosts) TEST(UtilsTests, SmootherTest) { models::ControlSequence noisey_sequence, sequence_init; - noisey_sequence.vx = 0.2 * xt::ones({30}); - noisey_sequence.vy = 0.0 * xt::ones({30}); - noisey_sequence.wz = 0.3 * xt::ones({30}); + noisey_sequence.vx = 0.2 * Eigen::ArrayXf::Ones(30); + noisey_sequence.vy = 0.0 * Eigen::ArrayXf::Ones(30); + noisey_sequence.wz = 0.3 * Eigen::ArrayXf::Ones(30); // Make the sequence noisy - auto noises = xt::random::randn({30}, 0.0, 0.2); + auto noises = ((Eigen::ArrayXf::Random(30)).abs()) / 5.0f; noisey_sequence.vx += noises; noisey_sequence.vy += noises; noisey_sequence.wz += noises; @@ -371,7 +369,7 @@ TEST(UtilsTests, SmootherTest) // Check that path is smoother float smoothed_val{0}, original_val{0}; - for (unsigned int i = 1; i != noisey_sequence.vx.shape(0) - 1; i++) { + for (unsigned int i = 1; i != noisey_sequence.vx.size() - 1; i++) { smoothed_val += fabs(noisey_sequence.vx(i) - 0.2); smoothed_val += fabs(noisey_sequence.vy(i) - 0.0); smoothed_val += fabs(noisey_sequence.wz(i) - 0.3); From 5fb800658e4e45dff335e392dab91ae8d4e53bf5 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Wed, 21 Aug 2024 00:45:50 +0530 Subject: [PATCH 11/21] Reverted unnecessary changes and fixed static instance in Noise generator Signed-off-by: Ayush1285 --- nav2_mppi_controller/CMakeLists.txt | 8 ++--- nav2_mppi_controller/benchmark/CMakeLists.txt | 2 +- .../benchmark/controller_benchmark.cpp | 20 ++++++----- .../benchmark/optimizer_benchmark.cpp | 22 ++++++------ .../nav2_mppi_controller/motion_models.hpp | 2 +- .../nav2_mppi_controller/optimizer.hpp | 2 +- .../tools/noise_generator.hpp | 9 ++--- nav2_mppi_controller/src/noise_generator.cpp | 15 ++++++-- nav2_mppi_controller/test/CMakeLists.txt | 34 +++++++++---------- 9 files changed, 64 insertions(+), 50 deletions(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 1f34e4bd7d..6a9942f9d7 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -73,7 +73,7 @@ add_library(mppi_controller SHARED src/controller.cpp src/optimizer.cpp src/critic_manager.cpp - # src/trajectory_visualizer.cpp + src/trajectory_visualizer.cpp src/path_handler.cpp src/parameters_handler.cpp src/noise_generator.cpp @@ -93,16 +93,16 @@ add_library(mppi_critics SHARED src/critics/velocity_deadband_critic.cpp ) -set(libraries mppi_controller mppi_critics) # +set(libraries mppi_controller mppi_critics) foreach(lib IN LISTS libraries) target_compile_options(${lib} PUBLIC -fconcepts) - target_include_directories(${lib} PUBLIC ${xsimd_INCLUDE_DIRS}) # ${OpenMP_INCLUDE_DIRS} + target_include_directories(${lib} PUBLIC ${xsimd_INCLUDE_DIRS}) # ${OpenMP_INCLUDE_DIRS} target_link_libraries(${lib} xtensor xtensor::optimize xtensor::use_xsimd) ament_target_dependencies(${lib} ${dependencies_pkgs}) endforeach() -install(TARGETS mppi_controller +install(TARGETS mppi_controller mppi_critics ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/nav2_mppi_controller/benchmark/CMakeLists.txt b/nav2_mppi_controller/benchmark/CMakeLists.txt index cbce8e1cf0..765368d192 100644 --- a/nav2_mppi_controller/benchmark/CMakeLists.txt +++ b/nav2_mppi_controller/benchmark/CMakeLists.txt @@ -13,7 +13,7 @@ foreach(name IN LISTS BENCHMARK_NAMES) ${dependencies_pkgs} ) target_link_libraries(${name} - mppi_controller benchmark + mppi_controller benchmark mppi_critics ) target_include_directories(${name} PRIVATE diff --git a/nav2_mppi_controller/benchmark/controller_benchmark.cpp b/nav2_mppi_controller/benchmark/controller_benchmark.cpp index 5306845f3c..dfff3d5403 100644 --- a/nav2_mppi_controller/benchmark/controller_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/controller_benchmark.cpp @@ -120,7 +120,8 @@ static void BM_DiffDrivePointFootprint(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "DiffDrive"; - std::vector critics = {}; + std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } @@ -129,7 +130,8 @@ static void BM_DiffDrive(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "DiffDrive"; - std::vector critics = {}; + std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } @@ -139,7 +141,8 @@ static void BM_Omni(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "Omni"; - std::vector critics = {}; + std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {"TwirlingCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } @@ -148,12 +151,13 @@ static void BM_Ackermann(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "Ackermann"; - std::vector critics = {}; + std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } -/*static void BM_GoalCritic(benchmark::State & state) +static void BM_GoalCritic(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "Ackermann"; @@ -214,19 +218,19 @@ static void BM_PathAngleCritic(benchmark::State & state) std::vector critics = {{"PathAngleCritic"}}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); -}*/ +} BENCHMARK(BM_DiffDrivePointFootprint)->Unit(benchmark::kMillisecond); BENCHMARK(BM_DiffDrive)->Unit(benchmark::kMillisecond); BENCHMARK(BM_Omni)->Unit(benchmark::kMillisecond); BENCHMARK(BM_Ackermann)->Unit(benchmark::kMillisecond); -/*BENCHMARK(BM_GoalCritic)->Unit(benchmark::kMillisecond); +BENCHMARK(BM_GoalCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_GoalAngleCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_PathAngleCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_PathFollowCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_ObstaclesCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_ObstaclesCriticPointFootprint)->Unit(benchmark::kMillisecond); -BENCHMARK(BM_TwilringCritic)->Unit(benchmark::kMillisecond);*/ +BENCHMARK(BM_TwilringCritic)->Unit(benchmark::kMillisecond); BENCHMARK_MAIN(); diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index 8f270c13d5..75a29bb91b 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -47,11 +47,11 @@ void prepareAndRunBenchmark( bool consider_footprint, std::string motion_model, std::vector critics, benchmark::State & state) { - int batch_size = 2000; - int time_steps = 70; - unsigned int path_points = 150u; + int batch_size = 300; + int time_steps = 12; + unsigned int path_points = 50u; int iteration_count = 2; - double lookahead_distance = 40.0; + double lookahead_distance = 10.0; TestCostmapSettings costmap_settings{}; auto costmap_ros = getDummyCostmapRos(costmap_settings); @@ -112,14 +112,14 @@ static void BM_DiffDrive(benchmark::State & state) } -/*static void BM_Omni(benchmark::State & state) +static void BM_Omni(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "Omni"; std::vector critics = {}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); -}*/ +} static void BM_Ackermann(benchmark::State & state) { @@ -131,7 +131,7 @@ static void BM_Ackermann(benchmark::State & state) prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } -/*static void BM_GoalCritic(benchmark::State & state) +static void BM_GoalCritic(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "Ackermann"; @@ -192,19 +192,19 @@ static void BM_PathAngleCritic(benchmark::State & state) std::vector critics = {{"PathAngleCritic"}}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); -}*/ +} BENCHMARK(BM_DiffDrivePointFootprint)->Unit(benchmark::kMillisecond); BENCHMARK(BM_DiffDrive)->Unit(benchmark::kMillisecond); -//BENCHMARK(BM_Omni)->Unit(benchmark::kMillisecond); +BENCHMARK(BM_Omni)->Unit(benchmark::kMillisecond); BENCHMARK(BM_Ackermann)->Unit(benchmark::kMillisecond); -/*BENCHMARK(BM_GoalCritic)->Unit(benchmark::kMillisecond); +BENCHMARK(BM_GoalCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_GoalAngleCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_PathAngleCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_PathFollowCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_ObstaclesCritic)->Unit(benchmark::kMillisecond); BENCHMARK(BM_ObstaclesCriticPointFootprint)->Unit(benchmark::kMillisecond); -BENCHMARK(BM_TwilringCritic)->Unit(benchmark::kMillisecond);*/ +BENCHMARK(BM_TwilringCritic)->Unit(benchmark::kMillisecond); BENCHMARK_MAIN(); 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 17fdc84555..7a5538a32b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -153,7 +153,7 @@ class AckermannMotionModel : public MotionModel float & wz_curr = *(wz_ptr + i); wz_curr = std::clamp(wz_curr, -1 * wz_constrained, wz_constrained); } - // Taking more time compared to for loop on raw ointer + // Taking more time compared to for loop on raw pointer //wz = ((vx.abs() / wz.abs() < min_turning_r_).select(wz, wz.sign() * vx / min_turning_r_)).eval(); } diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index c7a594afb9..98e4e87986 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -188,7 +188,7 @@ class Optimizer */ void integrateStateVelocities( models::Trajectories & trajectories, - models::State & state) const; + const models::State & state) const; /** * @brief Rollout velocities in state to poses diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp index 1902777d1a..4e9d4e69f5 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp @@ -79,9 +79,7 @@ class NoiseGenerator */ void reset(mppi::models::OptimizerSettings & settings, bool is_holonomic); - inline static std::normal_distribution ndistribution_vx; - inline static std::normal_distribution ndistribution_wz; - inline static std::normal_distribution ndistribution_vy; + protected: /** @@ -102,7 +100,10 @@ class NoiseGenerator Eigen::ArrayXXf noises_vy_; Eigen::ArrayXXf noises_wz_; - inline static std::default_random_engine generator_; + static std::default_random_engine generator_; + static std::normal_distribution ndistribution_vx_; + static std::normal_distribution ndistribution_wz_; + static std::normal_distribution ndistribution_vy_; mppi::models::OptimizerSettings settings_; bool is_holonomic_; diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index 1c3948095f..872e41580c 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -20,6 +20,11 @@ namespace mppi { +std::normal_distribution NoiseGenerator::ndistribution_vx_; +std::normal_distribution NoiseGenerator::ndistribution_vy_; +std::normal_distribution NoiseGenerator::ndistribution_wz_; +std::default_random_engine NoiseGenerator::generator_; + void NoiseGenerator::initialize( mppi::models::OptimizerSettings & settings, bool is_holonomic, const std::string & name, ParametersHandler * param_handler) @@ -28,6 +33,10 @@ void NoiseGenerator::initialize( is_holonomic_ = is_holonomic; active_ = true; + ndistribution_vx_ = std::normal_distribution(0.0f, settings_.sampling_std.vx); + ndistribution_vy_ = std::normal_distribution(0.0f, settings_.sampling_std.vy); + ndistribution_wz_ = std::normal_distribution(0.0f, settings_.sampling_std.wz); + auto getParam = param_handler->getParamGetter(name); getParam(regenerate_noises_, "regenerate_noises", false); @@ -104,10 +113,10 @@ void NoiseGenerator::noiseThread() void NoiseGenerator::generateNoisedControls() { auto & s = settings_; - noises_vx_ = Eigen::ArrayXXf::NullaryExpr(s.batch_size, s.time_steps, [&] () {return ndistribution_vx(generator_);}); - noises_wz_ = Eigen::ArrayXXf::NullaryExpr(s.batch_size, s.time_steps, [&] () {return ndistribution_wz(generator_);}); + noises_vx_ = Eigen::ArrayXXf::NullaryExpr(s.batch_size, s.time_steps, [&] () {return ndistribution_vx_(generator_);}); + noises_wz_ = Eigen::ArrayXXf::NullaryExpr(s.batch_size, s.time_steps, [&] () {return ndistribution_wz_(generator_);}); if(is_holonomic_) { - noises_vy_ = Eigen::ArrayXXf::NullaryExpr(s.batch_size, s.time_steps, [&] () {return ndistribution_vy(generator_);}); + noises_vy_ = Eigen::ArrayXXf::NullaryExpr(s.batch_size, s.time_steps, [&] () {return ndistribution_vy_(generator_);}); } } diff --git a/nav2_mppi_controller/test/CMakeLists.txt b/nav2_mppi_controller/test/CMakeLists.txt index 7923f99706..0c813400c4 100644 --- a/nav2_mppi_controller/test/CMakeLists.txt +++ b/nav2_mppi_controller/test/CMakeLists.txt @@ -1,15 +1,15 @@ set(TEST_NAMES - # optimizer_smoke_test - # controller_state_transition_test - # models_test - # noise_generator_test - # parameter_handler_test - # motion_model_tests - # trajectory_visualizer_tests + optimizer_smoke_test + controller_state_transition_test + models_test + noise_generator_test + parameter_handler_test + motion_model_tests + trajectory_visualizer_tests utils_test - # path_handler_test - # critic_manager_test - # optimizer_unit_tests + path_handler_test + critic_manager_test + optimizer_unit_tests ) foreach(name IN LISTS TEST_NAMES) @@ -31,10 +31,10 @@ foreach(name IN LISTS TEST_NAMES) endforeach() -# This is a special case requiring linking against the critics library -# ament_add_gtest(critics_tests critics_tests.cpp) -# ament_target_dependencies(critics_tests ${dependencies_pkgs}) -# target_link_libraries(critics_tests mppi_controller mppi_critics) -# if(${TEST_DEBUG_INFO}) -# target_compile_definitions(critics_tests PUBLIC -DTEST_DEBUG_INFO) -# endif() +This is a special case requiring linking against the critics library +ament_add_gtest(critics_tests critics_tests.cpp) +ament_target_dependencies(critics_tests ${dependencies_pkgs}) +target_link_libraries(critics_tests mppi_controller mppi_critics) +if(${TEST_DEBUG_INFO}) + target_compile_definitions(critics_tests PUBLIC -DTEST_DEBUG_INFO) +endif() From 46baa069952917a7b41d1ed4c37df5bf092551db Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Fri, 23 Aug 2024 22:37:29 +0530 Subject: [PATCH 12/21] changes std::abs to fabs, clamp to min-max Signed-off-by: Ayush1285 --- .../nav2_mppi_controller/motion_models.hpp | 12 +++++----- .../nav2_mppi_controller/tools/utils.hpp | 2 +- .../src/critics/constraint_critic.cpp | 14 ++++------- nav2_mppi_controller/src/optimizer.cpp | 23 ++++++++----------- 4 files changed, 21 insertions(+), 30 deletions(-) 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 7a5538a32b..acaef13e45 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -77,18 +77,18 @@ class MotionModel for (unsigned int j = 0; j != n_rows; j++) { float & vx_last = state.vx(j, i - 1); float & cvx_curr = state.cvx(j, i - 1); - cvx_curr = std::clamp(cvx_curr, vx_last + min_delta_vx, vx_last + max_delta_vx); + cvx_curr = std::min(vx_last + max_delta_vx, std::max(cvx_curr, vx_last + min_delta_vx)); state.vx(j, i) = cvx_curr; float & wz_last = state.wz(j, i - 1); float & cwz_curr = state.cwz(j, i - 1); - cwz_curr = std::clamp(cwz_curr, wz_last - max_delta_wz, wz_last + max_delta_wz); + cwz_curr = std::min(wz_last + max_delta_wz, std::max(cwz_curr, wz_last - max_delta_wz)); state.wz(j, i) = cwz_curr; if (is_holo) { float & vy_last = state.vy(j, i - 1); float & cvy_curr = state.cvy(j, i - 1); - cvy_curr = std::clamp(cvy_curr, vy_last - max_delta_vy, vy_last + max_delta_vy); + cvy_curr = std::min(vy_last + max_delta_vy, std::max(cvy_curr, vy_last - max_delta_vy)); state.vy(j, i) = cvy_curr; } } @@ -149,11 +149,11 @@ class AckermannMotionModel : public MotionModel int steps = control_sequence.vx.size(); for(int i = 0; i < steps; i++) { - float && wz_constrained = std::abs(*(vx_ptr + i) / min_turning_r_); + float && wz_constrained = fabs(*(vx_ptr + i) / min_turning_r_); float & wz_curr = *(wz_ptr + i); - wz_curr = std::clamp(wz_curr, -1 * wz_constrained, wz_constrained); + wz_curr = std::min(wz_constrained, std::max(wz_curr, -1 * wz_constrained)); } - // Taking more time compared to for loop on raw pointer + // Taking more time compared to for loop //wz = ((vx.abs() / wz.abs() < min_turning_r_).select(wz, wz.sign() * vx / min_turning_r_)).eval(); } diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 5bca01e86e..5617980673 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -730,7 +730,7 @@ inline auto point_corrected_yaws(const Eigen::ArrayXf & yaws_between_points, con for(int i = 0; i != size; i++) { const float & yaw_between_points = *(yaws_between_points_ptr + i); - *(yaws_between_points_corrected_ptr + i) = std::abs(angles::normalize_angle(yaw_between_points - goal_yaw)) < M_PIF_2 ? yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF); + *(yaws_between_points_corrected_ptr + i) = fabs(angles::normalize_angle(yaw_between_points - goal_yaw)) < M_PIF_2 ? yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF); } return yaws_between_points_corrected; } diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index ae744f191e..4b090d240f 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -58,17 +58,11 @@ void ConstraintCritic::score(CriticData & data) // Omnidirectional motion model auto omni = dynamic_cast(data.motion_model.get()); if (omni != nullptr) { - int n_rows = data.state.vx.rows(); - int n_cols = data.state.vx.cols(); + auto & vx = data.state.vx; + unsigned int n_rows = data.state.vx.rows(); + unsigned int n_cols = data.state.vx.cols(); Eigen::ArrayXXf sgn(n_rows, n_cols); - auto vx_ptr = data.state.vx.data(); - for(int i = 0; i != n_cols; i++) - { - for(int j = 0; j != n_rows; j++) - { - sgn(j, i) = *(vx_ptr + i * n_rows + j) > 0.0f ? 1.0f : -1.0f; - } - } + sgn = vx.unaryExpr([](const float x){ return copysignf(1.0f, x);}); auto vel_total = (data.state.vx.square() + data.state.vy.square()).sqrt() * sgn; if (power_ > 1u) { diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 3483c064fc..f2da7ef8e9 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -81,7 +81,7 @@ void Optimizer::getParams() getParam(s.sampling_std.wz, "wz_std", 0.4f); getParam(s.retry_attempt_limit, "retry_attempt_limit", 1); - s.base_constraints.ax_max = std::abs(s.base_constraints.ax_max); + s.base_constraints.ax_max = fabs(s.base_constraints.ax_max); if (s.base_constraints.ax_min > 0.0) { s.base_constraints.ax_min = -1.0 * s.base_constraints.ax_min; RCLCPP_WARN( @@ -92,9 +92,6 @@ void Optimizer::getParams() getParam(motion_model_name, "motion_model", std::string("DiffDrive")); s.constraints = s.base_constraints; - NoiseGenerator::ndistribution_vx = std::normal_distribution(0.0f, s.sampling_std.vx); - NoiseGenerator::ndistribution_vy = std::normal_distribution(0.0f, s.sampling_std.vy); - NoiseGenerator::ndistribution_wz = std::normal_distribution(0.0f, s.sampling_std.wz); setMotionModel(motion_model_name); parameters_handler_->addPostCallback([this]() {reset();}); @@ -245,24 +242,24 @@ void Optimizer::applyControlSequenceConstraints() float && min_delta_vx = s.model_dt * s.constraints.ax_min; float && max_delta_vy = s.model_dt * s.constraints.ay_max; float && max_delta_wz = s.model_dt * s.constraints.az_max; - float vx_last = std::clamp(control_sequence_.vx(0), vx_min, vx_max); - float vy_last = std::clamp(control_sequence_.vy(0), -vy, vy); - float wz_last = std::clamp(control_sequence_.wz(0), -wz, wz); + float vx_last = std::min(vx_max, std::max(control_sequence_.vx(0), vx_min)); + float vy_last = std::min(vy, std::max(control_sequence_.vy(0), -vy)); + float wz_last = std::min(wz, std::max(control_sequence_.wz(0), -wz)); for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) { float & vx_curr = control_sequence_.vx(i); - vx_curr = std::clamp(vx_curr, vx_min, vx_max); - vx_curr = std::clamp(vx_curr, vx_last + min_delta_vx, vx_last + max_delta_vx); + vx_curr = std::min(vx_max, std::max(vx_curr, vx_min)); + vx_curr = std::min(vx_last + max_delta_vx, std::max(vx_curr, vx_last + min_delta_vx)); vx_last = vx_curr; float & wz_curr = control_sequence_.wz(i); - wz_curr = std::clamp(wz_curr, -wz, wz); - wz_curr = std::clamp(wz_curr, wz_last - max_delta_wz, wz_last + max_delta_wz); + wz_curr = std::min(wz, std::max(wz_curr, -wz)); + wz_curr = std::min(wz_last + max_delta_wz, std::max(wz_curr, wz_last - max_delta_wz)); wz_last = wz_curr; if (isHolonomic()) { float & vy_curr = control_sequence_.vy(i); - vy_curr = std::clamp(vy_curr, -vy, vy); - vy_curr = std::clamp(vy_curr, vy_last - max_delta_vy, vy_last + max_delta_vy); + vy_curr = std::min(vy, std::max(vy_curr, -vy)); + vy_curr = std::min(vy_last + max_delta_vy, std::max(vy_curr, vy_last - max_delta_vy)); vy_last = vy_curr; } } From a3368141dc63df8de8425d579f2f6779898ed787 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Tue, 3 Sep 2024 01:45:56 +0530 Subject: [PATCH 13/21] Converted tests to Eigen Signed-off-by: Ayush1285 --- .../models/trajectories.hpp | 6 +- .../tools/noise_generator.hpp | 4 +- .../src/critics/path_angle_critic.cpp | 39 +++--- nav2_mppi_controller/src/optimizer.cpp | 83 +++++++----- .../src/trajectory_visualizer.cpp | 15 +-- .../test/critic_manager_test.cpp | 2 +- nav2_mppi_controller/test/models_test.cpp | 78 ++++++------ .../test/motion_model_tests.cpp | 44 +++---- .../test/noise_generator_test.cpp | 2 +- .../test/optimizer_unit_tests.cpp | 119 +++++++++--------- .../test/trajectory_visualizer_tests.cpp | 10 +- 11 files changed, 210 insertions(+), 192 deletions(-) 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 9b09d093a2..70c155ba5d 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp @@ -35,9 +35,9 @@ struct Trajectories */ void reset(unsigned int batch_size, unsigned int time_steps) { - x = Eigen::ArrayXXf::Zero(batch_size, time_steps);; - y = Eigen::ArrayXXf::Zero(batch_size, time_steps);; - yaws = Eigen::ArrayXXf::Zero(batch_size, time_steps);; + x = Eigen::ArrayXXf::Zero(batch_size, time_steps); + y = Eigen::ArrayXXf::Zero(batch_size, time_steps); + yaws = Eigen::ArrayXXf::Zero(batch_size, time_steps); } }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp index 4e9d4e69f5..e641012f17 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp @@ -77,9 +77,7 @@ class NoiseGenerator * @param settings Settings of controller * @param is_holonomic If base is holonomic */ - void reset(mppi::models::OptimizerSettings & settings, bool is_holonomic); - - + void reset(mppi::models::OptimizerSettings & settings, bool is_holonomic); protected: /** diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 6bd7a78a3a..dbdc666b6b 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -97,46 +97,45 @@ void PathAngleCritic::score(CriticData & data) } int && rightmost_idx = data.trajectories.y.cols() - 1; - auto yaws_between_points = (goal_y - data.trajectories.y.col(rightmost_idx)).binaryExpr( - (goal_x - data.trajectories.x.col(rightmost_idx)), [&](const float & y, const float & x){return std::atan2(y, x);}).eval(); + auto diff_y = goal_y - data.trajectories.y.col(rightmost_idx); + auto diff_x = goal_x - data.trajectories.x.col(rightmost_idx); + auto yaws_between_points = diff_y.binaryExpr(diff_x, [&](const float & y, const float & x){return atan2f(y, x);}); switch (mode_) { case PathAngleMode::FORWARD_PREFERENCE: { - Eigen::ArrayXf yaws = - Eigen::abs( - utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points)); + auto yaws = + utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points).abs(); if (power_ > 1u) { - data.costs += Eigen::pow(std::move(yaws) * weight_, power_); + data.costs += (yaws * weight_).pow(power_); } else { - data.costs += (std::move(yaws) * weight_).eval(); + data.costs += yaws * weight_; } return; } case PathAngleMode::NO_DIRECTIONAL_PREFERENCE: { - Eigen::ArrayXf yaws = - Eigen::abs( - utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points)); - Eigen::ArrayXf yaws_between_points_corrected = utils::point_corrected_yaws(yaws, yaws_between_points); - Eigen::ArrayXf corrected_yaws = Eigen::abs( - utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), std::move(yaws_between_points_corrected))); + auto yaws = + utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points).abs(); + auto yaws_between_points_corrected = utils::point_corrected_yaws(yaws, yaws_between_points); + auto corrected_yaws = + utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points_corrected).abs(); if (power_ > 1u) { - data.costs += Eigen::pow(std::move(corrected_yaws) * weight_, power_); + data.costs += (corrected_yaws * weight_).pow(power_); } else { - data.costs += std::move(corrected_yaws) * weight_; + data.costs += corrected_yaws * weight_; } return; } case PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS: { - Eigen::ArrayXf yaws_between_points_corrected = utils::point_corrected_yaws(yaws_between_points, goal_yaw); - Eigen::ArrayXf corrected_yaws = Eigen::abs( - utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), std::move(yaws_between_points_corrected))); + auto yaws_between_points_corrected = utils::point_corrected_yaws(yaws_between_points, goal_yaw); + auto corrected_yaws = + utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points_corrected).abs(); if (power_ > 1u) { - data.costs += Eigen::pow(std::move(corrected_yaws) * weight_, power_); + data.costs += (corrected_yaws * weight_).pow(power_); } else { - data.costs += std::move(corrected_yaws) * weight_; + data.costs += corrected_yaws * weight_; } return; } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index f2da7ef8e9..b0fd04e9ae 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -291,45 +291,66 @@ void Optimizer::propagateStateVelocitiesFromInitials( motion_model_->predict(state); } -/*void Optimizer::integrateStateVelocities( - xt::xtensor & trajectory, - const xt::xtensor & sequence) const +void Optimizer::integrateStateVelocities( + Eigen::ArrayXXf & trajectory, + const Eigen::ArrayXXf & sequence) const { float initial_yaw = static_cast(tf2::getYaw(state_.pose.pose.orientation)); + const float & dt_time = settings_.model_dt; + + const auto vx = sequence.col(0); + const auto wz = sequence.col(1); - const auto vx = xt::view(sequence, xt::all(), 0); - const auto wz = xt::view(sequence, xt::all(), 1); + auto traj_x = trajectory.col(0); + auto traj_y = trajectory.col(1); + auto traj_yaws = trajectory.col(2); - auto traj_x = xt::view(trajectory, xt::all(), 0); - auto traj_y = xt::view(trajectory, xt::all(), 1); - auto traj_yaws = xt::view(trajectory, xt::all(), 2); + unsigned int n_size = traj_yaws.size(); - xt::noalias(traj_yaws) = xt::cumsum(wz * settings_.model_dt, 0) + initial_yaw; + traj_yaws(0) = wz(0) * dt_time + initial_yaw; + float last_yaw = traj_yaws(0); + for(unsigned int i = 1; i != n_size; i++) + { + float & curr_yaw = traj_yaws(i); + curr_yaw = last_yaw + wz(i) * dt_time; + last_yaw = curr_yaw; + } - auto yaw_cos = xt::roll(xt::eval(xt::cos(traj_yaws)), 1); - auto yaw_sin = xt::roll(xt::eval(xt::sin(traj_yaws)), 1); - xt::view(yaw_cos, 0) = cosf(initial_yaw); - xt::view(yaw_sin, 0) = sinf(initial_yaw); + auto yaw_cos = (utils::rollColumns(traj_yaws, -1).cos()).eval(); + auto yaw_sin = (utils::rollColumns(traj_yaws, -1).sin()).eval(); + yaw_cos(0) = cosf(initial_yaw); + yaw_sin(0) = sinf(initial_yaw); - auto && dx = xt::eval(vx * yaw_cos); - auto && dy = xt::eval(vx * yaw_sin); + auto dx = (vx * yaw_cos).eval(); + auto dy = (vx * yaw_sin).eval(); if (isHolonomic()) { - const auto vy = xt::view(sequence, xt::all(), 2); - dx = dx - vy * yaw_sin; - dy = dy + vy * yaw_cos; + auto vy = sequence.col(2); + dx = (dx - vy * yaw_sin).eval(); + dy = (dy + vy * yaw_cos).eval(); } - xt::noalias(traj_x) = state_.pose.pose.position.x + xt::cumsum(dx * settings_.model_dt, 0); - xt::noalias(traj_y) = state_.pose.pose.position.y + xt::cumsum(dy * settings_.model_dt, 0); -}*/ + traj_x(0) = state_.pose.pose.position.x + dx(0) * dt_time; + traj_y(0) = state_.pose.pose.position.y + dy(0) * dt_time; + float last_x = traj_x(0); + float last_y = traj_y(0); + for(unsigned int i = 1; i != n_size; i++) + { + float & curr_x = traj_x(i); + float & curr_y = traj_y(i); + curr_x = last_x + dx(i) * dt_time; + curr_y = last_y + dy(i) * dt_time; + last_x = curr_x; + last_y = curr_y; + } +} void Optimizer::integrateStateVelocities( models::Trajectories & trajectories, - models::State & state) const + const models::State & state) const { const float initial_yaw = static_cast(tf2::getYaw(state.pose.pose.orientation)); - const unsigned int n_cols = state.wz.cols(); + const unsigned int n_cols = trajectories.yaws.cols(); const float & dt_time = settings_.model_dt; trajectories.yaws.col(0) = state.wz.col(0) * dt_time + initial_yaw; @@ -343,6 +364,7 @@ void Optimizer::integrateStateVelocities( yaw_cos.col(0) = cosf(initial_yaw); yaw_sin.col(0) = sinf(initial_yaw); + auto dx = (state.vx * yaw_cos).eval(); auto dy = (state.vx * yaw_sin).eval(); @@ -360,23 +382,22 @@ void Optimizer::integrateStateVelocities( } } -/*xt::xtensor Optimizer::getOptimizedTrajectory() +Eigen::ArrayXXf Optimizer::getOptimizedTrajectory() { const bool is_holo = isHolonomic(); - auto && sequence = - xt::xtensor::from_shape({settings_.time_steps, is_holo ? 3u : 2u}); - auto && trajectories = xt::xtensor::from_shape({settings_.time_steps, 3}); + auto && sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2); + auto && trajectories = Eigen::ArrayXXf(settings_.time_steps, 3); - xt::noalias(xt::view(sequence, xt::all(), 0)) = control_sequence_.vx; - xt::noalias(xt::view(sequence, xt::all(), 1)) = control_sequence_.wz; + sequence.col(0) = control_sequence_.vx; + sequence.col(1) = control_sequence_.wz; if (is_holo) { - xt::noalias(xt::view(sequence, xt::all(), 2)) = control_sequence_.vy; + sequence.col(2) = control_sequence_.vy; } integrateStateVelocities(trajectories, sequence); return std::move(trajectories); -}*/ +} void Optimizer::updateControlSequence() { diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index a6531b7d1d..a9e51663c3 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -57,9 +57,9 @@ void TrajectoryVisualizer::on_deactivate() } void TrajectoryVisualizer::add( - const xt::xtensor & trajectory, const std::string & marker_namespace) + const Eigen::ArrayXXf & trajectory, const std::string & marker_namespace) { - auto & size = trajectory.shape()[0]; + auto & size = trajectory.rows(); if (!size) { return; } @@ -86,12 +86,13 @@ void TrajectoryVisualizer::add( void TrajectoryVisualizer::add( const models::Trajectories & trajectories, const std::string & marker_namespace) { - auto & shape = trajectories.x.shape(); - const float shape_1 = static_cast(shape[1]); - points_->markers.reserve(floor(shape[0] / trajectory_step_) * floor(shape[1] * time_step_)); + int n_rows = trajectories.x.rows(); + int n_cols = trajectories.x.cols(); + const float shape_1 = static_cast(n_cols); + points_->markers.reserve(floor(n_rows / trajectory_step_) * floor(n_cols * time_step_)); - for (size_t i = 0; i < shape[0]; i += trajectory_step_) { - for (size_t j = 0; j < shape[1]; j += time_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); float blue_component = 1.0f - j_flt / shape_1; float green_component = j_flt / shape_1; diff --git a/nav2_mppi_controller/test/critic_manager_test.cpp b/nav2_mppi_controller/test/critic_manager_test.cpp index f24d205bfb..0c0dc2c437 100644 --- a/nav2_mppi_controller/test/critic_manager_test.cpp +++ b/nav2_mppi_controller/test/critic_manager_test.cpp @@ -117,7 +117,7 @@ TEST(CriticManagerTests, BasicCriticOperations) models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; - xt::xtensor costs; + Eigen::ArrayXf costs; float model_dt = 0.1; CriticData data = {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, diff --git a/nav2_mppi_controller/test/models_test.cpp b/nav2_mppi_controller/test/models_test.cpp index 12936d5875..0900ebac05 100644 --- a/nav2_mppi_controller/test/models_test.cpp +++ b/nav2_mppi_controller/test/models_test.cpp @@ -35,9 +35,9 @@ TEST(ModelsTest, ControlSequenceTest) { // populate the object ControlSequence sequence; - sequence.vx = xt::ones({10}); - sequence.vy = xt::ones({10}); - sequence.wz = xt::ones({10}); + sequence.vx = Eigen::ArrayXf::Ones(10); + sequence.vy = Eigen::ArrayXf::Ones(10); + sequence.wz = Eigen::ArrayXf::Ones(10); // Show you can get contents EXPECT_EQ(sequence.vx(4), 1); @@ -50,18 +50,18 @@ TEST(ModelsTest, ControlSequenceTest) EXPECT_EQ(sequence.vx(4), 0); EXPECT_EQ(sequence.vy(4), 0); EXPECT_EQ(sequence.wz(4), 0); - EXPECT_EQ(sequence.vx.shape(0), 20u); - EXPECT_EQ(sequence.vy.shape(0), 20u); - EXPECT_EQ(sequence.wz.shape(0), 20u); + EXPECT_EQ(sequence.vx.rows(), 20); + EXPECT_EQ(sequence.vy.rows(), 20); + EXPECT_EQ(sequence.wz.rows(), 20); } TEST(ModelsTest, PathTest) { // populate the object Path path; - path.x = xt::ones({10}); - path.y = xt::ones({10}); - path.yaws = xt::ones({10}); + path.x = Eigen::ArrayXf::Ones(); + path.y = Eigen::ArrayXf::Ones(); + path.yaws = Eigen::ArrayXf::Ones(); // Show you can get contents EXPECT_EQ(path.x(4), 1); @@ -74,21 +74,21 @@ TEST(ModelsTest, PathTest) EXPECT_EQ(path.x(4), 0); EXPECT_EQ(path.y(4), 0); EXPECT_EQ(path.yaws(4), 0); - EXPECT_EQ(path.x.shape(0), 20u); - EXPECT_EQ(path.y.shape(0), 20u); - EXPECT_EQ(path.yaws.shape(0), 20u); + EXPECT_EQ(path.x.rows(), 20); + EXPECT_EQ(path.y.rows(), 20); + EXPECT_EQ(path.yaws.rows(), 20); } TEST(ModelsTest, StateTest) { // populate the object State state; - state.vx = xt::ones({10, 10}); - state.vy = xt::ones({10, 10}); - state.wz = xt::ones({10, 10}); - state.cvx = xt::ones({10, 10}); - state.cvy = xt::ones({10, 10}); - state.cwz = xt::ones({10, 10}); + state.vx = Eigen::ArrayXXf::Ones(10, 10); + state.vy = Eigen::ArrayXXf::Ones(10, 10); + state.wz = Eigen::ArrayXXf::Ones(10, 10); + state.cvx = Eigen::ArrayXXf::Ones(10, 10); + state.cvy = Eigen::ArrayXXf::Ones(10, 10); + state.cwz = Eigen::ArrayXXf::Ones(10, 10); // Show you can get contents EXPECT_EQ(state.cvx(4), 1); @@ -107,27 +107,27 @@ TEST(ModelsTest, StateTest) EXPECT_EQ(state.vx(4), 0); EXPECT_EQ(state.vy(4), 0); EXPECT_EQ(state.wz(4), 0); - EXPECT_EQ(state.cvx.shape(0), 20u); - EXPECT_EQ(state.cvy.shape(0), 20u); - EXPECT_EQ(state.cwz.shape(0), 20u); - EXPECT_EQ(state.cvx.shape(1), 40u); - EXPECT_EQ(state.cvy.shape(1), 40u); - EXPECT_EQ(state.cwz.shape(1), 40u); - EXPECT_EQ(state.vx.shape(0), 20u); - EXPECT_EQ(state.vy.shape(0), 20u); - EXPECT_EQ(state.wz.shape(0), 20u); - EXPECT_EQ(state.vx.shape(1), 40u); - EXPECT_EQ(state.vy.shape(1), 40u); - EXPECT_EQ(state.wz.shape(1), 40u); + EXPECT_EQ(state.cvx.rows(), 20); + EXPECT_EQ(state.cvy.rows(), 20); + EXPECT_EQ(state.cwz.rows(), 20); + EXPECT_EQ(state.cvx.cols(), 40); + EXPECT_EQ(state.cvy.cols(), 40); + EXPECT_EQ(state.cwz.cols(), 40); + EXPECT_EQ(state.vx.rows(), 20); + EXPECT_EQ(state.vy.rows(), 20); + EXPECT_EQ(state.wz.rows(), 20); + EXPECT_EQ(state.vx.cols(), 40); + EXPECT_EQ(state.vy.cols(), 40); + EXPECT_EQ(state.wz.cols(), 40); } TEST(ModelsTest, TrajectoriesTest) { // populate the object Trajectories trajectories; - trajectories.x = xt::ones({10, 10}); - trajectories.y = xt::ones({10, 10}); - trajectories.yaws = xt::ones({10, 10}); + trajectories.x = Eigen::ArrayXXf::Ones(10, 10); + trajectories.y = Eigen::ArrayXXf::Ones(10, 10); + trajectories.yaws = Eigen::ArrayXXf::Ones(10, 10); // Show you can get contents EXPECT_EQ(trajectories.x(4), 1); @@ -140,10 +140,10 @@ TEST(ModelsTest, TrajectoriesTest) EXPECT_EQ(trajectories.x(4), 0); EXPECT_EQ(trajectories.y(4), 0); EXPECT_EQ(trajectories.yaws(4), 0); - EXPECT_EQ(trajectories.x.shape(0), 20u); - EXPECT_EQ(trajectories.y.shape(0), 20u); - EXPECT_EQ(trajectories.yaws.shape(0), 20u); - EXPECT_EQ(trajectories.x.shape(1), 40u); - EXPECT_EQ(trajectories.y.shape(1), 40u); - EXPECT_EQ(trajectories.yaws.shape(1), 40u); + EXPECT_EQ(trajectories.x.rows(), 20); + EXPECT_EQ(trajectories.y.rows(), 20); + EXPECT_EQ(trajectories.yaws.rows(), 20); + EXPECT_EQ(trajectories.x.cols(), 40); + EXPECT_EQ(trajectories.y.cols(), 40); + EXPECT_EQ(trajectories.yaws.cols(), 40); } diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp index 6085896cfe..469ba3bc26 100644 --- a/nav2_mppi_controller/test/motion_model_tests.cpp +++ b/nav2_mppi_controller/test/motion_model_tests.cpp @@ -45,22 +45,22 @@ TEST(MotionModelTests, DiffDriveTest) std::make_unique(); // Check that predict properly populates the trajectory velocities with the control velocities - state.cvx = 10 * xt::ones({batches, timesteps}); - state.cvy = 5 * xt::ones({batches, timesteps}); - state.cwz = 1 * xt::ones({batches, timesteps}); + state.cvx = 10 * Eigen::ArrayXXf::Ones(batches, timesteps); + state.cvy = 5 * Eigen::ArrayXXf::Ones(batches, timesteps); + state.cwz = 1 * Eigen::ArrayXXf::Ones(batches, timesteps); // Manually set state index 0 from initial conditions which would be the speed of the robot - xt::view(state.vx, xt::all(), 0) = 10; - xt::view(state.wz, xt::all(), 0) = 1; + state.vx.col(0) = 10; + state.wz.col(0) = 1; model->predict(state); EXPECT_EQ(state.vx, state.cvx); - EXPECT_EQ(state.vy, xt::zeros({batches, timesteps})); // non-holonomic + EXPECT_EQ(state.vy.isApprox(Eigen::ArrayXXf::Zero(batches, timesteps))); // non-holonomic EXPECT_EQ(state.wz, state.cwz); // Check that application of constraints are empty for Diff Drive - for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) { + for (unsigned int i = 0; i != control_sequence.vx.rows(); i++) { control_sequence.vx(i) = i * i * i; control_sequence.wz(i) = i * i * i; } @@ -90,14 +90,14 @@ TEST(MotionModelTests, OmniTest) std::make_unique(); // Check that predict properly populates the trajectory velocities with the control velocities - state.cvx = 10 * xt::ones({batches, timesteps}); - state.cvy = 5 * xt::ones({batches, timesteps}); - state.cwz = 1 * xt::ones({batches, timesteps}); + state.cvx = 10 * Eigen::ArrayXXf::Ones(batches, timesteps); + state.cvy = 5 * Eigen::ArrayXXf::Ones(batches, timesteps); + state.cwz = 1 * Eigen::ArrayXXf::Ones(batches, timesteps); // Manually set state index 0 from initial conditions which would be the speed of the robot - xt::view(state.vx, xt::all(), 0) = 10; - xt::view(state.vy, xt::all(), 0) = 5; - xt::view(state.wz, xt::all(), 0) = 1; + state.vx.col(0) = 10; + state.vy.col(0) = 5; + state.wz.col(0) = 1; model->predict(state); @@ -106,7 +106,7 @@ TEST(MotionModelTests, OmniTest) EXPECT_EQ(state.wz, state.cwz); // Check that application of constraints are empty for Omni Drive - for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) { + for (unsigned int i = 0; i != control_sequence.vx.rows(); i++) { control_sequence.vx(i) = i * i * i; control_sequence.vy(i) = i * i * i; control_sequence.wz(i) = i * i * i; @@ -139,22 +139,22 @@ TEST(MotionModelTests, AckermannTest) std::make_unique(¶m_handler); // Check that predict properly populates the trajectory velocities with the control velocities - state.cvx = 10 * xt::ones({batches, timesteps}); - state.cvy = 5 * xt::ones({batches, timesteps}); - state.cwz = 1 * xt::ones({batches, timesteps}); + state.cvx = 10 * Eigen::ArrayXXf::Ones(batches, timesteps); + state.cvy = 5 * Eigen::ArrayXXf::Ones(batches, timesteps); + state.cwz = 1 * Eigen::ArrayXXf::Ones(batches, timesteps); // Manually set state index 0 from initial conditions which would be the speed of the robot - xt::view(state.vx, xt::all(), 0) = 10; - xt::view(state.wz, xt::all(), 0) = 1; + state.vx.col(0) = 10; + state.wz.col(0) = 1; model->predict(state); EXPECT_EQ(state.vx, state.cvx); - EXPECT_EQ(state.vy, xt::zeros({batches, timesteps})); // non-holonomic + EXPECT_EQ(state.vy.isApprox(Eigen::ArrayXXf::Zero(batches, timesteps))); // non-holonomic EXPECT_EQ(state.wz, state.cwz); // Check that application of constraints are non-empty for Ackermann Drive - for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) { + for (unsigned int i = 0; i != control_sequence.vx.rows(); i++) { control_sequence.vx(i) = i * i * i; control_sequence.wz(i) = i * i * i * i; } @@ -167,7 +167,7 @@ TEST(MotionModelTests, AckermannTest) // Now, check the specifics of the minimum curvature constraint EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6); - for (unsigned int i = 1; i != control_sequence.vx.shape(0); i++) { + for (unsigned int i = 1; i != control_sequence.vx.rows(); i++) { EXPECT_TRUE(fabs(control_sequence.vx(i)) / fabs(control_sequence.wz(i)) >= 0.2); } diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp index 485cc375b0..0e3735c654 100644 --- a/nav2_mppi_controller/test/noise_generator_test.cpp +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -70,7 +70,7 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain) // Populate a potential control sequence mppi::models::ControlSequence control_sequence; control_sequence.reset(25); - for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) { + for (unsigned int i = 0; i != control_sequence.vx.rows(); i++) { control_sequence.vx(i) = i; control_sequence.vy(i) = i; control_sequence.wz(i) = i; diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index c1b5f56799..2d03d184cb 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -32,7 +32,6 @@ RosLockGuard g_rclcpp; using namespace mppi; // NOLINT using namespace mppi::critics; // NOLINT using namespace mppi::utils; // NOLINT -using xt::evaluation_strategy::immediate; class OptimizerTester : public Optimizer { @@ -96,23 +95,23 @@ class OptimizerTester : public Optimizer void fillOptimizerWithGarbage() { - state_.vx = 0.43432 * xt::ones({1000, 10}); - control_sequence_.vx = 342.0 * xt::ones({30}); + state_.vx = 0.43432 * Eigen::ArrayXXf::Ones(1000, 10); + control_sequence_.vx = 342.0 * Eigen::ArrayXf::Ones(30); control_history_[0] = {43, 5646, 32432}; - costs_ = 5.32 * xt::ones({56453}); - generated_trajectories_.x = 432.234 * xt::ones({7865, 1}); + costs_ = 5.32 * Eigen::ArrayXf::Ones(56453); + generated_trajectories_.x = 432.234 * Eigen::ArrayXf::Ones(7865); } void testReset() { reset(); - EXPECT_EQ(state_.vx, xt::zeros({1000, 50})); - EXPECT_EQ(control_sequence_.vx, xt::zeros({50})); + EXPECT_TRUE(state_.vx.isApprox(Eigen::ArrayXXf::Zero(1000, 50))); + EXPECT_TRUE(control_sequence_.vx.isApprox(Eigen::ArrayXf::Zero(50))); EXPECT_EQ(control_history_[0].vx, 0.0); EXPECT_EQ(control_history_[0].vy, 0.0); - EXPECT_NEAR(xt::sum(costs_, immediate)(), 0, 1e-6); - EXPECT_EQ(generated_trajectories_.x, xt::zeros({1000, 50})); + EXPECT_NEAR(costs_.sum(), 0, 1e-6); + EXPECT_TRUE(generated_trajectories_.x.isApprox(Eigen::ArrayXXf::Zero(1000, 50))); } bool fallbackWrapper(bool fail) @@ -128,14 +127,14 @@ class OptimizerTester : public Optimizer prepare(robot_pose, robot_speed, plan, goal_checker); EXPECT_EQ(critics_data_.goal_checker, nullptr); - EXPECT_NEAR(xt::sum(costs_, immediate)(), 0, 1e-6); // should be reset + EXPECT_NEAR(costs_.sum(), 0, 1e-6); // should be reset EXPECT_FALSE(critics_data_.fail_flag); // should be reset EXPECT_FALSE(critics_data_.motion_model->isHolonomic()); // object is valid + diff drive EXPECT_FALSE(critics_data_.furthest_reached_path_point.has_value()); // val is not set EXPECT_FALSE(critics_data_.path_pts_valid.has_value()); // val is not set EXPECT_EQ(state_.pose.pose.position.x, 999); EXPECT_EQ(state_.speed.linear.y, 4.0); - EXPECT_EQ(path_.x.shape(0), 17u); + EXPECT_EQ(path_.x.rows(), 17); } void shiftControlSequenceWrapper() @@ -172,9 +171,9 @@ class OptimizerTester : public Optimizer state.speed.linear.x = 5.0; state.speed.linear.y = 1.0; state.speed.angular.z = 6.0; - state.cvx = 0.75 * xt::ones({1000, 50}); - state.cvy = 0.5 * xt::ones({1000, 50}); - state.cwz = 0.1 * xt::ones({1000, 50}); + state.cvx = 0.75 * Eigen::ArrayXXf::Ones(1000, 50); + state.cvy = 0.5 * Eigen::ArrayXXf::Ones(1000, 50); + state.cwz = 0.1 * Eigen::ArrayXXf::Ones(1000, 50); updateInitialStateVelocities(state); EXPECT_NEAR(state.vx(0, 0), 5.0, 1e-6); EXPECT_NEAR(state.vy(0, 0), 1.0, 1e-6); @@ -198,9 +197,9 @@ class OptimizerTester : public Optimizer state.speed.linear.x = -5.0; state.speed.linear.y = -1.0; state.speed.angular.z = -6.0; - state.cvx = -0.75 * xt::ones({1000, 50}); - state.cvy = -0.5 * xt::ones({1000, 50}); - state.cwz = -0.1 * xt::ones({1000, 50}); + state.cvx = -0.75 * Eigen::ArrayXXf::Ones(1000, 50); + state.cvy = -0.5 * Eigen::ArrayXXf::Ones(1000, 50); + state.cwz = -0.1 * Eigen::ArrayXXf::Ones(1000, 50); updateStateVelocities(state); EXPECT_NEAR(state.vx(0, 0), -5.0, 1e-6); EXPECT_NEAR(state.vy(0, 0), -1.0, 1e-6); @@ -246,9 +245,9 @@ TEST(OptimizerTests, BasicInitializedFunctions) // Should be empty of size batches x time steps // and tests getting set params: time_steps, batch_size, controller_frequency auto trajs = optimizer_tester.getGeneratedTrajectories(); - EXPECT_EQ(trajs.x.shape(0), 1000u); - EXPECT_EQ(trajs.x.shape(1), 50u); - EXPECT_EQ(trajs.x, xt::zeros({1000, 50})); + EXPECT_EQ(trajs.x.rows(), 1000); + EXPECT_EQ(trajs.x.cols(), 50); + EXPECT_TRUE(trajs.x.isApprox(Eigen::ArrayXXf::Zero(1000, 50))); optimizer_tester.resetMotionModel(); optimizer_tester.testSetOmniModel(); @@ -256,8 +255,8 @@ TEST(OptimizerTests, BasicInitializedFunctions) EXPECT_EQ(traj(5, 0), 0.0); // x EXPECT_EQ(traj(5, 1), 0.0); // y EXPECT_EQ(traj(5, 2), 0.0); // yaw - EXPECT_EQ(traj.shape(0), 50u); - EXPECT_EQ(traj.shape(1), 3u); + EXPECT_EQ(traj.rows(), 50); + EXPECT_EQ(traj.cols(), 3); optimizer_tester.reset(); optimizer_tester.shutdown(); @@ -402,7 +401,7 @@ TEST(OptimizerTests, shiftControlSequenceTests) optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); // Test shiftControlSequence by setting the 2nd value to something unique to neighbors - auto & sequence = optimizer_tester.grabControlSequence(); + auto sequence = optimizer_tester.grabControlSequence(); sequence.reset(100); sequence.vx(0) = 9999; sequence.vx(1) = 6; @@ -492,34 +491,34 @@ TEST(OptimizerTests, applyControlSequenceConstraintsTests) // in motion_models_test.cpp optimizer_tester.resetMotionModel(); optimizer_tester.testSetOmniModel(); - auto & sequence = optimizer_tester.grabControlSequence(); + auto sequence = optimizer_tester.grabControlSequence(); // Test boundary of limits - sequence.vx = xt::ones({50}); - sequence.vy = 0.75 * xt::ones({50}); - sequence.wz = 2.0 * xt::ones({50}); + sequence.vx = Eigen::ArrayXf::Ones(50); + sequence.vy = 0.75 * Eigen::ArrayXf::Ones(50); + sequence.wz = 2.0 * Eigen::ArrayXf::Ones(50); optimizer_tester.applyControlSequenceConstraintsWrapper(); - EXPECT_EQ(sequence.vx, xt::ones({50})); - EXPECT_EQ(sequence.vy, 0.75 * xt::ones({50})); - EXPECT_EQ(sequence.wz, 2.0 * xt::ones({50})); + EXPECT_TRUE(sequence.vx.isApprox(Eigen::ArrayXf::Ones(50))); + EXPECT_TRUE(sequence.vy.isApprox(0.75 * Eigen::ArrayXf::Ones(50))); + EXPECT_TRUE(sequence.wz.isApprox(2.0 * Eigen::ArrayXf::Ones(50))); // Test breaking limits sets to maximum - sequence.vx = 5.0 * xt::ones({50}); - sequence.vy = 5.0 * xt::ones({50}); - sequence.wz = 5.0 * xt::ones({50}); + sequence.vx = 5.0 * Eigen::ArrayXf::Ones(50); + sequence.vy = 5.0 * Eigen::ArrayXf::Ones(50); + sequence.wz = 5.0 * Eigen::ArrayXf::Ones(50); optimizer_tester.applyControlSequenceConstraintsWrapper(); - EXPECT_EQ(sequence.vx, xt::ones({50})); - EXPECT_EQ(sequence.vy, 0.75 * xt::ones({50})); - EXPECT_EQ(sequence.wz, 2.0 * xt::ones({50})); + EXPECT_TRUE(sequence.vx.isApprox(Eigen::ArrayXf::Ones(50))); + EXPECT_TRUE(sequence.vy.isApprox(0.75 * Eigen::ArrayXf::Ones(50))); + EXPECT_TRUE(sequence.wz.isApprox(2.0 * Eigen::ArrayXf::Ones(50))); // Test breaking limits sets to minimum - sequence.vx = -5.0 * xt::ones({50}); - sequence.vy = -5.0 * xt::ones({50}); - sequence.wz = -5.0 * xt::ones({50}); + sequence.vx = -5.0 * Eigen::ArrayXf::Ones(50); + sequence.vy = -5.0 * Eigen::ArrayXf::Ones(50); + sequence.wz = -5.0 * Eigen::ArrayXf::Ones(50); optimizer_tester.applyControlSequenceConstraintsWrapper(); - EXPECT_EQ(sequence.vx, -1.0 * xt::ones({50})); - EXPECT_EQ(sequence.vy, -0.75 * xt::ones({50})); - EXPECT_EQ(sequence.wz, -2.0 * xt::ones({50})); + EXPECT_TRUE(sequence.vx.isApprox(-1.0 * Eigen::ArrayXf::Ones(50))); + EXPECT_TRUE(sequence.vy.isApprox(-0.75 * Eigen::ArrayXf::Ones(50))); + EXPECT_TRUE(sequence.wz.isApprox(-2.0 * Eigen::ArrayXf::Ones(50))); } TEST(OptimizerTests, updateStateVelocitiesTests) @@ -571,9 +570,9 @@ TEST(OptimizerTests, getControlFromSequenceAsTwistTests) // Test conversion of control sequence into a Twist command to execute auto & sequence = optimizer_tester.grabControlSequence(); - sequence.vx = 0.25 * xt::ones({10}); - sequence.vy = 0.5 * xt::ones({10}); - sequence.wz = 0.1 * xt::ones({10}); + sequence.vx = 0.25 * Eigen::ArrayXf::Ones(10); + sequence.vy = 0.5 * Eigen::ArrayXf::Ones(10); + sequence.wz = 0.1 * Eigen::ArrayXf::Ones(10); auto diff_t = optimizer_tester.getControlFromSequenceAsTwistWrapper(); EXPECT_NEAR(diff_t.twist.linear.x, 0.25, 1e-6); @@ -612,38 +611,38 @@ TEST(OptimizerTests, integrateStateVelocitiesTests) models::State state; state.reset(1000, 50); models::Trajectories traj; - state.vx = 0.1 * xt::ones({1000, 50}); - xt::view(state.vx, xt::all(), 0) = xt::zeros({1000}); - state.vy = xt::zeros({1000, 50}); - state.wz = xt::zeros({1000, 50}); + state.vx = 0.1 * Eigen::ArrayXXf::Ones(1000, 50); + state.vx.col(0) = Eigen::ArrayXf::Zero(1000); + state.vy = Eigen::ArrayXXf::Zero(1000, 50); + state.wz = Eigen::ArrayXXf::Zero(1000, 50); optimizer_tester.integrateStateVelocitiesWrapper(traj, state); - EXPECT_EQ(traj.y, xt::zeros({1000, 50})); - EXPECT_EQ(traj.yaws, xt::zeros({1000, 50})); - for (unsigned int i = 0; i != traj.x.shape(1); i++) { + EXPECT_TRUE(traj.y.isApprox(Eigen::ArrayXXf::Zero(1000, 50))); + EXPECT_TRUE(traj.yaws.isApprox(Eigen::ArrayXXf::Zero(1000, 50))); + for (unsigned int i = 0; i != traj.x.cols(); i++) { EXPECT_NEAR(traj.x(1, i), i * 0.1 /*vel*/ * 0.1 /*dt*/, 1e-3); } // Give it a bit of a more complex trajectory to crunch - state.vy = 0.2 * xt::ones({1000, 50}); - xt::view(state.vy, xt::all(), 0) = xt::zeros({1000}); + state.vy = 0.2 * Eigen::ArrayXXf::Ones(1000, 50); + state.vy.col(0) = Eigen::ArrayXf::Zero(1000); optimizer_tester.integrateStateVelocitiesWrapper(traj, state); - EXPECT_EQ(traj.yaws, xt::zeros({1000, 50})); - for (unsigned int i = 0; i != traj.x.shape(1); i++) { + EXPECT_TRUE(traj.yaws.isApprox(Eigen::ArrayXXf::Zero(1000, 50))); + for (unsigned int i = 0; i != traj.x.cols(); i++) { EXPECT_NEAR(traj.x(1, i), i * 0.1 /*vel*/ * 0.1 /*dt*/, 1e-3); EXPECT_NEAR(traj.y(1, i), i * 0.2 /*vel*/ * 0.1 /*dt*/, 1e-3); } // Lets add some angular motion to the mix - state.vy = xt::zeros({1000, 50}); - state.wz = 0.2 * xt::ones({1000, 50}); - xt::view(state.wz, xt::all(), 0) = xt::zeros({1000}); + state.vy = Eigen::ArrayXXf::Zero(1000, 50); + state.wz = 0.2 * Eigen::ArrayXXf::Ones(1000, 50); + state.wz.col(0) = Eigen::ArrayXf::Zero(1000); optimizer_tester.integrateStateVelocitiesWrapper(traj, state); float x = 0; float y = 0; - for (unsigned int i = 1; i != traj.x.shape(1); i++) { + for (unsigned int i = 1; i != traj.x.cols(); i++) { std::cout << i << std::endl; x += (0.1 /*vx*/ * cosf(0.2 /*wz*/ * 0.1 /*model_dt*/ * (i - 1))) * 0.1 /*model_dt*/; y += (0.1 /*vx*/ * sinf(0.2 /*wz*/ * 0.1 /*model_dt*/ * (i - 1))) * 0.1 /*model_dt*/; diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index 7ebada2a6a..77b7b8d294 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -77,7 +77,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) [&](const visualization_msgs::msg::MarkerArray msg) {recieved_msg = msg;}); // optimal_trajectory empty, should fail to publish - xt::xtensor optimal_trajectory; + Eigen::ArrayXXf optimal_trajectory; TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); vis.on_activate(); @@ -89,7 +89,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) EXPECT_EQ(recieved_msg.markers.size(), 0u); // Now populated with content, should publish - optimal_trajectory = xt::ones({20, 2}); + optimal_trajectory = Eigen::ArrayXXf::Ones(20,2); vis.add(optimal_trajectory, "Optimal Trajectory"); vis.visualize(bogus_path); @@ -138,9 +138,9 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) [&](const visualization_msgs::msg::MarkerArray msg) {recieved_msg = msg;}); models::Trajectories candidate_trajectories; - candidate_trajectories.x = xt::ones({200, 12}); - candidate_trajectories.y = xt::ones({200, 12}); - candidate_trajectories.yaws = xt::ones({200, 12}); + candidate_trajectories.x = Eigen::ArrayXXf::Ones(200, 12); + candidate_trajectories.y = Eigen::ArrayXXf::Ones(200, 12); + candidate_trajectories.yaws = Eigen::ArrayXXf::Ones(200, 12); TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); From 93d017b94c53d0f4fdcf5762e00f24be4e00b835 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Tue, 3 Sep 2024 23:13:19 +0530 Subject: [PATCH 14/21] Complete conversion from xtensor to Eigen Signed-off-by: Ayush1285 --- nav2_mppi_controller/CMakeLists.txt | 16 +- .../tools/trajectory_visualizer.hpp | 9 +- .../nav2_mppi_controller/tools/utils.hpp | 22 +-- nav2_mppi_controller/package.xml | 2 - .../src/critics/path_angle_critic.cpp | 2 +- .../src/trajectory_visualizer.cpp | 14 +- nav2_mppi_controller/test/CMakeLists.txt | 5 +- nav2_mppi_controller/test/critics_tests.cpp | 145 +++++++++--------- nav2_mppi_controller/test/models_test.cpp | 6 +- .../test/motion_model_tests.cpp | 34 ++-- nav2_mppi_controller/test/utils/utils.hpp | 6 +- 11 files changed, 123 insertions(+), 138 deletions(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 6a9942f9d7..c481100bab 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -1,19 +1,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_mppi_controller) -add_definitions(-DXTENSOR_ENABLE_XSIMD) -add_definitions(-DXTENSOR_USE_XSIMD) - -set(XTENSOR_USE_TBB 0) -set(XTENSOR_USE_OPENMP 0) -set(XTENSOR_USE_XSIMD 1) - -# set(XTENSOR_DEFAULT_LAYOUT column_major) # row_major, column_major -# set(XTENSOR_DEFAULT_TRAVERSAL row_major) # row_major, column_major - find_package(ament_cmake REQUIRED) -find_package(xsimd REQUIRED) -find_package(xtensor REQUIRED) find_package(Eigen3 REQUIRED) include_directories( @@ -97,8 +85,8 @@ set(libraries mppi_controller mppi_critics) foreach(lib IN LISTS libraries) target_compile_options(${lib} PUBLIC -fconcepts) - target_include_directories(${lib} PUBLIC ${xsimd_INCLUDE_DIRS}) # ${OpenMP_INCLUDE_DIRS} - target_link_libraries(${lib} xtensor xtensor::optimize xtensor::use_xsimd) + target_include_directories(${lib} PUBLIC) # ${OpenMP_INCLUDE_DIRS} + target_link_libraries(${lib}) ament_target_dependencies(${lib} ${dependencies_pkgs}) endforeach() diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp index d9cdc95ce9..4a02a308fb 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp @@ -18,12 +18,7 @@ #include #include -// xtensor creates warnings that needs to be ignored as we are building with -Werror -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Warray-bounds" -#pragma GCC diagnostic ignored "-Wstringop-overflow" -#include -#pragma GCC diagnostic pop +#include #include "nav_msgs/msg/path.hpp" #include "rclcpp/rclcpp.hpp" @@ -79,7 +74,7 @@ class TrajectoryVisualizer * @brief Add an optimal trajectory to visualize * @param trajectory Optimal trajectory */ - void add(const xt::xtensor & trajectory, const std::string & marker_namespace); + void add(const Eigen::ArrayXXf & trajectory, const std::string & marker_namespace); /** * @brief Add candidate trajectories to visualize diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 5617980673..949b3c208b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -708,30 +708,34 @@ auto rollColumns(T&& e, std::ptrdiff_t shift) inline auto point_corrected_yaws(const Eigen::ArrayXf & yaws, const Eigen::ArrayXf & yaws_between_points) { - const auto yaws_ptr = yaws.data(); - const auto yaws_between_points_ptr = yaws_between_points.data(); int size = yaws.size(); Eigen::ArrayXf yaws_between_points_corrected(size); - auto yaws_between_points_corrected_ptr = yaws_between_points_corrected.data(); for(int i = 0; i != size; i++) { - const float & yaw_between_points = *(yaws_between_points_ptr + i); - *(yaws_between_points_corrected_ptr + i) = *(yaws_ptr + i) < M_PIF_2 ? yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF); + const float & yaw_between_points = yaws_between_points[i]; + yaws_between_points_corrected[i] = yaws[i] < M_PIF_2 ? yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF); } + + // binaryExpr slower than for loop + // Eigen::ArrayXf yaws_between_points_corrected = yaws.binaryExpr(yaws_between_points, [&](const float & yaw, const float & yaw_between_points) + // {return yaw < M_PIF_2 ? yaw_between_points : normalize_anglef(yaw_between_points + M_PIF);}); return yaws_between_points_corrected; } inline auto point_corrected_yaws(const Eigen::ArrayXf & yaws_between_points, const float & goal_yaw) { - const auto yaws_between_points_ptr = yaws_between_points.data(); int size = yaws_between_points.size(); Eigen::ArrayXf yaws_between_points_corrected(size); - auto yaws_between_points_corrected_ptr = yaws_between_points_corrected.data(); for(int i = 0; i != size; i++) { - const float & yaw_between_points = *(yaws_between_points_ptr + i); - *(yaws_between_points_corrected_ptr + i) = fabs(angles::normalize_angle(yaw_between_points - goal_yaw)) < M_PIF_2 ? yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF); + const float & yaw_between_points = yaws_between_points[i]; + yaws_between_points_corrected[i] = fabs(angles::normalize_angle(yaw_between_points - goal_yaw)) < M_PIF_2 ? + yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF); } + + // unaryExpr slower than for loop + // Eigen::ArrayXf yaws_between_points_corrected = yaws_between_points.unaryExpr([&](const float & yaw_between_points) + // {return fabs(normalize_anglef(yaw_between_points - goal_yaw)) < M_PIF_2 ? yaw_between_points : normalize_anglef(yaw_between_points + M_PIF);}); return yaws_between_points_corrected; } diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 77e79672ca..fae7dd53c7 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -25,10 +25,8 @@ tf2_eigen tf2_ros std_msgs - xtensor libomp-dev benchmark - xsimd eigen3_cmake_module eigen diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index dbdc666b6b..7d75f409dc 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -148,4 +148,4 @@ void PathAngleCritic::score(CriticData & data) PLUGINLIB_EXPORT_CLASS( mppi::critics::PathAngleCritic, - mppi::critics::CriticFunction) + mppi::critics::CriticFunction) \ No newline at end of file diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index a9e51663c3..885eb06f89 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -59,7 +59,7 @@ void TrajectoryVisualizer::on_deactivate() void TrajectoryVisualizer::add( const Eigen::ArrayXXf & trajectory, const std::string & marker_namespace) { - auto & size = trajectory.rows(); + size_t size = trajectory.rows(); if (!size) { return; } @@ -78,7 +78,7 @@ void TrajectoryVisualizer::add( points_->markers.push_back(marker); }; - for (size_t i = 0; i < size; i++) { + for (size_t i = 0; i != size; i++) { add_marker(i); } } @@ -86,13 +86,13 @@ void TrajectoryVisualizer::add( void TrajectoryVisualizer::add( const models::Trajectories & trajectories, const std::string & marker_namespace) { - int n_rows = trajectories.x.rows(); - int n_cols = trajectories.x.cols(); + size_t n_rows = trajectories.x.rows(); + size_t n_cols = trajectories.x.cols(); const float shape_1 = static_cast(n_cols); points_->markers.reserve(floor(n_rows / trajectory_step_) * floor(n_cols * time_step_)); - for (size_t i = 0; i < n_rows; i += trajectory_step_) { - for (size_t j = 0; j < n_cols; j += time_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); float blue_component = 1.0f - j_flt / shape_1; float green_component = j_flt / shape_1; @@ -128,4 +128,4 @@ void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan) } } -} // namespace mppi +} // namespace mppi \ No newline at end of file diff --git a/nav2_mppi_controller/test/CMakeLists.txt b/nav2_mppi_controller/test/CMakeLists.txt index 0c813400c4..ec6720a312 100644 --- a/nav2_mppi_controller/test/CMakeLists.txt +++ b/nav2_mppi_controller/test/CMakeLists.txt @@ -12,6 +12,7 @@ set(TEST_NAMES optimizer_unit_tests ) + foreach(name IN LISTS TEST_NAMES) ament_add_gtest(${name} ${name}.cpp @@ -31,10 +32,10 @@ foreach(name IN LISTS TEST_NAMES) endforeach() -This is a special case requiring linking against the critics library +# This is a special case requiring linking against the critics library ament_add_gtest(critics_tests critics_tests.cpp) ament_target_dependencies(critics_tests ${dependencies_pkgs}) target_link_libraries(critics_tests mppi_controller mppi_critics) if(${TEST_DEBUG_INFO}) target_compile_definitions(critics_tests PUBLIC -DTEST_DEBUG_INFO) -endif() +endif() \ No newline at end of file diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index ef85ab293d..c3b7908acf 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -40,7 +40,6 @@ using namespace mppi; // NOLINT using namespace mppi::critics; // NOLINT using namespace mppi::utils; // NOLINT -using xt::evaluation_strategy::immediate; class PathAngleCriticWrapper : public PathAngleCritic { @@ -70,7 +69,7 @@ TEST(CriticTests, ConstraintsCritic) models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; - xt::xtensor costs = xt::zeros({1000}); + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, @@ -89,41 +88,41 @@ TEST(CriticTests, ConstraintsCritic) // Scoring testing // provide velocities in constraints, should not have any costs - state.vx = 0.40 * xt::ones({1000, 30}); - state.vy = xt::zeros({1000, 30}); - state.wz = xt::ones({1000, 30}); + state.vx = 0.40 * Eigen::ArrayXXf::Ones(1000, 30); + state.vy = Eigen::ArrayXXf::Zero(1000, 30); + state.wz = Eigen::ArrayXXf::Ones(1000, 30); critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); + EXPECT_NEAR(costs.sum(), 0, 1e-6); // provide out of maximum velocity constraint - auto last_batch_traj_in_full = xt::view(state.vx, -1, xt::all()); - last_batch_traj_in_full = 0.60 * xt::ones({30}); + auto last_batch_traj_in_full = state.vx.row(-1); + last_batch_traj_in_full = 0.60 * Eigen::ArrayXf::Ones(30); critic.score(data); - EXPECT_GT(xt::sum(costs, immediate)(), 0); + EXPECT_GT(costs.sum(), 0); // 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2 EXPECT_NEAR(costs(999), 1.2, 0.01); - costs = xt::zeros({1000}); + costs = Eigen::ArrayXf::Zero(1000); // provide out of minimum velocity constraint - auto first_batch_traj_in_full = xt::view(state.vx, 1, xt::all()); - first_batch_traj_in_full = -0.45 * xt::ones({30}); + auto first_batch_traj_in_full = state.vx.row(0); + first_batch_traj_in_full = -0.45 * Eigen::ArrayXf::Ones(30);; critic.score(data); - EXPECT_GT(xt::sum(costs, immediate)(), 0); + EXPECT_GT(costs.sum(), 0); // 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2 EXPECT_NEAR(costs(1), 1.2, 0.01); - costs = xt::zeros({1000}); + costs = Eigen::ArrayXf::Zero(1000); // Now with ackermann, all in constraint so no costs to score - state.vx = 0.40 * xt::ones({1000, 30}); - state.wz = 1.5 * xt::ones({1000, 30}); + state.vx = 0.40 * Eigen::ArrayXXf::Ones(1000, 30); + state.wz = 1.5 * Eigen::ArrayXXf::Ones(1000, 30); data.motion_model = std::make_shared(¶m_handler); critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); + EXPECT_NEAR(costs.sum(), 0, 1e-6); // Now violating the ackermann constraints - state.wz = 2.5 * xt::ones({1000, 30}); + state.wz = 2.5 * Eigen::ArrayXXf::Ones(1000, 30); critic.score(data); - EXPECT_GT(xt::sum(costs, immediate)(), 0); + EXPECT_GT(costs.sum(), 0); // 4.0 weight * 0.1 model_dt * (0.2 - 0.4/2.5) * 30 timesteps = 0.48 EXPECT_NEAR(costs(1), 0.48, 0.01); } @@ -143,7 +142,7 @@ TEST(CriticTests, GoalAngleCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; - xt::xtensor costs = xt::zeros({1000}); + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, @@ -166,17 +165,17 @@ TEST(CriticTests, GoalAngleCritic) path.y(9) = 0.0; path.yaws(9) = 3.14; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); + EXPECT_NEAR(costs.sum(), 0, 1e-6); // Lets move it even closer, just to be sure it still doesn't trigger state.pose.pose.position.x = 9.2; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); + EXPECT_NEAR(costs.sum(), 0, 1e-6); // provide state pose and path below `threshold_to_consider` to consider state.pose.pose.position.x = 9.7; critic.score(data); - EXPECT_GT(xt::sum(costs, immediate)(), 0); + EXPECT_GT(costs.sum(), 0); EXPECT_NEAR(costs(0), 9.42, 0.02); // (3.14 - 0.0) * 3.0 weight } @@ -195,7 +194,7 @@ TEST(CriticTests, GoalCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; - xt::xtensor costs = xt::zeros({1000}); + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, @@ -218,15 +217,15 @@ TEST(CriticTests, GoalCritic) path.y(9) = 0.0; critic.score(data); EXPECT_NEAR(costs(2), 0.0, 1e-6); // (0 * 5.0 weight - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); // Should all be 0 * 1000 - costs = xt::zeros({1000}); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // Should all be 0 * 1000 + costs = Eigen::ArrayXf::Zero(1000); // provide state pose and path close path.x(9) = 0.5; path.y(9) = 0.0; critic.score(data); EXPECT_NEAR(costs(2), 2.5, 1e-6); // (sqrt(10.0 * 10.0) * 5.0 weight - EXPECT_NEAR(xt::sum(costs, immediate)(), 2500.0, 1e-6); // should be 2.5 * 1000 + EXPECT_NEAR(costs.sum(), 2500.0, 1e-6); // should be 2.5 * 1000 } TEST(CriticTests, PathAngleCritic) @@ -245,7 +244,7 @@ TEST(CriticTests, PathAngleCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; - xt::xtensor costs = xt::zeros({1000}); + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, @@ -268,7 +267,7 @@ TEST(CriticTests, PathAngleCritic) path.reset(10); path.x(9) = 0.15; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path close but outside of tol. with less than PI/2 angular diff. path.x(9) = 0.95; @@ -276,60 +275,60 @@ TEST(CriticTests, PathAngleCritic) path.x(6) = 1.0; // angle between path point and pose = 0 < max_angle_to_furthest_ path.y(6) = 0.0; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path close but outside of tol. with more than PI/2 angular diff. path.x(6) = -1.0; // angle between path point and pose > max_angle_to_furthest_ path.y(6) = 4.0; critic.score(data); - EXPECT_GT(xt::sum(costs, immediate)(), 0.0); + EXPECT_GT(costs.sum(), 0.0); EXPECT_NEAR(costs(0), 3.9947, 1e-2); // atan2(4,-1) [1.81] * 2.2 weight // Set mode to no directional preferences + reset costs critic.setMode(1); - costs = xt::zeros({1000}); + costs = Eigen::ArrayXf::Zero(1000); // provide state pose and path close but outside of tol. with more than PI/2 angular diff. path.x(6) = 1.0; // angle between path point and pose < max_angle_to_furthest_ path.y(6) = 0.0; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path close but outside of tol. with more than PI/2 angular diff. path.x(6) = -1.0; // angle between path pt and pose < max_angle_to_furthest_ IF non-directional path.y(6) = 0.0; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path close but outside of tol. with more than PI/2 angular diff. path.x(6) = -1.0; // angle between path point and pose < max_angle_to_furthest_ path.y(6) = 4.0; critic.score(data); - EXPECT_GT(xt::sum(costs, immediate)(), 0.0); + EXPECT_GT(costs.sum(), 0.0); // should use reverse orientation as the closer angle in no dir preference mode EXPECT_NEAR(costs(0), 2.9167, 1e-2); // Set mode to consider path directionality + reset costs critic.setMode(2); - costs = xt::zeros({1000}); + costs = Eigen::ArrayXf::Zero(1000); // provide state pose and path close but outside of tol. with more than PI/2 angular diff. path.x(6) = 1.0; // angle between path point and pose < max_angle_to_furthest_ path.y(6) = 0.0; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path close but outside of tol. with more than PI/2 angular diff. path.x(6) = -1.0; // angle between path pt and pose < max_angle_to_furthest_ IF non-directional path.y(6) = 0.0; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path close but outside of tol. with more than PI/2 angular diff. path.x(6) = -1.0; // angle between path point and pose < max_angle_to_furthest_ path.y(6) = 4.0; critic.score(data); - EXPECT_GT(xt::sum(costs, immediate)(), 0.0); + EXPECT_GT(costs.sum(), 0.0); // should use reverse orientation as the closer angle in no dir preference mode EXPECT_NEAR(costs(0), 2.9167, 1e-2); @@ -360,7 +359,7 @@ TEST(CriticTests, PreferForwardCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; - xt::xtensor costs = xt::zeros({1000}); + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, @@ -382,18 +381,18 @@ TEST(CriticTests, PreferForwardCritic) path.reset(10); path.x(9) = 10.0; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0f, 1e-6f); + EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f); // provide state pose and path close to trigger behavior but with all forward motion path.x(9) = 0.15; - state.vx = xt::ones({1000, 30}); + state.vx = Eigen::ArrayXXf::Ones(1000, 30); critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0f, 1e-6f); + EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f); // provide state pose and path close to trigger behavior but with all reverse motion - state.vx = -1.0 * xt::ones({1000, 30}); + state.vx = -1.0 * Eigen::ArrayXXf::Ones(1000, 30); critic.score(data); - EXPECT_GT(xt::sum(costs, immediate)(), 0.0f); + EXPECT_GT(costs.sum(), 0.0f); EXPECT_NEAR(costs(0), 15.0f, 1e-3f); // 1.0 * 0.1 model_dt * 5.0 weight * 30 length } @@ -413,7 +412,7 @@ TEST(CriticTests, TwirlingCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; - xt::xtensor costs = xt::zeros({1000}); + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, @@ -436,23 +435,23 @@ TEST(CriticTests, TwirlingCritic) path.reset(10); path.x(9) = 10.0; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path close to trigger behavior but with no angular variation path.x(9) = 0.15; - state.wz = xt::zeros({1000, 30}); + state.wz = Eigen::ArrayXXf::Zero(1000, 30); critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // Provide nearby with some motion - auto traj_view = xt::view(state.wz, 0, xt::all()); + auto traj_view = state.wz.row(0); traj_view = 10.0; critic.score(data); EXPECT_NEAR(costs(0), 100.0, 1e-6); // (mean(10.0) * 10.0 weight - costs = xt::zeros({1000}); + costs = Eigen::ArrayXf::Zero(1000); // Now try again with some wiggling noise - traj_view = xt::random::randn({30}, 0.0, 0.5); + traj_view = Eigen::ArrayXf::Random(30).abs()/2.0f; critic.score(data); EXPECT_NEAR(costs(0), 3.3, 4e-1); // (mean of noise with mu=0, sigma=0.5 * 10.0 weight } @@ -473,7 +472,7 @@ TEST(CriticTests, PathFollowCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; - xt::xtensor costs = xt::zeros({1000}); + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, @@ -496,13 +495,13 @@ TEST(CriticTests, PathFollowCritic) path.reset(6); path.x(5) = 1.7; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path far enough to enable // pose differential is (0, 0) and (0.15, 0) path.x(5) = 0.15; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 750.0, 1e-2); // 0.15 * 5 weight * 1000 + EXPECT_NEAR(costs.sum(), 750.0, 1e-2); // 0.15 * 5 weight * 1000 } TEST(CriticTests, PathAlignCritic) @@ -521,7 +520,7 @@ TEST(CriticTests, PathAlignCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; - xt::xtensor costs = xt::zeros({1000}); + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, @@ -544,20 +543,20 @@ TEST(CriticTests, PathAlignCritic) path.reset(10); path.x(9) = 0.85; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path far enough to enable // but data furthest point reached is 0 and offset default is 20, so returns path.x(9) = 0.15; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path far enough to enable, with data to pass condition // but with empty trajectories and paths, should still be zero *data.furthest_reached_path_point = 21; path.x(9) = 0.15; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path far enough to enable, with data to pass condition // and with a valid path to pass invalid path condition @@ -586,10 +585,10 @@ TEST(CriticTests, PathAlignCritic) path.x(19) = 0.9; path.x(20) = 0.9; path.x(21) = 0.9; - generated_trajectories.x = 0.66 * xt::ones({1000, 30}); + generated_trajectories.x = 0.66 * Eigen::ArrayXXf::Ones(1000, 30); critic.score(data); // 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term - EXPECT_NEAR(xt::sum(costs, immediate)(), 6600.0, 1e-2); + EXPECT_NEAR(costs.sum(), 6600.0, 1e-2); // provide state pose and path far enough to enable, with data to pass condition // but path is blocked in collision @@ -602,11 +601,11 @@ TEST(CriticTests, PathAlignCritic) } data.path_pts_valid.reset(); // Recompute on new path - costs = xt::zeros({1000}); - path.x = 1.5 * xt::ones({22}); - path.y = 1.5 * xt::ones({22}); + costs = Eigen::ArrayXf::Zero(1000); + path.x = 1.5 * Eigen::ArrayXf::Ones(22); + path.y = 1.5 * Eigen::ArrayXf::Ones(22); critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); } TEST(CriticTests, VelocityDeadbandCritic) @@ -626,7 +625,7 @@ TEST(CriticTests, VelocityDeadbandCritic) models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; - xt::xtensor costs = xt::zeros({1000}); + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, @@ -643,16 +642,16 @@ TEST(CriticTests, VelocityDeadbandCritic) // Scoring testing // provide velocities out of deadband bounds, should not have any costs - state.vx = 0.80 * xt::ones({1000, 30}); - state.vy = 0.60 * xt::ones({1000, 30}); - state.wz = 0.80 * xt::ones({1000, 30}); + state.vx = 0.80 * Eigen::ArrayXXf::Ones(1000, 30); + state.vy = 0.60 * Eigen::ArrayXXf::Ones(1000, 30); + state.wz = 0.80 * Eigen::ArrayXXf::Ones(1000, 30); critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); + EXPECT_NEAR(costs.sum(), 0, 1e-6); // Test cost value - state.vx = 0.01 * xt::ones({1000, 30}); - state.vy = 0.02 * xt::ones({1000, 30}); - state.wz = 0.021 * xt::ones({1000, 30}); + state.vx = 0.01 * Eigen::ArrayXXf::Ones(1000, 30); + state.vy = 0.02 * Eigen::ArrayXXf::Ones(1000, 30); + state.wz = 0.021 * Eigen::ArrayXXf::Ones(1000, 30); critic.score(data); // 35.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7 EXPECT_NEAR(costs(1), 19.845, 0.01); diff --git a/nav2_mppi_controller/test/models_test.cpp b/nav2_mppi_controller/test/models_test.cpp index 0900ebac05..4a08f7a271 100644 --- a/nav2_mppi_controller/test/models_test.cpp +++ b/nav2_mppi_controller/test/models_test.cpp @@ -59,9 +59,9 @@ TEST(ModelsTest, PathTest) { // populate the object Path path; - path.x = Eigen::ArrayXf::Ones(); - path.y = Eigen::ArrayXf::Ones(); - path.yaws = Eigen::ArrayXf::Ones(); + path.x = Eigen::ArrayXf::Ones(10); + path.y = Eigen::ArrayXf::Ones(10); + path.yaws = Eigen::ArrayXf::Ones(10); // Show you can get contents EXPECT_EQ(path.x(4), 1); diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp index 469ba3bc26..f5c2c5e50e 100644 --- a/nav2_mppi_controller/test/motion_model_tests.cpp +++ b/nav2_mppi_controller/test/motion_model_tests.cpp @@ -55,9 +55,9 @@ TEST(MotionModelTests, DiffDriveTest) model->predict(state); - EXPECT_EQ(state.vx, state.cvx); - EXPECT_EQ(state.vy.isApprox(Eigen::ArrayXXf::Zero(batches, timesteps))); // non-holonomic - EXPECT_EQ(state.wz, state.cwz); + EXPECT_TRUE(state.vx.isApprox(state.cvx)); + EXPECT_TRUE(state.vy.isApprox(Eigen::ArrayXXf::Zero(batches, timesteps))); // non-holonomic + EXPECT_TRUE(state.wz.isApprox(state.cwz)); // Check that application of constraints are empty for Diff Drive for (unsigned int i = 0; i != control_sequence.vx.rows(); i++) { @@ -67,9 +67,9 @@ TEST(MotionModelTests, DiffDriveTest) models::ControlSequence initial_control_sequence = control_sequence; model->applyConstraints(control_sequence); - EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); - EXPECT_EQ(initial_control_sequence.vy, control_sequence.vy); - EXPECT_EQ(initial_control_sequence.wz, control_sequence.wz); + EXPECT_TRUE(initial_control_sequence.vx.isApprox(control_sequence.vx)); + EXPECT_TRUE(initial_control_sequence.vy.isApprox(control_sequence.vy)); + EXPECT_TRUE(initial_control_sequence.wz.isApprox(control_sequence.wz)); // Check that Diff Drive is properly non-holonomic EXPECT_EQ(model->isHolonomic(), false); @@ -101,9 +101,9 @@ TEST(MotionModelTests, OmniTest) model->predict(state); - EXPECT_EQ(state.vx, state.cvx); - EXPECT_EQ(state.vy, state.cvy); // holonomic - EXPECT_EQ(state.wz, state.cwz); + EXPECT_TRUE(state.vx.isApprox(state.cvx)); + EXPECT_TRUE(state.vy.isApprox(state.cvy)); // holonomic + EXPECT_TRUE(state.wz.isApprox(state.cwz)); // Check that application of constraints are empty for Omni Drive for (unsigned int i = 0; i != control_sequence.vx.rows(); i++) { @@ -114,9 +114,9 @@ TEST(MotionModelTests, OmniTest) models::ControlSequence initial_control_sequence = control_sequence; model->applyConstraints(control_sequence); - EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); - EXPECT_EQ(initial_control_sequence.vy, control_sequence.vy); - EXPECT_EQ(initial_control_sequence.wz, control_sequence.wz); + EXPECT_TRUE(initial_control_sequence.vx.isApprox(control_sequence.vx)); + EXPECT_TRUE(initial_control_sequence.vy.isApprox(control_sequence.vy)); + EXPECT_TRUE(initial_control_sequence.wz.isApprox(control_sequence.wz)); // Check that Omni Drive is properly holonomic EXPECT_EQ(model->isHolonomic(), true); @@ -149,9 +149,9 @@ TEST(MotionModelTests, AckermannTest) model->predict(state); - EXPECT_EQ(state.vx, state.cvx); - EXPECT_EQ(state.vy.isApprox(Eigen::ArrayXXf::Zero(batches, timesteps))); // non-holonomic - EXPECT_EQ(state.wz, state.cwz); + EXPECT_TRUE(state.vx.isApprox(state.cvx)); + EXPECT_TRUE(state.vy.isApprox(Eigen::ArrayXXf::Zero(batches, timesteps))); // non-holonomic + EXPECT_TRUE(state.wz.isApprox(state.cwz)); // Check that application of constraints are non-empty for Ackermann Drive for (unsigned int i = 0; i != control_sequence.vx.rows(); i++) { @@ -162,8 +162,8 @@ TEST(MotionModelTests, AckermannTest) models::ControlSequence initial_control_sequence = control_sequence; model->applyConstraints(control_sequence); // VX equal since this doesn't change, the WZ is reduced if breaking the constraint - EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); - EXPECT_NE(initial_control_sequence.wz, control_sequence.wz); + EXPECT_TRUE(initial_control_sequence.vx.isApprox(control_sequence.vx)); + EXPECT_FALSE(initial_control_sequence.wz.isApprox(control_sequence.wz)); // Now, check the specifics of the minimum curvature constraint EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6); diff --git a/nav2_mppi_controller/test/utils/utils.hpp b/nav2_mppi_controller/test/utils/utils.hpp index 0093492706..50e991e5fe 100644 --- a/nav2_mppi_controller/test/utils/utils.hpp +++ b/nav2_mppi_controller/test/utils/utils.hpp @@ -109,7 +109,7 @@ void printMapWithTrajectoryAndGoal( // add trajectory on map unsigned int point_mx = 0; unsigned int point_my = 0; - for (size_t i = 0; i < trajectory.shape()[0]; ++i) { + for (size_t i = 0; i < trajectory.rows(); ++i) { costmap2d.worldToMap(trajectory(i, 0), trajectory(i, 1), point_mx, point_my); costmap2d.setCost(point_mx, point_my, trajectory_cost); } @@ -182,7 +182,7 @@ bool inCollision(const TTrajectory & trajectory, const nav2_costmap_2d::Costmap2 unsigned int point_mx = 0; unsigned int point_my = 0; - for (size_t i = 0; i < trajectory.shape(0); ++i) { + for (size_t i = 0; i < trajectory.rows(); ++i) { costmap.worldToMap(trajectory(i, 0), trajectory(i, 1), point_mx, point_my); auto cost_ = costmap.getCost(point_mx, point_my); if (cost_ > nav2_costmap_2d::FREE_SPACE || cost_ == nav2_costmap_2d::NO_INFORMATION) { @@ -237,7 +237,7 @@ bool isGoalReached( }; // clang-format on - for (size_t i = 0; i < trajectory.shape(0); ++i) { + for (size_t i = 0; i < trajectory.rows(); ++i) { costmap.worldToMap(trajectory(i, 0), trajectory(i, 1), trajectory_j, trajectory_i); if (match_near(trajectory_i, trajectory_j)) { return true; From 0f82afe3a3afa9b35d6d8191992932a6092a02f3 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Fri, 6 Sep 2024 00:47:36 +0530 Subject: [PATCH 15/21] fixed few review comments Signed-off-by: Ayush1285 --- .../benchmark/optimizer_benchmark.cpp | 3 +- .../nav2_mppi_controller/controller.hpp | 4 +- nav2_mppi_controller/src/controller.cpp | 20 +++--- nav2_mppi_controller/src/optimizer.cpp | 69 +++++++++---------- 4 files changed, 45 insertions(+), 51 deletions(-) diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index 75a29bb91b..40ea9c042b 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -116,7 +116,8 @@ static void BM_Omni(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "Omni"; - std::vector critics = {}; + std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index 893342e733..82b850bc46 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -20,7 +20,7 @@ #include "nav2_mppi_controller/tools/path_handler.hpp" #include "nav2_mppi_controller/optimizer.hpp" -//#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp" +#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp" #include "nav2_mppi_controller/models/constraints.hpp" #include "nav2_mppi_controller/tools/utils.hpp" @@ -117,7 +117,7 @@ class MPPIController : public nav2_core::Controller std::unique_ptr parameters_handler_; Optimizer optimizer_; PathHandler path_handler_; - //TrajectoryVisualizer trajectory_visualizer_; + TrajectoryVisualizer trajectory_visualizer_; bool visualize_; }; diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index dacdca432b..6c83131263 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -41,9 +41,9 @@ void MPPIController::configure( // Configure composed objects optimizer_.initialize(parent_, name_, costmap_ros_, parameters_handler_.get()); path_handler_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get()); - //trajectory_visualizer_.on_configure( - //parent_, name_, - //costmap_ros_->getGlobalFrameID(), parameters_handler_.get()); + trajectory_visualizer_.on_configure( + parent_, name_, + costmap_ros_->getGlobalFrameID(), parameters_handler_.get()); RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str()); } @@ -51,21 +51,21 @@ void MPPIController::configure( void MPPIController::cleanup() { optimizer_.shutdown(); - //trajectory_visualizer_.on_cleanup(); + trajectory_visualizer_.on_cleanup(); parameters_handler_.reset(); RCLCPP_INFO(logger_, "Cleaned up MPPI Controller: %s", name_.c_str()); } void MPPIController::activate() { - //trajectory_visualizer_.on_activate(); + trajectory_visualizer_.on_activate(); parameters_handler_->start(); RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str()); } void MPPIController::deactivate() { - //trajectory_visualizer_.on_deactivate(); + trajectory_visualizer_.on_deactivate(); RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str()); } @@ -105,11 +105,11 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( return cmd; } -void MPPIController::visualize(nav_msgs::msg::Path /*transformed_plan*/) +void MPPIController::visualize(nav_msgs::msg::Path transformed_plan) { - //trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories"); - //trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory"); - //trajectory_visualizer_.visualize(std::move(transformed_plan)); + trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories"); + trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory"); + trajectory_visualizer_.visualize(std::move(transformed_plan)); } void MPPIController::setPlan(const nav_msgs::msg::Path & path) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index b0fd04e9ae..0fd0a5dff9 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -169,7 +169,6 @@ geometry_msgs::msg::TwistStamped Optimizer::evalControl( void Optimizer::optimize() { for (size_t i = 0; i < settings_.iteration_count; ++i) { - generateNoisedTrajectories(); critic_manager_.evalTrajectoriesScores(critics_data_); updateControlSequence(); @@ -233,32 +232,28 @@ void Optimizer::generateNoisedTrajectories() void Optimizer::applyControlSequenceConstraints() { auto & s = settings_; - float & vx_min = s.constraints.vx_min; - float & vx_max = s.constraints.vx_max; - float & vy = s.constraints.vy; - float & wz = s.constraints.wz; - - float && max_delta_vx = s.model_dt * s.constraints.ax_max; - float && min_delta_vx = s.model_dt * s.constraints.ax_min; - float && max_delta_vy = s.model_dt * s.constraints.ay_max; - float && max_delta_wz = s.model_dt * s.constraints.az_max; - float vx_last = std::min(vx_max, std::max(control_sequence_.vx(0), vx_min)); - float vy_last = std::min(vy, std::max(control_sequence_.vy(0), -vy)); - float wz_last = std::min(wz, std::max(control_sequence_.wz(0), -wz)); + + float max_delta_vx = s.model_dt * s.constraints.ax_max; + float min_delta_vx = s.model_dt * s.constraints.ax_min; + float max_delta_vy = s.model_dt * s.constraints.ay_max; + float max_delta_wz = s.model_dt * s.constraints.az_max; + float vx_last = std::min(s.constraints.vx_max, std::max(control_sequence_.vx(0), s.constraints.vx_min)); + float vy_last = std::min(s.constraints.vy, std::max(control_sequence_.vy(0), -s.constraints.vy)); + float wz_last = std::min(s.constraints.wz, std::max(control_sequence_.wz(0), -s.constraints.wz)); for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) { float & vx_curr = control_sequence_.vx(i); - vx_curr = std::min(vx_max, std::max(vx_curr, vx_min)); + vx_curr = std::min(s.constraints.vx_max, std::max(vx_curr, s.constraints.vx_min)); vx_curr = std::min(vx_last + max_delta_vx, std::max(vx_curr, vx_last + min_delta_vx)); vx_last = vx_curr; float & wz_curr = control_sequence_.wz(i); - wz_curr = std::min(wz, std::max(wz_curr, -wz)); + wz_curr = std::min(s.constraints.wz, std::max(wz_curr, -s.constraints.wz)); wz_curr = std::min(wz_last + max_delta_wz, std::max(wz_curr, wz_last - max_delta_wz)); wz_last = wz_curr; if (isHolonomic()) { float & vy_curr = control_sequence_.vy(i); - vy_curr = std::min(vy, std::max(vy_curr, -vy)); + vy_curr = std::min(s.constraints.vy, std::max(vy_curr, -s.constraints.vy)); vy_curr = std::min(vy_last + max_delta_vy, std::max(vy_curr, vy_last - max_delta_vy)); vy_last = vy_curr; } @@ -292,11 +287,10 @@ void Optimizer::propagateStateVelocitiesFromInitials( } void Optimizer::integrateStateVelocities( - Eigen::ArrayXXf & trajectory, + Eigen::Array & trajectory, const Eigen::ArrayXXf & sequence) const { float initial_yaw = static_cast(tf2::getYaw(state_.pose.pose.orientation)); - const float & dt_time = settings_.model_dt; const auto vx = sequence.col(0); const auto wz = sequence.col(1); @@ -305,19 +299,19 @@ void Optimizer::integrateStateVelocities( auto traj_y = trajectory.col(1); auto traj_yaws = trajectory.col(2); - unsigned int n_size = traj_yaws.size(); + size_t n_size = traj_yaws.size(); - traj_yaws(0) = wz(0) * dt_time + initial_yaw; + traj_yaws(0) = wz(0) * settings_.model_dt + initial_yaw; float last_yaw = traj_yaws(0); - for(unsigned int i = 1; i != n_size; i++) + for(size_t i = 1; i != n_size; i++) { float & curr_yaw = traj_yaws(i); - curr_yaw = last_yaw + wz(i) * dt_time; + curr_yaw = last_yaw + wz(i) * settings_.model_dt; last_yaw = curr_yaw; } - auto yaw_cos = (utils::rollColumns(traj_yaws, -1).cos()).eval(); - auto yaw_sin = (utils::rollColumns(traj_yaws, -1).sin()).eval(); + Eigen::ArrayXf yaw_cos = utils::rollColumns(traj_yaws, -1).cos(); + Eigen::ArrayXf yaw_sin = utils::rollColumns(traj_yaws, -1).sin(); yaw_cos(0) = cosf(initial_yaw); yaw_sin(0) = sinf(initial_yaw); @@ -330,16 +324,16 @@ void Optimizer::integrateStateVelocities( dy = (dy + vy * yaw_cos).eval(); } - traj_x(0) = state_.pose.pose.position.x + dx(0) * dt_time; - traj_y(0) = state_.pose.pose.position.y + dy(0) * dt_time; + traj_x(0) = state_.pose.pose.position.x + dx(0) * settings_.model_dt; + traj_y(0) = state_.pose.pose.position.y + dy(0) * settings_.model_dt; float last_x = traj_x(0); float last_y = traj_y(0); for(unsigned int i = 1; i != n_size; i++) { float & curr_x = traj_x(i); float & curr_y = traj_y(i); - curr_x = last_x + dx(i) * dt_time; - curr_y = last_y + dy(i) * dt_time; + curr_x = last_x + dx(i) * settings_.model_dt; + curr_y = last_y + dy(i) * settings_.model_dt; last_x = curr_x; last_y = curr_y; } @@ -351,12 +345,11 @@ void Optimizer::integrateStateVelocities( { const float initial_yaw = static_cast(tf2::getYaw(state.pose.pose.orientation)); const unsigned int n_cols = trajectories.yaws.cols(); - const float & dt_time = settings_.model_dt; - trajectories.yaws.col(0) = state.wz.col(0) * dt_time + initial_yaw; + trajectories.yaws.col(0) = state.wz.col(0) * settings_.model_dt + initial_yaw; for(unsigned int i = 1; i != n_cols; i++) { - trajectories.yaws.col(i) = trajectories.yaws.col(i - 1) + state.wz.col(i) * dt_time; + trajectories.yaws.col(i) = trajectories.yaws.col(i - 1) + state.wz.col(i) * settings_.model_dt; } auto yaw_cos = (utils::rollColumns(trajectories.yaws, -1).cos()).eval(); @@ -373,20 +366,20 @@ void Optimizer::integrateStateVelocities( dy = dy + state.vy * yaw_cos; } - trajectories.x.col(0) = dx.col(0) * dt_time + state.pose.pose.position.x; - trajectories.y.col(0) = dy.col(0) * dt_time + state.pose.pose.position.y; + trajectories.x.col(0) = dx.col(0) * settings_.model_dt + state.pose.pose.position.x; + trajectories.y.col(0) = dy.col(0) * settings_.model_dt + state.pose.pose.position.y; for(unsigned int i = 1; i != n_cols; i++) { - trajectories.x.col(i) = trajectories.x.col(i - 1) + dx.col(i) * dt_time; - trajectories.y.col(i) = trajectories.y.col(i - 1) + dy.col(i) * dt_time; + trajectories.x.col(i) = trajectories.x.col(i - 1) + dx.col(i) * settings_.model_dt; + trajectories.y.col(i) = trajectories.y.col(i - 1) + dy.col(i) * settings_.model_dt; } } Eigen::ArrayXXf Optimizer::getOptimizedTrajectory() { const bool is_holo = isHolonomic(); - auto && sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2); - auto && trajectories = Eigen::ArrayXXf(settings_.time_steps, 3); + Eigen::ArrayXXf sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2); + Eigen::Array trajectories = Eigen::Array(settings_.time_steps, 3); sequence.col(0) = control_sequence_.vx; sequence.col(1) = control_sequence_.wz; @@ -396,7 +389,7 @@ Eigen::ArrayXXf Optimizer::getOptimizedTrajectory() } integrateStateVelocities(trajectories, sequence); - return std::move(trajectories); + return trajectories; } void Optimizer::updateControlSequence() From ca61afcd735271b8ab04af9e1085e5691066f2c8 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Wed, 11 Sep 2024 22:49:34 +0530 Subject: [PATCH 16/21] Fixed linters and few review comments Signed-off-by: Ayush1285 --- nav2_mppi_controller/CMakeLists.txt | 6 +- .../benchmark/controller_benchmark.cpp | 5 +- .../benchmark/optimizer_benchmark.cpp | 4 +- .../nav2_mppi_controller/critic_data.hpp | 6 +- .../nav2_mppi_controller/critic_manager.hpp | 2 - .../nav2_mppi_controller/motion_models.hpp | 21 ++--- .../nav2_mppi_controller/optimizer.hpp | 8 +- .../tools/noise_generator.hpp | 14 ++-- .../tools/trajectory_visualizer.hpp | 4 +- .../nav2_mppi_controller/tools/utils.hpp | 69 +++++++++------- .../src/critics/constraint_critic.cpp | 24 +++--- .../src/critics/cost_critic.cpp | 22 +++--- .../src/critics/goal_angle_critic.cpp | 8 +- .../src/critics/goal_critic.cpp | 6 +- .../src/critics/obstacles_critic.cpp | 16 ++-- .../src/critics/path_align_critic.cpp | 13 ++-- .../src/critics/path_angle_critic.cpp | 27 ++++--- .../src/critics/prefer_forward_critic.cpp | 6 +- .../src/critics/velocity_deadband_critic.cpp | 29 +++---- nav2_mppi_controller/src/noise_generator.cpp | 14 ++-- nav2_mppi_controller/src/optimizer.cpp | 34 ++++---- .../src/trajectory_visualizer.cpp | 4 +- nav2_mppi_controller/test/CMakeLists.txt | 6 -- nav2_mppi_controller/test/critics_tests.cpp | 4 +- .../test/motion_model_tests.cpp | 78 ------------------- .../test/optimizer_smoke_test.cpp | 4 - .../test/trajectory_visualizer_tests.cpp | 8 +- nav2_mppi_controller/test/utils_test.cpp | 1 - 28 files changed, 196 insertions(+), 247 deletions(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 2f54d9d054..e42f380377 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -60,10 +60,9 @@ add_library(mppi_controller SHARED src/path_handler.cpp src/trajectory_visualizer.cpp ) -target_compile_options(mppi_controller PUBLIC -fconcepts -O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic) +target_compile_options(mppi_controller PUBLIC -fconcepts -O3) target_include_directories(mppi_controller PUBLIC - ${xsimd_INCLUDE_DIRS} "$" "$") target_link_libraries(mppi_controller PUBLIC @@ -96,10 +95,9 @@ add_library(mppi_critics SHARED src/critics/twirling_critic.cpp src/critics/velocity_deadband_critic.cpp ) -target_compile_options(mppi_critics PUBLIC -fconcepts -O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic) +target_compile_options(mppi_critics PUBLIC -fconcepts -O3) target_include_directories(mppi_critics PUBLIC - ${xsimd_INCLUDE_DIRS} "$" "$") target_link_libraries(mppi_critics PUBLIC diff --git a/nav2_mppi_controller/benchmark/controller_benchmark.cpp b/nav2_mppi_controller/benchmark/controller_benchmark.cpp index dfff3d5403..6585fa8cbd 100644 --- a/nav2_mppi_controller/benchmark/controller_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/controller_benchmark.cpp @@ -13,6 +13,9 @@ // limitations under the License. #include + +#include + #include #include @@ -24,8 +27,6 @@ #include #include -#include - #include "nav2_mppi_controller/motion_models.hpp" #include "nav2_mppi_controller/controller.hpp" diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index f5802fae2a..f58339076e 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -14,6 +14,8 @@ #include +#include + #include #include @@ -27,8 +29,6 @@ #include #include -#include - #include "nav2_mppi_controller/optimizer.hpp" #include "nav2_mppi_controller/motion_models.hpp" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp index 209cec4e2b..ac5447873d 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp @@ -15,11 +15,11 @@ #ifndef NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_ #define NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_ +#include + #include #include -#include - #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/goal_checker.hpp" #include "nav2_mppi_controller/models/state.hpp" @@ -39,7 +39,7 @@ namespace mppi struct CriticData { const models::State & state; - const models::Trajectories & trajectories; + models::Trajectories & trajectories; const models::Path & path; Eigen::ArrayXf & costs; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp index 0f227d5dfd..8790476b5d 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp @@ -20,8 +20,6 @@ #include #include - - #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" 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 5df705caad..3533eac778 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -15,15 +15,16 @@ #ifndef NAV2_MPPI_CONTROLLER__MOTION_MODELS_HPP_ #define NAV2_MPPI_CONTROLLER__MOTION_MODELS_HPP_ +#include + #include #include +#include #include "nav2_mppi_controller/models/control_sequence.hpp" #include "nav2_mppi_controller/models/state.hpp" #include "nav2_mppi_controller/models/constraints.hpp" -#include - #include "nav2_mppi_controller/tools/parameters_handler.hpp" namespace mppi @@ -72,14 +73,15 @@ class MotionModel unsigned int n_rows = state.vx.rows(); unsigned int n_cols = state.vx.cols(); - // Default layout in eigen is column-major, hence accessing elements in column-major fashion to utilize L1 cache as much as possible + // Default layout in eigen is column-major, hence accessing elements in + // column-major fashion to utilize L1 cache as much as possible for (unsigned int i = 1; i != n_cols; i++) { for (unsigned int j = 0; j != n_rows; j++) { float & vx_last = state.vx(j, i - 1); float & cvx_curr = state.cvx(j, i - 1); cvx_curr = std::min(vx_last + max_delta_vx, std::max(cvx_curr, vx_last + min_delta_vx)); state.vx(j, i) = cvx_curr; - + float & wz_last = state.wz(j, i - 1); float & cwz_curr = state.cwz(j, i - 1); cwz_curr = std::min(wz_last + max_delta_wz, std::max(cwz_curr, wz_last - max_delta_wz)); @@ -89,7 +91,7 @@ class MotionModel float & vy_last = state.vy(j, i - 1); float & cvy_curr = state.cvy(j, i - 1); cvy_curr = std::min(vy_last + max_delta_vy, std::max(cvy_curr, vy_last - max_delta_vy)); - state.vy(j, i) = cvy_curr; + state.vy(j, i) = cvy_curr; } } } @@ -147,15 +149,14 @@ class AckermannMotionModel : public MotionModel 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_); + 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 = std::min(wz_constrained, std::max(wz_curr, -1 * wz_constrained)); } // Taking more time compared to for loop - //wz = ((vx.abs() / wz.abs() < min_turning_r_).select(wz, wz.sign() * vx / min_turning_r_)).eval(); - + // wz = ((vx.abs() / wz.abs() < min_turning_r_).select( + // wz, wz.sign() * vx / min_turning_r_)).eval(); } /** diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 98e4e87986..66780d21f3 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -15,11 +15,11 @@ #ifndef NAV2_MPPI_CONTROLLER__OPTIMIZER_HPP_ #define NAV2_MPPI_CONTROLLER__OPTIMIZER_HPP_ +#include + #include #include -#include - #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" @@ -50,7 +50,7 @@ namespace mppi */ class Optimizer { -public: +public: /** * @brief Constructor for mppi::Optimizer */ @@ -196,7 +196,7 @@ class Optimizer * @param state fill state */ void integrateStateVelocities( - Eigen::ArrayXXf & trajectories, + Eigen::Array & trajectories, const Eigen::ArrayXXf & state) const; /** diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp index e641012f17..5823d64878 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp @@ -15,6 +15,8 @@ #ifndef NAV2_MPPI_CONTROLLER__TOOLS__NOISE_GENERATOR_HPP_ #define NAV2_MPPI_CONTROLLER__TOOLS__NOISE_GENERATOR_HPP_ +#include + #include #include #include @@ -22,8 +24,6 @@ #include #include -#include - #include "nav2_mppi_controller/models/optimizer_settings.hpp" #include "nav2_mppi_controller/tools/parameters_handler.hpp" #include "nav2_mppi_controller/models/control_sequence.hpp" @@ -77,7 +77,7 @@ class NoiseGenerator * @param settings Settings of controller * @param is_holonomic If base is holonomic */ - void reset(mppi::models::OptimizerSettings & settings, bool is_holonomic); + void reset(mppi::models::OptimizerSettings & settings, bool is_holonomic); protected: /** @@ -98,10 +98,10 @@ class NoiseGenerator Eigen::ArrayXXf noises_vy_; Eigen::ArrayXXf noises_wz_; - static std::default_random_engine generator_; - static std::normal_distribution ndistribution_vx_; - static std::normal_distribution ndistribution_wz_; - static std::normal_distribution ndistribution_vy_; + std::default_random_engine generator_; + std::normal_distribution ndistribution_vx_; + std::normal_distribution ndistribution_wz_; + std::normal_distribution ndistribution_vy_; mppi::models::OptimizerSettings settings_; bool is_holonomic_; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp index e82b589e18..c60d8637c9 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp @@ -15,11 +15,11 @@ #ifndef NAV2_MPPI_CONTROLLER__TOOLS__TRAJECTORY_VISUALIZER_HPP_ #define NAV2_MPPI_CONTROLLER__TOOLS__TRAJECTORY_VISUALIZER_HPP_ +#include + #include #include -#include - #include "nav_msgs/msg/path.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 949b3c208b..9d004ac42e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -16,6 +16,8 @@ #ifndef NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ #define NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ +#include + #include #include #include @@ -23,8 +25,6 @@ #include #include -#include - #include "angles/angles.h" #include "tf2/utils.h" @@ -265,8 +265,13 @@ inline bool withinPositionGoalTolerance( template auto normalize_angles(const T & angles) { - Eigen::ArrayXXf theta = (angles + M_PIF).unaryExpr([](const float x){ float remainder = std::fmod(x, 2.0f * M_PIF); return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF;}); - return ((theta < 0.0f).select(theta + M_PIF, theta - M_PIF)).eval(); + // Eigen::ArrayXXf theta = (angles + M_PIF).unaryExpr([](const float x){ + // float remainder = std::fmod(x, 2.0f * M_PIF); + // return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF; + // }); + // return ((theta < 0.0f).select(theta + M_PIF, theta - M_PIF)).eval(); + auto theta = angles - (M_PIF * ((angles + M_PIF_2) * (1.0f / M_PIF)).floor()); + return theta; } /** @@ -308,12 +313,12 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) int max_id_by_trajectories = 0, min_id_by_path = 0; float min_distance_by_path = std::numeric_limits::max(); - int n_rows = dists.rows(); - int n_cols = dists.cols(); - for (int i = 0; i < n_rows; i++) { + size_t n_rows = dists.rows(); + size_t n_cols = dists.cols(); + for (size_t i = 0; i != n_rows; i++) { min_id_by_path = 0; min_distance_by_path = std::numeric_limits::max(); - for (int j = max_id_by_trajectories; j < n_cols; j++) { + for (size_t j = max_id_by_trajectories; j != n_cols; j++) { const float cur_dist = dists(i, j); if (cur_dist < min_distance_by_path) { min_distance_by_path = cur_dist; @@ -691,51 +696,63 @@ struct Pose2D float x, y, theta; }; +// TODO(Ayush1285) Re-factor this utility method to +// perform in-place column shifting operation /** - * @brief Shift columns of a Eigen Array - * @param e input Eigen Array or expression - * @return a column wise shifted Eigen Array + * @brief Shift the columns of a Eigen Array + * @param e Eigen expression or Array + * @param shift Number of columns by which array will be shifted + * ex: -1 means shifting columns to right by 1 place + * @return shifted Eigen Array */ template -auto rollColumns(T&& e, std::ptrdiff_t shift) +auto rollColumns(T && e, std::ptrdiff_t shift) { shift = shift >= 0 ? shift : e.cols() + shift; auto flat_size = shift * e.rows(); Eigen::ArrayXXf cpyMatrix(e.rows(), e.cols()); - std::copy(e.data(), e.data() + flat_size, std::copy(e.data() + flat_size, e.data() + e.size(), cpyMatrix.data())); + std::copy(e.data(), e.data() + flat_size, std::copy( + e.data() + flat_size, e.data() + e.size(), cpyMatrix.data())); return cpyMatrix; } -inline auto point_corrected_yaws(const Eigen::ArrayXf & yaws, const Eigen::ArrayXf & yaws_between_points) +inline auto point_corrected_yaws( + const Eigen::ArrayXf & yaws, const Eigen::ArrayXf & yaws_between_points) { int size = yaws.size(); Eigen::ArrayXf yaws_between_points_corrected(size); - for(int i = 0; i != size; i++) - { + for(int i = 0; i != size; i++) { const float & yaw_between_points = yaws_between_points[i]; - yaws_between_points_corrected[i] = yaws[i] < M_PIF_2 ? yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF); + yaws_between_points_corrected[i] = yaws[i] < M_PIF_2 ? + yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF); } // binaryExpr slower than for loop - // Eigen::ArrayXf yaws_between_points_corrected = yaws.binaryExpr(yaws_between_points, [&](const float & yaw, const float & yaw_between_points) - // {return yaw < M_PIF_2 ? yaw_between_points : normalize_anglef(yaw_between_points + M_PIF);}); + // Eigen::ArrayXf yaws_between_points_corrected = yaws.binaryExpr( + // yaws_between_points, [&](const float & yaw, const float & yaw_between_points) { + // return yaw < M_PIF_2 ? yaw_between_points : normalize_anglef(yaw_between_points + M_PIF); + // }); return yaws_between_points_corrected; } -inline auto point_corrected_yaws(const Eigen::ArrayXf & yaws_between_points, const float & goal_yaw) +inline auto point_corrected_yaws( + const Eigen::ArrayXf & yaws_between_points, const float & goal_yaw) { int size = yaws_between_points.size(); Eigen::ArrayXf yaws_between_points_corrected(size); - for(int i = 0; i != size; i++) - { + for(int i = 0; i != size; i++) { const float & yaw_between_points = yaws_between_points[i]; - yaws_between_points_corrected[i] = fabs(angles::normalize_angle(yaw_between_points - goal_yaw)) < M_PIF_2 ? - yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF); + yaws_between_points_corrected[i] = fabs( + angles::normalize_angle(yaw_between_points - goal_yaw)) < M_PIF_2 ? + yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF); } // unaryExpr slower than for loop - // Eigen::ArrayXf yaws_between_points_corrected = yaws_between_points.unaryExpr([&](const float & yaw_between_points) - // {return fabs(normalize_anglef(yaw_between_points - goal_yaw)) < M_PIF_2 ? yaw_between_points : normalize_anglef(yaw_between_points + M_PIF);}); + // Eigen::ArrayXf yaws_between_points_corrected = yaws_between_points.unaryExpr( + // [&](const float & yaw_between_points) { + // return fabs(normalize_anglef(yaw_between_points - goal_yaw)) < M_PIF_2 ? + // yaw_between_points : normalize_anglef(yaw_between_points + M_PIF); + // }); return yaws_between_points_corrected; } diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index 4b090d240f..0d2f61c78f 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -48,9 +48,11 @@ void ConstraintCritic::score(CriticData & data) auto diff = dynamic_cast(data.motion_model.get()); if (diff != nullptr) { if (power_ > 1u) { - data.costs += (((((data.state.vx - max_vel_).max(0.0f) + (min_vel_ - data.state.vx).max(0.0f)) * data.model_dt).rowwise().sum().eval()) * weight_).pow(power_); + data.costs += (((((data.state.vx - max_vel_).max(0.0f) + (min_vel_ - data.state.vx). + max(0.0f)) * data.model_dt).rowwise().sum().eval()) * weight_).pow(power_); } else { - data.costs += ((((data.state.vx - max_vel_).max(0.0f) + (min_vel_ - data.state.vx).max(0.0f)) * data.model_dt).rowwise().sum().eval()) * weight_; + data.costs += ((((data.state.vx - max_vel_).max(0.0f) + (min_vel_ - data.state.vx). + max(0.0f)) * data.model_dt).rowwise().sum().eval()) * weight_; } return; } @@ -62,13 +64,15 @@ void ConstraintCritic::score(CriticData & data) unsigned int n_rows = data.state.vx.rows(); unsigned int n_cols = data.state.vx.cols(); Eigen::ArrayXXf sgn(n_rows, n_cols); - sgn = vx.unaryExpr([](const float x){ return copysignf(1.0f, x);}); + sgn = vx.unaryExpr([](const float x){return copysignf(1.0f, x);}); auto vel_total = (data.state.vx.square() + data.state.vy.square()).sqrt() * sgn; if (power_ > 1u) { - data.costs += ((std::move((vel_total - max_vel_).max(0.0f) + (min_vel_ - vel_total).max(0.0f)) * data.model_dt).rowwise().sum().eval() * weight_).pow(power_); + data.costs += ((std::move((vel_total - max_vel_).max(0.0f) + (min_vel_ - vel_total). + max(0.0f)) * data.model_dt).rowwise().sum().eval() * weight_).pow(power_); } else { - data.costs += (std::move((vel_total - max_vel_).max(0.0f) + (min_vel_ - vel_total).max(0.0f)) * data.model_dt).rowwise().sum().eval() * weight_; + data.costs += (std::move((vel_total - max_vel_).max(0.0f) + (min_vel_ - vel_total). + max(0.0f)) * data.model_dt).rowwise().sum().eval() * weight_; } return; } @@ -79,13 +83,13 @@ void ConstraintCritic::score(CriticData & data) 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); + auto out_of_turning_rad_motion = (min_turning_rad - (vx.abs() / wz.abs())).max(0.0f); if (power_ > 1u) { - data.costs += ((std::move((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) + out_of_turning_rad_motion) - * data.model_dt).rowwise().sum().eval() * weight_).pow(power_); + data.costs += ((std::move((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) + + out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * weight_).pow(power_); } else { - data.costs += (std::move((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) + out_of_turning_rad_motion) - * data.model_dt).rowwise().sum().eval() * weight_; + data.costs += (std::move((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) + + out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * weight_; } return; } diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index e92d61f115..84c1444489 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -123,16 +123,19 @@ void CostCritic::score(CriticData & data) Eigen::ArrayXf repulsive_cost(data.costs.rows()); bool all_trajectories_collide = true; - int strided_traj_cols = data.trajectories.x.cols()/trajectory_point_step_ + 1; + int strided_traj_cols = data.trajectories.x.cols() / trajectory_point_step_ + 1; int strided_traj_rows = data.trajectories.x.rows(); int outer_stride = strided_traj_rows * trajectory_point_step_; - const auto traj_x = Eigen::Map> - (data.trajectories.x.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); - const auto traj_y = Eigen::Map> - (data.trajectories.y.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); - const auto traj_yaw = Eigen::Map> - (data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto traj_x = Eigen::Map>(data.trajectories.x.data(), strided_traj_rows, strided_traj_cols, + Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto traj_y = Eigen::Map>(data.trajectories.y.data(), strided_traj_rows, strided_traj_cols, + Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto traj_yaw = Eigen::Map>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols, + Eigen::Stride<-1, -1>(outer_stride, 1)); for (int i = 0; i < strided_traj_rows; ++i) { bool trajectory_collide = false; @@ -183,9 +186,10 @@ void CostCritic::score(CriticData & data) } if (power_ > 1u) { - data.costs += (std::move(repulsive_cost) * (weight_/static_cast(strided_traj_cols))).pow(power_); + data.costs += (std::move(repulsive_cost) * + (weight_ / static_cast(strided_traj_cols))).pow(power_); } else { - data.costs += std::move(repulsive_cost) * (weight_/static_cast(strided_traj_cols)); + data.costs += std::move(repulsive_cost) * (weight_ / static_cast(strided_traj_cols)); } data.fail_flag = all_trajectories_collide; diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index edaa41ada0..e0036a8534 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -44,10 +44,12 @@ void GoalAngleCritic::score(CriticData & data) const auto goal_idx = data.path.x.size() - 1; const float goal_yaw = data.path.yaws(goal_idx); - if( power_ > 1u) { - data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()).rowwise().mean())*weight_).pow(power_); + if(power_ > 1u) { + data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). + rowwise().mean()) * weight_).pow(power_); } else { - data.costs += ((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()).rowwise().mean())*weight_; + data.costs += ((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). + rowwise().mean()) * weight_; } } diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index cf9e9a1f6b..ea4125b35e 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -49,9 +49,11 @@ void GoalCritic::score(CriticData & data) const auto delta_y = data.trajectories.y - goal_y; if(power_ > 1u) { - data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean() * weight_).pow(power_); + data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean() * + weight_).pow(power_); } else { - data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean() * weight_).eval(); + data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean() * + weight_).eval(); } } diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index d998d99f29..94a5ae7a23 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -127,15 +127,13 @@ void ObstaclesCritic::score(CriticData & data) bool all_trajectories_collide = true; const auto & traj = data.trajectories; - // Default layout in eigen is column-major, hence accessing elements in column-major fashion to utilize L1 cache as much as possible - for(unsigned int i = 0; i != traj_len; i++) - { + // Default layout in eigen is column-major, hence accessing elements + // in column-major fashion to utilize L1 cache as much as possible + for(unsigned int i = 0; i != traj_len; i++) { bool trajectory_collide = false; CollisionCost pose_cost; - for(unsigned int j = 0; j != batch_size; j++) - { - if(raw_cost[j] == collision_cost_) - { + for(unsigned int j = 0; j != batch_size; j++) { + if(raw_cost[j] == collision_cost_) { continue; } @@ -169,7 +167,6 @@ void ObstaclesCritic::score(CriticData & data) } if (!trajectory_collide) {all_trajectories_collide = false;} - } // Normalize repulsive cost by trajectory length & lowest score to not overweight importance @@ -177,7 +174,8 @@ void ObstaclesCritic::score(CriticData & data) auto && repulsive_cost_normalized = (repulsive_cost - repulsive_cost.minCoeff()) / traj_len; if (power_ > 1u) { - data.costs += Eigen::pow((critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized), power_); + data.costs += Eigen::pow( + (critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized), power_); } else { data.costs += (critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized); } diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index a3d0552be4..84da4edf51 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -96,15 +96,18 @@ void PathAlignCritic::score(CriticData & data) float traj_integrated_distance = 0.0f; int strided_traj_rows = data.trajectories.x.rows(); - int strided_traj_cols = data.trajectories.x.cols()/trajectory_point_step_ + 1; + int strided_traj_cols = data.trajectories.x.cols() / trajectory_point_step_ + 1; int outer_stride = strided_traj_rows * trajectory_point_step_; // Get strided trajectory information - const auto T_x = Eigen::Map>(data.trajectories.x.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); - const auto T_y = Eigen::Map>(data.trajectories.y.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); - const auto T_yaw = Eigen::Map>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto T_x = Eigen::Map>(data.trajectories.x.data(), + strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto T_y = Eigen::Map>(data.trajectories.y.data(), + strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto T_yaw = Eigen::Map>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols, + Eigen::Stride<-1, -1>(outer_stride, 1)); const auto traj_sampled_size = T_x.cols(); - // TODO: check performance for both row-major and column-major looping for (size_t t = 0; t < batch_size; ++t) { summed_path_dist = 0.0f; num_samples = 0u; diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 7d75f409dc..692b436ff0 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -69,7 +69,8 @@ void PathAngleCritic::score(CriticData & data) utils::setPathFurthestPointIfNotSet(data); auto offseted_idx = std::min( - *data.furthest_reached_path_point + offset_from_furthest_, (size_t)data.path.x.size() - 1); + *data.furthest_reached_path_point + offset_from_furthest_, + static_cast(data.path.x.size()) - 1); const float goal_x = data.path.x(offseted_idx); const float goal_y = data.path.y(offseted_idx); @@ -99,13 +100,14 @@ void PathAngleCritic::score(CriticData & data) int && rightmost_idx = data.trajectories.y.cols() - 1; auto diff_y = goal_y - data.trajectories.y.col(rightmost_idx); auto diff_x = goal_x - data.trajectories.x.col(rightmost_idx); - auto yaws_between_points = diff_y.binaryExpr(diff_x, [&](const float & y, const float & x){return atan2f(y, x);}); + auto yaws_between_points = diff_y.binaryExpr( + diff_x, [&](const float & y, const float & x){return atan2f(y, x);}); switch (mode_) { case PathAngleMode::FORWARD_PREFERENCE: { - auto yaws = - utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points).abs(); + auto yaws = utils::shortest_angular_distance( + data.trajectories.yaws.col(rightmost_idx), yaws_between_points).abs(); if (power_ > 1u) { data.costs += (yaws * weight_).pow(power_); } else { @@ -115,11 +117,11 @@ void PathAngleCritic::score(CriticData & data) } case PathAngleMode::NO_DIRECTIONAL_PREFERENCE: { - auto yaws = - utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points).abs(); + auto yaws = utils::shortest_angular_distance( + data.trajectories.yaws.col(rightmost_idx), yaws_between_points).abs(); auto yaws_between_points_corrected = utils::point_corrected_yaws(yaws, yaws_between_points); - auto corrected_yaws = - utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points_corrected).abs(); + auto corrected_yaws = utils::shortest_angular_distance( + data.trajectories.yaws.col(rightmost_idx), yaws_between_points_corrected).abs(); if (power_ > 1u) { data.costs += (corrected_yaws * weight_).pow(power_); } else { @@ -129,9 +131,10 @@ void PathAngleCritic::score(CriticData & data) } case PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS: { - auto yaws_between_points_corrected = utils::point_corrected_yaws(yaws_between_points, goal_yaw); - auto corrected_yaws = - utils::shortest_angular_distance(data.trajectories.yaws.col(rightmost_idx), yaws_between_points_corrected).abs(); + auto yaws_between_points_corrected = + utils::point_corrected_yaws(yaws_between_points, goal_yaw); + auto corrected_yaws = utils::shortest_angular_distance( + data.trajectories.yaws.col(rightmost_idx), yaws_between_points_corrected).abs(); if (power_ > 1u) { data.costs += (corrected_yaws * weight_).pow(power_); } else { @@ -148,4 +151,4 @@ void PathAngleCritic::score(CriticData & data) PLUGINLIB_EXPORT_CLASS( mppi::critics::PathAngleCritic, - mppi::critics::CriticFunction) \ No newline at end of file + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index fb2157de0e..98bec19435 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -42,9 +42,11 @@ void PreferForwardCritic::score(CriticData & data) if (power_ > 1u) { data.costs += Eigen::pow( - (data.state.vx.unaryExpr([&](const float & x){ return std::max(-x, 0.0f);}) * data.model_dt).rowwise().sum() * weight_, power_); + (data.state.vx.unaryExpr([&](const float & x){return std::max(-x, 0.0f);}) * + data.model_dt).rowwise().sum() * weight_, power_); } else { - data.costs += (data.state.vx.unaryExpr([&](const float & x){ return std::max(-x, 0.0f);}) * data.model_dt).rowwise().sum() * weight_; + data.costs += (data.state.vx.unaryExpr([&](const float & x){return std::max(-x, 0.0f);}) * + data.model_dt).rowwise().sum() * weight_; } } diff --git a/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp index a553391495..5befdf34ea 100644 --- a/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp +++ b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp @@ -40,32 +40,35 @@ void VelocityDeadbandCritic::initialize() void VelocityDeadbandCritic::score(CriticData & data) { - if (!enabled_) { return; } if (data.motion_model->isHolonomic()) { if (power_ > 1u) { - data.costs += ((((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + - (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + - (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * data.model_dt).rowwise().sum() * weight_).pow(power_); + data.costs += ((((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + + (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + + (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * + data.model_dt).rowwise().sum() * weight_).pow(power_); } else { - data.costs += (((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + - (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + - (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * data.model_dt).rowwise().sum() * weight_; + data.costs += (((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + + (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + + (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * + data.model_dt).rowwise().sum() * weight_; } return; } if (power_ > 1u) { - data.costs += ((((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + - (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + - (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * data.model_dt).rowwise().sum() * weight_).pow(power_); + data.costs += ((((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + + (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + + (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * + data.model_dt).rowwise().sum() * weight_).pow(power_); } else { - data.costs += (((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + - (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + - (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * data.model_dt).rowwise().sum() * weight_; + data.costs += (((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + + (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + + (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * + data.model_dt).rowwise().sum() * weight_; } return; } diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index 872e41580c..de08cf3b34 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -20,11 +20,6 @@ namespace mppi { -std::normal_distribution NoiseGenerator::ndistribution_vx_; -std::normal_distribution NoiseGenerator::ndistribution_vy_; -std::normal_distribution NoiseGenerator::ndistribution_wz_; -std::default_random_engine NoiseGenerator::generator_; - void NoiseGenerator::initialize( mppi::models::OptimizerSettings & settings, bool is_holonomic, const std::string & name, ParametersHandler * param_handler) @@ -113,10 +108,13 @@ void NoiseGenerator::noiseThread() void NoiseGenerator::generateNoisedControls() { auto & s = settings_; - noises_vx_ = Eigen::ArrayXXf::NullaryExpr(s.batch_size, s.time_steps, [&] () {return ndistribution_vx_(generator_);}); - noises_wz_ = Eigen::ArrayXXf::NullaryExpr(s.batch_size, s.time_steps, [&] () {return ndistribution_wz_(generator_);}); + noises_vx_ = Eigen::ArrayXXf::NullaryExpr( + s.batch_size, s.time_steps, [&] () {return ndistribution_vx_(generator_);}); + noises_wz_ = Eigen::ArrayXXf::NullaryExpr( + s.batch_size, s.time_steps, [&] () {return ndistribution_wz_(generator_);}); if(is_holonomic_) { - noises_vy_ = Eigen::ArrayXXf::NullaryExpr(s.batch_size, s.time_steps, [&] () {return ndistribution_vy_(generator_);}); + noises_vy_ = Eigen::ArrayXXf::NullaryExpr( + s.batch_size, s.time_steps, [&] () {return ndistribution_vy_(generator_);}); } } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 9bd143a7e3..36271d19e7 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -237,9 +237,13 @@ void Optimizer::applyControlSequenceConstraints() float min_delta_vx = s.model_dt * s.constraints.ax_min; float max_delta_vy = s.model_dt * s.constraints.ay_max; float max_delta_wz = s.model_dt * s.constraints.az_max; - float vx_last = std::min(s.constraints.vx_max, std::max(control_sequence_.vx(0), s.constraints.vx_min)); - float vy_last = std::min(s.constraints.vy, std::max(control_sequence_.vy(0), -s.constraints.vy)); - float wz_last = std::min(s.constraints.wz, std::max(control_sequence_.wz(0), -s.constraints.wz)); + float vx_last = std::min( + s.constraints.vx_max, std::max(control_sequence_.vx(0), s.constraints.vx_min)); + float vy_last = std::min( + s.constraints.vy, std::max(control_sequence_.vy(0), -s.constraints.vy)); + float wz_last = std::min( + s.constraints.wz, std::max(control_sequence_.wz(0), -s.constraints.wz)); + for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) { float & vx_curr = control_sequence_.vx(i); vx_curr = std::min(s.constraints.vx_max, std::max(vx_curr, s.constraints.vx_min)); @@ -303,8 +307,7 @@ void Optimizer::integrateStateVelocities( traj_yaws(0) = wz(0) * settings_.model_dt + initial_yaw; float last_yaw = traj_yaws(0); - for(size_t i = 1; i != n_size; i++) - { + for(size_t i = 1; i != n_size; i++) { float & curr_yaw = traj_yaws(i); curr_yaw = last_yaw + wz(i) * settings_.model_dt; last_yaw = curr_yaw; @@ -328,8 +331,7 @@ void Optimizer::integrateStateVelocities( traj_y(0) = state_.pose.pose.position.y + dy(0) * settings_.model_dt; float last_x = traj_x(0); float last_y = traj_y(0); - for(unsigned int i = 1; i != n_size; i++) - { + for(unsigned int i = 1; i != n_size; i++) { float & curr_x = traj_x(i); float & curr_y = traj_y(i); curr_x = last_x + dx(i) * settings_.model_dt; @@ -347,8 +349,7 @@ void Optimizer::integrateStateVelocities( const unsigned int n_cols = trajectories.yaws.cols(); trajectories.yaws.col(0) = state.wz.col(0) * settings_.model_dt + initial_yaw; - for(unsigned int i = 1; i != n_cols; i++) - { + for(unsigned int i = 1; i != n_cols; i++) { trajectories.yaws.col(i) = trajectories.yaws.col(i - 1) + state.wz.col(i) * settings_.model_dt; } @@ -368,8 +369,7 @@ void Optimizer::integrateStateVelocities( trajectories.x.col(0) = dx.col(0) * settings_.model_dt + state.pose.pose.position.x; trajectories.y.col(0) = dy.col(0) * settings_.model_dt + state.pose.pose.position.y; - for(unsigned int i = 1; i != n_cols; i++) - { + for(unsigned int i = 1; i != n_cols; i++) { trajectories.x.col(i) = trajectories.x.col(i - 1) + dx.col(i) * settings_.model_dt; trajectories.y.col(i) = trajectories.y.col(i - 1) + dy.col(i) * settings_.model_dt; } @@ -379,7 +379,8 @@ Eigen::ArrayXXf Optimizer::getOptimizedTrajectory() { const bool is_holo = isHolonomic(); Eigen::ArrayXXf sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2); - Eigen::Array trajectories = Eigen::Array(settings_.time_steps, 3); + Eigen::Array trajectories = + Eigen::Array(settings_.time_steps, 3); sequence.col(0) = control_sequence_.vx; sequence.col(1) = control_sequence_.wz; @@ -398,11 +399,14 @@ void Optimizer::updateControlSequence() auto & s = settings_; auto && bounded_noises_vx = state_.cvx.rowwise() - control_sequence_.vx.transpose(); auto && bounded_noises_wz = state_.cwz.rowwise() - control_sequence_.wz.transpose(); - costs_ += s.gamma / powf(s.sampling_std.vx, 2) * (bounded_noises_vx.rowwise() * control_sequence_.vx.transpose()).rowwise().sum(); - costs_ += s.gamma / powf(s.sampling_std.wz, 2) * (bounded_noises_wz.rowwise() * control_sequence_.wz.transpose()).rowwise().sum(); + costs_ += s.gamma / powf(s.sampling_std.vx, 2) * + (bounded_noises_vx.rowwise() * control_sequence_.vx.transpose()).rowwise().sum(); + costs_ += s.gamma / powf(s.sampling_std.wz, 2) * + (bounded_noises_wz.rowwise() * control_sequence_.wz.transpose()).rowwise().sum(); if (is_holo) { auto bounded_noises_vy = state_.cvy.rowwise() - control_sequence_.vy.transpose(); - costs_ += s.gamma / powf(s.sampling_std.vy, 2) * (bounded_noises_vy.rowwise() * control_sequence_.vy.transpose()).rowwise().sum(); + costs_ += s.gamma / powf(s.sampling_std.vy, 2) * + (bounded_noises_vy.rowwise() * control_sequence_.vy.transpose()).rowwise().sum(); } auto && costs_normalized = costs_ - costs_.minCoeff(); diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 5490d9e52d..5d4df563e8 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -97,7 +97,7 @@ void TrajectoryVisualizer::add( optimal_path_->header.stamp = cmd_stamp; optimal_path_->header.frame_id = frame_id_; - for (size_t i = 0; i != size; i++) { + for (size_t i = 0; i < size; i++) { add_marker(i); } } @@ -152,4 +152,4 @@ void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan) } } -} // namespace mppi \ No newline at end of file +} // namespace mppi diff --git a/nav2_mppi_controller/test/CMakeLists.txt b/nav2_mppi_controller/test/CMakeLists.txt index 77e3cf2fe6..e1cddfbb4b 100644 --- a/nav2_mppi_controller/test/CMakeLists.txt +++ b/nav2_mppi_controller/test/CMakeLists.txt @@ -25,9 +25,6 @@ foreach(name IN LISTS TEST_NAMES) nav2_costmap_2d::nav2_costmap_2d_core rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle - xtensor - xtensor::optimize - xtensor::use_xsimd nav2_core::nav2_core ) @@ -44,9 +41,6 @@ target_link_libraries(critics_tests rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle nav2_costmap_2d::nav2_costmap_2d_core - xtensor - xtensor::optimize - xtensor::use_xsimd ) if(${TEST_DEBUG_INFO}) target_compile_definitions(critics_tests PUBLIC -DTEST_DEBUG_INFO) diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index bca44e5da6..d5660c88fd 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -105,7 +105,7 @@ TEST(CriticTests, ConstraintsCritic) // provide out of minimum velocity constraint auto first_batch_traj_in_full = state.vx.row(0); - first_batch_traj_in_full = -0.45 * Eigen::ArrayXf::Ones(30);; + first_batch_traj_in_full = -0.45 * Eigen::ArrayXf::Ones(30); critic.score(data); EXPECT_GT(costs.sum(), 0); // 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2 @@ -451,7 +451,7 @@ TEST(CriticTests, TwirlingCritic) costs = Eigen::ArrayXf::Zero(1000); // Now try again with some wiggling noise - traj_view = Eigen::ArrayXf::Random(30).abs()/2.0f; + traj_view = Eigen::ArrayXf::Random(30).abs() / 2.0f; critic.score(data); EXPECT_NEAR(costs(0), 3.3, 4e-1); // (mean of noise with mu=0, sigma=0.5 * 10.0 weight } diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp index 987082df90..2dd0827383 100644 --- a/nav2_mppi_controller/test/motion_model_tests.cpp +++ b/nav2_mppi_controller/test/motion_model_tests.cpp @@ -164,9 +164,6 @@ TEST(MotionModelTests, AckermannTest) // VX equal since this doesn't change, the WZ is reduced if breaking the constraint EXPECT_TRUE(initial_control_sequence.vx.isApprox(control_sequence.vx)); EXPECT_FALSE(initial_control_sequence.wz.isApprox(control_sequence.wz)); - for (unsigned int i = 1; i != control_sequence.wz.shape(0); i++) { - EXPECT_GT(control_sequence.wz(i), 0.0); - } // Now, check the specifics of the minimum curvature constraint EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6); @@ -180,78 +177,3 @@ TEST(MotionModelTests, AckermannTest) // Check it cleanly destructs model.reset(); } - -TEST(MotionModelTests, AckermannReversingTest) -{ - models::ControlSequence control_sequence; - models::ControlSequence control_sequence2; - models::State state; - int batches = 1000; - int timesteps = 50; - control_sequence.reset(timesteps); // populates with zeros - control_sequence2.reset(timesteps); // populates with zeros - state.reset(batches, timesteps); // populates with zeros - auto node = std::make_shared("my_node"); - ParametersHandler param_handler(node); - std::unique_ptr model = - std::make_unique(¶m_handler, node->get_name()); - - // Check that predict properly populates the trajectory velocities with the control velocities - state.cvx = 10 * Eigen::ArrayXXf::Ones(batches, timesteps); - state.cvy = 5 * Eigen::ArrayXXf::Ones(batches, timesteps); - state.cwz = 1 * Eigen::ArrayXXf::Ones(batches, timesteps); - - // Manually set state index 0 from initial conditions which would be the speed of the robot - state.vx.col(0) = 10; - state.wz.col(0) = 1; - - model->predict(state); - - EXPECT_TRUE(state.vx.isApprox(state.cvx)); - EXPECT_TRUE(state.vy.isApprox(Eigen::ArrayXXf::Zero(batches, timesteps))); // non-holonomic - EXPECT_TRUE(state.wz.isApprox(state.cwz)); - - // Check that application of constraints are non-empty for Ackermann Drive - for (unsigned int i = 0; i != control_sequence.vx.rows(); i++) { - float idx = static_cast(i); - control_sequence.vx(i) = -idx * idx * idx; // now reversing - control_sequence.wz(i) = idx * idx * idx * idx; - } - - models::ControlSequence initial_control_sequence = control_sequence; - model->applyConstraints(control_sequence); - // VX equal since this doesn't change, the WZ is reduced if breaking the constraint - EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); - EXPECT_NE(initial_control_sequence.wz, control_sequence.wz); - for (unsigned int i = 1; i != control_sequence.wz.rows(); i++) { - EXPECT_GT(control_sequence.wz(i), 0.0); - } - - // Repeat with negative rotation direction - for (unsigned int i = 0; i != control_sequence2.vx.rows(); i++) { - float idx = static_cast(i); - control_sequence2.vx(i) = -idx * idx * idx; // now reversing - control_sequence2.wz(i) = -idx * idx * idx * idx; - } - - models::ControlSequence initial_control_sequence2 = control_sequence2; - model->applyConstraints(control_sequence2); - // VX equal since this doesn't change, the WZ is reduced if breaking the constraint - EXPECT_EQ(initial_control_sequence2.vx, control_sequence2.vx); - EXPECT_NE(initial_control_sequence2.wz, control_sequence2.wz); - for (unsigned int i = 1; i != control_sequence2.wz.rows(); i++) { - EXPECT_LT(control_sequence2.wz(i), 0.0); - } - - // Now, check the specifics of the minimum curvature constraint - EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6); - for (unsigned int i = 1; i != control_sequence2.vx.rows(); i++) { - EXPECT_TRUE(fabs(control_sequence2.vx(i)) / fabs(control_sequence2.wz(i)) >= 0.2); - } - - // Check that Ackermann Drive is properly non-holonomic and parameterized - EXPECT_EQ(model->isHolonomic(), false); - - // Check it cleanly destructs - model.reset(); -} diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index 9a77f6d8b0..b5163f11a2 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -22,10 +22,6 @@ #include #include -#include -#include -#include - #include "nav2_mppi_controller/optimizer.hpp" #include "nav2_mppi_controller/tools/parameters_handler.hpp" #include "nav2_mppi_controller/motion_models.hpp" diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index b0ea4d0ea2..207e855949 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -90,7 +90,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) EXPECT_EQ(recieved_msg.markers.size(), 0u); // Now populated with content, should publish - optimal_trajectory = Eigen::ArrayXXf::Ones(20,2); + optimal_trajectory = Eigen::ArrayXXf::Ones(20, 2); vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp); vis.visualize(bogus_path); @@ -169,7 +169,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) [&](const nav_msgs::msg::Path msg) {recieved_path = msg;}); // optimal_trajectory empty, should fail to publish - xt::xtensor optimal_trajectory; + Eigen::ArrayXXf optimal_trajectory; TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); vis.on_activate(); @@ -181,8 +181,8 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) EXPECT_EQ(recieved_path.poses.size(), 0u); // Now populated with content, should publish - optimal_trajectory.resize({20, 2}); - for (unsigned int i = 0; i != optimal_trajectory.shape()[0] - 1; i++) { + optimal_trajectory.resize(20, 2); + for (unsigned int i = 0; i != optimal_trajectory.rows() - 1; i++) { optimal_trajectory(i, 0) = static_cast(i); optimal_trajectory(i, 1) = static_cast(i); } diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 1787bc7e33..7e17606b5c 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -179,7 +179,6 @@ TEST(UtilsTests, AnglesTests) auto norm_ang = normalize_angles(angles); for (unsigned int i = 0; i != norm_ang.size(); i++) { - //Sstd::cout << "norm_ang(" << i << ") = " << norm_ang(i) << std::endl; EXPECT_TRUE((norm_ang(i) >= -M_PIF) && (norm_ang(i) <= M_PIF)); } From 61a1d51b3533ccee838a6154d1c30302fe23d6bb Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Wed, 11 Sep 2024 22:55:24 +0530 Subject: [PATCH 17/21] Fixed mis-merge of AckermannReversingTest Signed-off-by: Ayush1285 --- .../test/motion_model_tests.cpp | 78 +++++++++++++++++++ 1 file changed, 78 insertions(+) diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp index 2dd0827383..1a5a5a1149 100644 --- a/nav2_mppi_controller/test/motion_model_tests.cpp +++ b/nav2_mppi_controller/test/motion_model_tests.cpp @@ -164,6 +164,9 @@ TEST(MotionModelTests, AckermannTest) // VX equal since this doesn't change, the WZ is reduced if breaking the constraint EXPECT_TRUE(initial_control_sequence.vx.isApprox(control_sequence.vx)); EXPECT_FALSE(initial_control_sequence.wz.isApprox(control_sequence.wz)); + for (unsigned int i = 1; i != control_sequence.wz.rows(); i++) { + EXPECT_GT(control_sequence.wz(i), 0.0); + } // Now, check the specifics of the minimum curvature constraint EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6); @@ -177,3 +180,78 @@ TEST(MotionModelTests, AckermannTest) // Check it cleanly destructs model.reset(); } + +TEST(MotionModelTests, AckermannReversingTest) +{ + models::ControlSequence control_sequence; + models::ControlSequence control_sequence2; + models::State state; + int batches = 1000; + int timesteps = 50; + control_sequence.reset(timesteps); // populates with zeros + control_sequence2.reset(timesteps); // populates with zeros + state.reset(batches, timesteps); // populates with zeros + auto node = std::make_shared("my_node"); + ParametersHandler param_handler(node); + std::unique_ptr model = + std::make_unique(¶m_handler, node->get_name()); + + // Check that predict properly populates the trajectory velocities with the control velocities + state.cvx = 10 * Eigen::ArrayXXf::Ones(batches, timesteps); + state.cvy = 5 * Eigen::ArrayXXf::Ones(batches, timesteps); + state.cwz = 1 * Eigen::ArrayXXf::Ones(batches, timesteps); + + // Manually set state index 0 from initial conditions which would be the speed of the robot + state.vx.col(0) = 10; + state.wz.col(0) = 1; + + model->predict(state); + + EXPECT_TRUE(state.vx.isApprox(state.cvx)); + EXPECT_TRUE(state.vy.isApprox(Eigen::ArrayXXf::Zero(batches, timesteps))); // non-holonomic + EXPECT_TRUE(state.wz.isApprox(state.cwz)); + + // Check that application of constraints are non-empty for Ackermann Drive + for (unsigned int i = 0; i != control_sequence.vx.rows(); i++) { + float idx = static_cast(i); + control_sequence.vx(i) = -idx * idx * idx; // now reversing + control_sequence.wz(i) = idx * idx * idx * idx; + } + + models::ControlSequence initial_control_sequence = control_sequence; + model->applyConstraints(control_sequence); + // VX equal since this doesn't change, the WZ is reduced if breaking the constraint + EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); + EXPECT_NE(initial_control_sequence.wz, control_sequence.wz); + for (unsigned int i = 1; i != control_sequence.wz.rows(); i++) { + EXPECT_GT(control_sequence.wz(i), 0.0); + } + + // Repeat with negative rotation direction + for (unsigned int i = 0; i != control_sequence2.vx.rows(); i++) { + float idx = static_cast(i); + control_sequence2.vx(i) = -idx * idx * idx; // now reversing + control_sequence2.wz(i) = -idx * idx * idx * idx; + } + + models::ControlSequence initial_control_sequence2 = control_sequence2; + model->applyConstraints(control_sequence2); + // VX equal since this doesn't change, the WZ is reduced if breaking the constraint + EXPECT_EQ(initial_control_sequence2.vx, control_sequence2.vx); + EXPECT_NE(initial_control_sequence2.wz, control_sequence2.wz); + for (unsigned int i = 1; i != control_sequence2.wz.rows(); i++) { + EXPECT_LT(control_sequence2.wz(i), 0.0); + } + + // Now, check the specifics of the minimum curvature constraint + EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6); + for (unsigned int i = 1; i != control_sequence2.vx.rows(); i++) { + EXPECT_TRUE(fabs(control_sequence2.vx(i)) / fabs(control_sequence2.wz(i)) >= 0.2); + } + + // Check that Ackermann Drive is properly non-holonomic and parameterized + EXPECT_EQ(model->isHolonomic(), false); + + // Check it cleanly destructs + model.reset(); +} From 63b1e615d8d39a569aa6b8e12a014a78baea67e0 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Wed, 11 Sep 2024 23:26:21 +0530 Subject: [PATCH 18/21] fixed gtest assertion Signed-off-by: Ayush1285 --- nav2_mppi_controller/test/motion_model_tests.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp index 1a5a5a1149..0548815d56 100644 --- a/nav2_mppi_controller/test/motion_model_tests.cpp +++ b/nav2_mppi_controller/test/motion_model_tests.cpp @@ -221,8 +221,8 @@ TEST(MotionModelTests, AckermannReversingTest) models::ControlSequence initial_control_sequence = control_sequence; model->applyConstraints(control_sequence); // VX equal since this doesn't change, the WZ is reduced if breaking the constraint - EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); - EXPECT_NE(initial_control_sequence.wz, control_sequence.wz); + EXPECT_TRUE(initial_control_sequence.vx.isApprox(control_sequence.vx)); + EXPECT_FALSE(initial_control_sequence.wz.isApprox(control_sequence.wz)); for (unsigned int i = 1; i != control_sequence.wz.rows(); i++) { EXPECT_GT(control_sequence.wz(i), 0.0); } @@ -237,8 +237,8 @@ TEST(MotionModelTests, AckermannReversingTest) models::ControlSequence initial_control_sequence2 = control_sequence2; model->applyConstraints(control_sequence2); // VX equal since this doesn't change, the WZ is reduced if breaking the constraint - EXPECT_EQ(initial_control_sequence2.vx, control_sequence2.vx); - EXPECT_NE(initial_control_sequence2.wz, control_sequence2.wz); + EXPECT_TRUE(initial_control_sequence2.vx.isApprox(control_sequence2.vx)); + EXPECT_FALSE(initial_control_sequence2.wz.isApprox(control_sequence2.wz)); for (unsigned int i = 1; i != control_sequence2.wz.rows(); i++) { EXPECT_LT(control_sequence2.wz(i), 0.0); } From 286b3c0b863cd1558929707807b51aa4d5a122ec Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Mon, 16 Sep 2024 12:25:33 +0530 Subject: [PATCH 19/21] Fixed optimizer_unit_tests and related issues Signed-off-by: Ayush1285 --- .../nav2_mppi_controller/critic_data.hpp | 2 +- .../models/control_sequence.hpp | 6 +-- .../nav2_mppi_controller/models/path.hpp | 6 +-- .../nav2_mppi_controller/models/state.hpp | 12 +++--- .../models/trajectories.hpp | 6 +-- .../nav2_mppi_controller/tools/utils.hpp | 40 ++++++++++++------- .../src/critics/cost_critic.cpp | 6 +-- .../src/critics/path_align_critic.cpp | 12 +++--- nav2_mppi_controller/src/optimizer.cpp | 33 +++++++++------ nav2_mppi_controller/test/CMakeLists.txt | 3 +- .../test/optimizer_unit_tests.cpp | 39 +++++++++--------- 11 files changed, 91 insertions(+), 74 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp index ac5447873d..91f5c8b7e0 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp @@ -39,7 +39,7 @@ namespace mppi struct CriticData { const models::State & state; - models::Trajectories & trajectories; + const models::Trajectories & trajectories; const models::Path & path; Eigen::ArrayXf & costs; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp index cb34f5b6c2..1f555b4d6b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp @@ -41,9 +41,9 @@ struct ControlSequence void reset(unsigned int time_steps) { - vx = Eigen::ArrayXf::Zero(time_steps); - vy = Eigen::ArrayXf::Zero(time_steps); - wz = Eigen::ArrayXf::Zero(time_steps); + vx.setZero(time_steps); + vy.setZero(time_steps); + wz.setZero(time_steps); } }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp index 27645a50cf..bc969796f7 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp @@ -35,9 +35,9 @@ struct Path */ void reset(unsigned int size) { - x = Eigen::ArrayXf::Zero(size); - y = Eigen::ArrayXf::Zero(size); - yaws = Eigen::ArrayXf::Zero(size); + x.setZero(size); + y.setZero(size); + yaws.setZero(size); } }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp index 6a680d814f..a8c81c3b07 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp @@ -46,13 +46,13 @@ struct State */ void reset(unsigned int batch_size, unsigned int time_steps) { - vx = Eigen::ArrayXXf::Zero(batch_size, time_steps); - vy = Eigen::ArrayXXf::Zero(batch_size, time_steps); - wz = Eigen::ArrayXXf::Zero(batch_size, time_steps); + vx.setZero(batch_size, time_steps); + vy.setZero(batch_size, time_steps); + wz.setZero(batch_size, time_steps); - cvx = Eigen::ArrayXXf::Zero(batch_size, time_steps); - cvy = Eigen::ArrayXXf::Zero(batch_size, time_steps); - cwz = Eigen::ArrayXXf::Zero(batch_size, time_steps); + cvx.setZero(batch_size, time_steps); + cvy.setZero(batch_size, time_steps); + cwz.setZero(batch_size, time_steps); } }; } // namespace mppi::models 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 70c155ba5d..24dbcb7f69 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp @@ -35,9 +35,9 @@ struct Trajectories */ void reset(unsigned int batch_size, unsigned int time_steps) { - x = Eigen::ArrayXXf::Zero(batch_size, time_steps); - y = Eigen::ArrayXXf::Zero(batch_size, time_steps); - yaws = Eigen::ArrayXXf::Zero(batch_size, time_steps); + x.setZero(batch_size, time_steps); + y.setZero(batch_size, time_steps); + yaws.setZero(batch_size, time_steps); } }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 9d004ac42e..8f07837c2c 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -696,24 +696,34 @@ struct Pose2D float x, y, theta; }; -// TODO(Ayush1285) Re-factor this utility method to -// perform in-place column shifting operation /** - * @brief Shift the columns of a Eigen Array - * @param e Eigen expression or Array - * @param shift Number of columns by which array will be shifted - * ex: -1 means shifting columns to right by 1 place - * @return shifted Eigen Array + * @brief Shift the columns of a 2D Eigen Array or scalar values of + * 1D Eigen Array by 1 place. + * @param e Eigen Array + * @param direction direction in which Array will be shifted. + * 1 for shift in right direction and -1 for left direction. */ -template -auto rollColumns(T && e, std::ptrdiff_t shift) +inline void shiftColumnsByOnePlace(Eigen::Ref e, int direction) { - shift = shift >= 0 ? shift : e.cols() + shift; - auto flat_size = shift * e.rows(); - Eigen::ArrayXXf cpyMatrix(e.rows(), e.cols()); - std::copy(e.data(), e.data() + flat_size, std::copy( - e.data() + flat_size, e.data() + e.size(), cpyMatrix.data())); - return cpyMatrix; + int size = e.size(); + if((e.cols() == 1 || e.rows() == 1) && size > 1) { + auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1; + auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1; + while(start_ptr != end_ptr) { + *(start_ptr + direction) = *start_ptr; + start_ptr -= direction; + } + *(start_ptr + direction) = *start_ptr; + } else { + auto start_ptr = direction == 1 ? e.data() + size - 2 * e.rows() : e.data() + e.rows(); + auto end_ptr = direction == 1 ? e.data() : e.data() + size - e.rows(); + auto span = e.rows(); + while(start_ptr != end_ptr) { + std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); + start_ptr -= (direction * span); + } + std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); + } } inline auto point_corrected_yaws( diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index 84c1444489..d784eade02 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -127,13 +127,13 @@ void CostCritic::score(CriticData & data) int strided_traj_rows = data.trajectories.x.rows(); int outer_stride = strided_traj_rows * trajectory_point_step_; - const auto traj_x = Eigen::Map>(data.trajectories.x.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); - const auto traj_y = Eigen::Map>(data.trajectories.y.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); - const auto traj_yaw = Eigen::Map>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 84da4edf51..73be9db274 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -99,11 +99,13 @@ void PathAlignCritic::score(CriticData & data) int strided_traj_cols = data.trajectories.x.cols() / trajectory_point_step_ + 1; int outer_stride = strided_traj_rows * trajectory_point_step_; // Get strided trajectory information - const auto T_x = Eigen::Map>(data.trajectories.x.data(), - strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); - const auto T_y = Eigen::Map>(data.trajectories.y.data(), - strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); - const auto T_yaw = Eigen::Map>(data.trajectories.x.data(), + strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto T_y = Eigen::Map>(data.trajectories.y.data(), + strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto T_yaw = Eigen::Map>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); const auto traj_sampled_size = T_x.cols(); diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 36271d19e7..3e25947287 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -213,11 +213,15 @@ void Optimizer::prepare( void Optimizer::shiftControlSequence() { - control_sequence_.vx = utils::rollColumns(control_sequence_.vx, 1); - control_sequence_.wz = utils::rollColumns(control_sequence_.wz, 1); + auto size = control_sequence_.vx.size(); + utils::shiftColumnsByOnePlace(control_sequence_.vx, -1); + utils::shiftColumnsByOnePlace(control_sequence_.wz, -1); + control_sequence_.vx(size - 1) = control_sequence_.vx(size - 2); + control_sequence_.wz(size - 1) = control_sequence_.wz(size - 2); if (isHolonomic()) { - control_sequence_.vy = utils::rollColumns(control_sequence_.vy, 1); + utils::shiftColumnsByOnePlace(control_sequence_.vy, -1); + control_sequence_.vy(size - 1) = control_sequence_.vy(size - 2); } } @@ -243,6 +247,11 @@ void Optimizer::applyControlSequenceConstraints() s.constraints.vy, std::max(control_sequence_.vy(0), -s.constraints.vy)); float wz_last = std::min( s.constraints.wz, std::max(control_sequence_.wz(0), -s.constraints.wz)); + control_sequence_.vx(0) = vx_last; + control_sequence_.wz(0) = wz_last; + if (isHolonomic()) { + control_sequence_.vy(0) = vy_last; + } for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) { float & vx_curr = control_sequence_.vx(i); @@ -313,10 +322,10 @@ void Optimizer::integrateStateVelocities( last_yaw = curr_yaw; } - Eigen::ArrayXf yaw_cos = utils::rollColumns(traj_yaws, -1).cos(); - Eigen::ArrayXf yaw_sin = utils::rollColumns(traj_yaws, -1).sin(); - yaw_cos(0) = cosf(initial_yaw); - yaw_sin(0) = sinf(initial_yaw); + utils::shiftColumnsByOnePlace(traj_yaws, 1); + traj_yaws(0) = initial_yaw; + Eigen::ArrayXf yaw_cos = traj_yaws.cos(); + Eigen::ArrayXf yaw_sin = traj_yaws.sin(); auto dx = (vx * yaw_cos).eval(); auto dy = (vx * yaw_sin).eval(); @@ -352,12 +361,10 @@ void Optimizer::integrateStateVelocities( for(unsigned int i = 1; i != n_cols; i++) { trajectories.yaws.col(i) = trajectories.yaws.col(i - 1) + state.wz.col(i) * settings_.model_dt; } - - auto yaw_cos = (utils::rollColumns(trajectories.yaws, -1).cos()).eval(); - auto yaw_sin = (utils::rollColumns(trajectories.yaws, -1).sin()).eval(); - - yaw_cos.col(0) = cosf(initial_yaw); - yaw_sin.col(0) = sinf(initial_yaw); + utils::shiftColumnsByOnePlace(trajectories.yaws, 1); + trajectories.yaws.col(0) = initial_yaw; + auto yaw_cos = trajectories.yaws.cos().eval(); + auto yaw_sin = trajectories.yaws.sin().eval(); auto dx = (state.vx * yaw_cos).eval(); auto dy = (state.vx * yaw_sin).eval(); diff --git a/nav2_mppi_controller/test/CMakeLists.txt b/nav2_mppi_controller/test/CMakeLists.txt index e1cddfbb4b..01004e0fa7 100644 --- a/nav2_mppi_controller/test/CMakeLists.txt +++ b/nav2_mppi_controller/test/CMakeLists.txt @@ -12,7 +12,6 @@ set(TEST_NAMES optimizer_unit_tests ) - foreach(name IN LISTS TEST_NAMES) ament_add_gtest(${name} ${name}.cpp @@ -44,4 +43,4 @@ target_link_libraries(critics_tests ) if(${TEST_DEBUG_INFO}) target_compile_definitions(critics_tests PUBLIC -DTEST_DEBUG_INFO) -endif() \ No newline at end of file +endif() diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index 2d03d184cb..801cfff38a 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -106,12 +106,12 @@ class OptimizerTester : public Optimizer { reset(); - EXPECT_TRUE(state_.vx.isApprox(Eigen::ArrayXXf::Zero(1000, 50))); - EXPECT_TRUE(control_sequence_.vx.isApprox(Eigen::ArrayXf::Zero(50))); + EXPECT_TRUE(state_.vx.isApproxToConstant(0.0f)); + EXPECT_TRUE(control_sequence_.vx.isApproxToConstant(0.0f)); EXPECT_EQ(control_history_[0].vx, 0.0); EXPECT_EQ(control_history_[0].vy, 0.0); EXPECT_NEAR(costs_.sum(), 0, 1e-6); - EXPECT_TRUE(generated_trajectories_.x.isApprox(Eigen::ArrayXXf::Zero(1000, 50))); + EXPECT_TRUE(generated_trajectories_.x.isApproxToConstant(0.0f)); } bool fallbackWrapper(bool fail) @@ -247,7 +247,7 @@ TEST(OptimizerTests, BasicInitializedFunctions) auto trajs = optimizer_tester.getGeneratedTrajectories(); EXPECT_EQ(trajs.x.rows(), 1000); EXPECT_EQ(trajs.x.cols(), 50); - EXPECT_TRUE(trajs.x.isApprox(Eigen::ArrayXXf::Zero(1000, 50))); + EXPECT_TRUE(trajs.x.isApproxToConstant(0.0f)); optimizer_tester.resetMotionModel(); optimizer_tester.testSetOmniModel(); @@ -401,7 +401,7 @@ TEST(OptimizerTests, shiftControlSequenceTests) optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); // Test shiftControlSequence by setting the 2nd value to something unique to neighbors - auto sequence = optimizer_tester.grabControlSequence(); + auto & sequence = optimizer_tester.grabControlSequence(); sequence.reset(100); sequence.vx(0) = 9999; sequence.vx(1) = 6; @@ -491,34 +491,34 @@ TEST(OptimizerTests, applyControlSequenceConstraintsTests) // in motion_models_test.cpp optimizer_tester.resetMotionModel(); optimizer_tester.testSetOmniModel(); - auto sequence = optimizer_tester.grabControlSequence(); + auto & sequence = optimizer_tester.grabControlSequence(); // Test boundary of limits sequence.vx = Eigen::ArrayXf::Ones(50); sequence.vy = 0.75 * Eigen::ArrayXf::Ones(50); sequence.wz = 2.0 * Eigen::ArrayXf::Ones(50); optimizer_tester.applyControlSequenceConstraintsWrapper(); - EXPECT_TRUE(sequence.vx.isApprox(Eigen::ArrayXf::Ones(50))); - EXPECT_TRUE(sequence.vy.isApprox(0.75 * Eigen::ArrayXf::Ones(50))); - EXPECT_TRUE(sequence.wz.isApprox(2.0 * Eigen::ArrayXf::Ones(50))); + EXPECT_TRUE(sequence.vx.isApproxToConstant(1.0f)); + EXPECT_TRUE(sequence.vy.isApproxToConstant(0.75f)); + EXPECT_TRUE(sequence.wz.isApproxToConstant(2.0f)); // Test breaking limits sets to maximum sequence.vx = 5.0 * Eigen::ArrayXf::Ones(50); sequence.vy = 5.0 * Eigen::ArrayXf::Ones(50); sequence.wz = 5.0 * Eigen::ArrayXf::Ones(50); optimizer_tester.applyControlSequenceConstraintsWrapper(); - EXPECT_TRUE(sequence.vx.isApprox(Eigen::ArrayXf::Ones(50))); - EXPECT_TRUE(sequence.vy.isApprox(0.75 * Eigen::ArrayXf::Ones(50))); - EXPECT_TRUE(sequence.wz.isApprox(2.0 * Eigen::ArrayXf::Ones(50))); + EXPECT_TRUE(sequence.vx.isApproxToConstant(1.0f)); + EXPECT_TRUE(sequence.vy.isApproxToConstant(0.75f)); + EXPECT_TRUE(sequence.wz.isApproxToConstant(2.0f)); // Test breaking limits sets to minimum sequence.vx = -5.0 * Eigen::ArrayXf::Ones(50); sequence.vy = -5.0 * Eigen::ArrayXf::Ones(50); sequence.wz = -5.0 * Eigen::ArrayXf::Ones(50); optimizer_tester.applyControlSequenceConstraintsWrapper(); - EXPECT_TRUE(sequence.vx.isApprox(-1.0 * Eigen::ArrayXf::Ones(50))); - EXPECT_TRUE(sequence.vy.isApprox(-0.75 * Eigen::ArrayXf::Ones(50))); - EXPECT_TRUE(sequence.wz.isApprox(-2.0 * Eigen::ArrayXf::Ones(50))); + EXPECT_TRUE(sequence.vx.isApproxToConstant(-1.0f)); + EXPECT_TRUE(sequence.vy.isApproxToConstant(-0.75f)); + EXPECT_TRUE(sequence.wz.isApproxToConstant(-2.0f)); } TEST(OptimizerTests, updateStateVelocitiesTests) @@ -611,14 +611,14 @@ TEST(OptimizerTests, integrateStateVelocitiesTests) models::State state; state.reset(1000, 50); models::Trajectories traj; + traj.reset(1000, 50); state.vx = 0.1 * Eigen::ArrayXXf::Ones(1000, 50); state.vx.col(0) = Eigen::ArrayXf::Zero(1000); state.vy = Eigen::ArrayXXf::Zero(1000, 50); state.wz = Eigen::ArrayXXf::Zero(1000, 50); - optimizer_tester.integrateStateVelocitiesWrapper(traj, state); - EXPECT_TRUE(traj.y.isApprox(Eigen::ArrayXXf::Zero(1000, 50))); - EXPECT_TRUE(traj.yaws.isApprox(Eigen::ArrayXXf::Zero(1000, 50))); + EXPECT_TRUE(traj.y.isApproxToConstant(0.0f)); + EXPECT_TRUE(traj.yaws.isApproxToConstant(0.0f)); for (unsigned int i = 0; i != traj.x.cols(); i++) { EXPECT_NEAR(traj.x(1, i), i * 0.1 /*vel*/ * 0.1 /*dt*/, 1e-3); } @@ -628,7 +628,7 @@ TEST(OptimizerTests, integrateStateVelocitiesTests) state.vy.col(0) = Eigen::ArrayXf::Zero(1000); optimizer_tester.integrateStateVelocitiesWrapper(traj, state); - EXPECT_TRUE(traj.yaws.isApprox(Eigen::ArrayXXf::Zero(1000, 50))); + EXPECT_TRUE(traj.yaws.isApproxToConstant(0.0f)); for (unsigned int i = 0; i != traj.x.cols(); i++) { EXPECT_NEAR(traj.x(1, i), i * 0.1 /*vel*/ * 0.1 /*dt*/, 1e-3); EXPECT_NEAR(traj.y(1, i), i * 0.2 /*vel*/ * 0.1 /*dt*/, 1e-3); @@ -643,7 +643,6 @@ TEST(OptimizerTests, integrateStateVelocitiesTests) float x = 0; float y = 0; for (unsigned int i = 1; i != traj.x.cols(); i++) { - std::cout << i << std::endl; x += (0.1 /*vx*/ * cosf(0.2 /*wz*/ * 0.1 /*model_dt*/ * (i - 1))) * 0.1 /*model_dt*/; y += (0.1 /*vx*/ * sinf(0.2 /*wz*/ * 0.1 /*model_dt*/ * (i - 1))) * 0.1 /*model_dt*/; From 847f8375f65b992b820bdef885abf15981152ae2 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Wed, 2 Oct 2024 20:08:33 +0530 Subject: [PATCH 20/21] Fixed all the unit tests and critic tests, all unit tests passing locally Signed-off-by: Ayush1285 --- .../nav2_mppi_controller/tools/utils.hpp | 29 ++++--- .../src/critics/constraint_critic.cpp | 19 ++-- .../src/critics/goal_angle_critic.cpp | 6 +- .../src/critics/path_align_critic.cpp | 7 +- .../src/critics/path_angle_critic.cpp | 15 ++-- .../src/critics/twirling_critic.cpp | 4 +- .../src/critics/velocity_deadband_critic.cpp | 12 +-- .../src/trajectory_visualizer.cpp | 4 +- nav2_mppi_controller/test/critics_tests.cpp | 86 +++++++++---------- .../test/noise_generator_test.cpp | 30 +++---- nav2_mppi_controller/test/utils_test.cpp | 10 ++- 11 files changed, 116 insertions(+), 106 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 8f07837c2c..26afa8b7c1 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -265,13 +265,12 @@ inline bool withinPositionGoalTolerance( template auto normalize_angles(const T & angles) { - // Eigen::ArrayXXf theta = (angles + M_PIF).unaryExpr([](const float x){ - // float remainder = std::fmod(x, 2.0f * M_PIF); - // return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF; - // }); - // return ((theta < 0.0f).select(theta + M_PIF, theta - M_PIF)).eval(); - auto theta = angles - (M_PIF * ((angles + M_PIF_2) * (1.0f / M_PIF)).floor()); - return theta; + return (angles + M_PIF).unaryExpr([&](const float x){ + float remainder = std::fmod(x, 2.0f * M_PIF); + return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF; + }); + // auto theta = angles - (M_PIF * ((angles + M_PIF_2) * (1.0f / M_PIF)).floor()); + // return theta; } /** @@ -303,11 +302,12 @@ auto shortest_angular_distance( */ inline size_t findPathFurthestReachedPoint(const CriticData & data) { - const auto traj_x = data.trajectories.x.col(-1); - const auto traj_y = data.trajectories.y.col(-1); + int traj_cols = data.trajectories.x.cols(); + const auto traj_x = data.trajectories.x.col(traj_cols - 1); + const auto traj_y = data.trajectories.y.col(traj_cols - 1); - const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1) - traj_x; - const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1) - traj_y; + const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1).colwise() - traj_x; + const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1).colwise() - traj_y; const auto dists = dx * dx + dy * dy; @@ -453,8 +453,9 @@ inline void savitskyGolayFilter( const models::OptimizerSettings & settings) { // Savitzky-Golay Quadratic, 9-point Coefficients - Eigen::Array filter = {-21.0, 14.0, 39.0, 54.0, 59.0, 54.0, 39.0, 14.0, -21.0}; - filter /= 231.0; + Eigen::Array filter = {-21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f, + -21.0f}; + filter /= 231.0f; const unsigned int num_sequences = control_sequence.vx.size() - 1; @@ -464,7 +465,7 @@ inline void savitskyGolayFilter( } auto applyFilter = [&](const Eigen::Array & data) -> float { - return (data * filter).sum(); + return (data * filter).eval().sum(); }; auto applyFilterOverAxis = diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index 0d2f61c78f..54ea40f2c5 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -49,10 +49,10 @@ void ConstraintCritic::score(CriticData & data) if (diff != nullptr) { if (power_ > 1u) { data.costs += (((((data.state.vx - max_vel_).max(0.0f) + (min_vel_ - data.state.vx). - max(0.0f)) * data.model_dt).rowwise().sum().eval()) * weight_).pow(power_); + max(0.0f)) * data.model_dt).rowwise().sum().eval()) * weight_).pow(power_).eval(); } else { - data.costs += ((((data.state.vx - max_vel_).max(0.0f) + (min_vel_ - data.state.vx). - max(0.0f)) * data.model_dt).rowwise().sum().eval()) * weight_; + data.costs += (((((data.state.vx - max_vel_).max(0.0f) + (min_vel_ - data.state.vx). + max(0.0f)) * data.model_dt).rowwise().sum().eval()) * weight_).eval(); } return; } @@ -69,10 +69,10 @@ void ConstraintCritic::score(CriticData & data) auto vel_total = (data.state.vx.square() + data.state.vy.square()).sqrt() * sgn; if (power_ > 1u) { data.costs += ((std::move((vel_total - max_vel_).max(0.0f) + (min_vel_ - vel_total). - max(0.0f)) * data.model_dt).rowwise().sum().eval() * weight_).pow(power_); + max(0.0f)) * data.model_dt).rowwise().sum().eval() * weight_).pow(power_).eval(); } else { - data.costs += (std::move((vel_total - max_vel_).max(0.0f) + (min_vel_ - vel_total). - max(0.0f)) * data.model_dt).rowwise().sum().eval() * weight_; + data.costs += ((std::move((vel_total - max_vel_).max(0.0f) + (min_vel_ - vel_total). + max(0.0f)) * data.model_dt).rowwise().sum().eval() * weight_).eval(); } return; } @@ -86,10 +86,11 @@ void ConstraintCritic::score(CriticData & data) auto out_of_turning_rad_motion = (min_turning_rad - (vx.abs() / wz.abs())).max(0.0f); if (power_ > 1u) { data.costs += ((std::move((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) + - out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * weight_).pow(power_); + out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * + weight_).pow(power_).eval(); } else { - data.costs += (std::move((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) + - out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * weight_; + data.costs += ((std::move((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) + + out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * weight_).eval(); } return; } diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index e0036a8534..c564358011 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -46,10 +46,10 @@ void GoalAngleCritic::score(CriticData & data) if(power_ > 1u) { data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). - rowwise().mean()) * weight_).pow(power_); + rowwise().mean()) * weight_).pow(power_).eval(); } else { - data.costs += ((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). - rowwise().mean()) * weight_; + data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). + rowwise().mean()) * weight_).eval(); } } diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 73be9db274..19e4ecbd32 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -66,8 +66,9 @@ void PathAlignCritic::score(CriticData & data) } } - const size_t batch_size = data.trajectories.x.cols(); + const size_t batch_size = data.trajectories.x.rows(); Eigen::ArrayXf cost(data.costs.rows()); + cost.setZero(); // Find integrated distance in the path std::vector path_integrated_distances(path_segments_count, 0.0f); @@ -151,9 +152,9 @@ void PathAlignCritic::score(CriticData & data) } if (power_ > 1u) { - data.costs += (std::move(cost) * weight_).pow(power_); + data.costs += (cost * weight_).pow(power_).eval(); } else { - data.costs += std::move(cost) * weight_; + data.costs += (cost * weight_).eval(); } } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 692b436ff0..de3b0cf53f 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -97,17 +97,18 @@ void PathAngleCritic::score(CriticData & data) throw nav2_core::ControllerException("Invalid path angle mode!"); } - int && rightmost_idx = data.trajectories.y.cols() - 1; + int rightmost_idx = data.trajectories.y.cols() - 1; auto diff_y = goal_y - data.trajectories.y.col(rightmost_idx); auto diff_x = goal_x - data.trajectories.x.col(rightmost_idx); auto yaws_between_points = diff_y.binaryExpr( - diff_x, [&](const float & y, const float & x){return atan2f(y, x);}); + diff_x, [&](const float & y, const float & x){return atan2f(y, x);}).eval(); switch (mode_) { case PathAngleMode::FORWARD_PREFERENCE: { + auto rightmost_yaw = data.trajectories.yaws.col(rightmost_idx); auto yaws = utils::shortest_angular_distance( - data.trajectories.yaws.col(rightmost_idx), yaws_between_points).abs(); + rightmost_yaw, yaws_between_points).abs(); if (power_ > 1u) { data.costs += (yaws * weight_).pow(power_); } else { @@ -117,11 +118,12 @@ void PathAngleCritic::score(CriticData & data) } case PathAngleMode::NO_DIRECTIONAL_PREFERENCE: { + auto rightmost_yaw = data.trajectories.yaws.col(rightmost_idx); auto yaws = utils::shortest_angular_distance( - data.trajectories.yaws.col(rightmost_idx), yaws_between_points).abs(); + rightmost_yaw, yaws_between_points).abs(); auto yaws_between_points_corrected = utils::point_corrected_yaws(yaws, yaws_between_points); auto corrected_yaws = utils::shortest_angular_distance( - data.trajectories.yaws.col(rightmost_idx), yaws_between_points_corrected).abs(); + rightmost_yaw, yaws_between_points_corrected).abs(); if (power_ > 1u) { data.costs += (corrected_yaws * weight_).pow(power_); } else { @@ -131,10 +133,11 @@ void PathAngleCritic::score(CriticData & data) } case PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS: { + auto rightmost_yaw = data.trajectories.yaws.col(rightmost_idx); auto yaws_between_points_corrected = utils::point_corrected_yaws(yaws_between_points, goal_yaw); auto corrected_yaws = utils::shortest_angular_distance( - data.trajectories.yaws.col(rightmost_idx), yaws_between_points_corrected).abs(); + rightmost_yaw, yaws_between_points_corrected).abs(); if (power_ > 1u) { data.costs += (corrected_yaws * weight_).pow(power_); } else { diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index 0a897626dd..7fd838e8ee 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -39,9 +39,9 @@ void TwirlingCritic::score(CriticData & data) } if (power_ > 1u) { - data.costs += Eigen::pow((Eigen::abs(data.state.wz)).rowwise().mean().eval() * weight_, power_); + data.costs += ((data.state.wz.abs().rowwise().mean()) * weight_).pow(power_).eval(); } else { - data.costs += (Eigen::abs(data.state.wz)).rowwise().mean().eval() * weight_; + data.costs += ((data.state.wz.abs().rowwise().mean()) * weight_).eval(); } } diff --git a/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp index 5befdf34ea..c2d7e14094 100644 --- a/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp +++ b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp @@ -49,12 +49,12 @@ void VelocityDeadbandCritic::score(CriticData & data) data.costs += ((((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * - data.model_dt).rowwise().sum() * weight_).pow(power_); + data.model_dt).rowwise().sum() * weight_).pow(power_).eval(); } else { - data.costs += (((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + + data.costs += ((((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * - data.model_dt).rowwise().sum() * weight_; + data.model_dt).rowwise().sum() * weight_).eval(); } return; } @@ -63,12 +63,12 @@ void VelocityDeadbandCritic::score(CriticData & data) data.costs += ((((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * - data.model_dt).rowwise().sum() * weight_).pow(power_); + data.model_dt).rowwise().sum() * weight_).pow(power_).eval(); } else { - data.costs += (((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + + data.costs += ((((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * - data.model_dt).rowwise().sum() * weight_; + data.model_dt).rowwise().sum() * weight_).eval(); } return; } diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 5d4df563e8..ece3971768 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -110,8 +110,8 @@ void TrajectoryVisualizer::add( const float shape_1 = static_cast(n_cols); points_->markers.reserve(floor(n_rows / trajectory_step_) * floor(n_cols * time_step_)); - for (size_t i = 0; i != n_rows; i += trajectory_step_) { - for (size_t j = 0; j != n_cols; j += time_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); float blue_component = 1.0f - j_flt / shape_1; float green_component = j_flt / shape_1; diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index d5660c88fd..9402343f33 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -15,6 +15,7 @@ #include #include +#include #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" @@ -66,6 +67,10 @@ TEST(CriticTests, ConstraintsCritic) costmap_ros->on_configure(lstate); models::State state; + // provide velocities in constraints, should not have any costs + state.vx = 0.40 * Eigen::ArrayXXf::Ones(1000, 30); + state.vy = Eigen::ArrayXXf::Zero(1000, 30); + state.wz = Eigen::ArrayXXf::Ones(1000, 30); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; @@ -86,41 +91,34 @@ TEST(CriticTests, ConstraintsCritic) EXPECT_TRUE(critic.getMinVelConstraint() < 0.0); // Scoring testing - - // provide velocities in constraints, should not have any costs - state.vx = 0.40 * Eigen::ArrayXXf::Ones(1000, 30); - state.vy = Eigen::ArrayXXf::Zero(1000, 30); - state.wz = Eigen::ArrayXXf::Ones(1000, 30); critic.score(data); EXPECT_NEAR(costs.sum(), 0, 1e-6); // provide out of maximum velocity constraint - auto last_batch_traj_in_full = state.vx.row(-1); - last_batch_traj_in_full = 0.60 * Eigen::ArrayXf::Ones(30); + state.vx.row(999).setConstant(0.60f); critic.score(data); EXPECT_GT(costs.sum(), 0); // 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2 EXPECT_NEAR(costs(999), 1.2, 0.01); - costs = Eigen::ArrayXf::Zero(1000); + costs.setZero(); // provide out of minimum velocity constraint - auto first_batch_traj_in_full = state.vx.row(0); - first_batch_traj_in_full = -0.45 * Eigen::ArrayXf::Ones(30); + state.vx.row(1).setConstant(-0.45f); critic.score(data); EXPECT_GT(costs.sum(), 0); // 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2 EXPECT_NEAR(costs(1), 1.2, 0.01); - costs = Eigen::ArrayXf::Zero(1000); + costs.setZero(); // Now with ackermann, all in constraint so no costs to score - state.vx = 0.40 * Eigen::ArrayXXf::Ones(1000, 30); - state.wz = 1.5 * Eigen::ArrayXXf::Ones(1000, 30); + state.vx.setConstant(0.40f); + state.wz.setConstant(1.5f); data.motion_model = std::make_shared(¶m_handler, node->get_name()); critic.score(data); EXPECT_NEAR(costs.sum(), 0, 1e-6); // Now violating the ackermann constraints - state.wz = 2.5 * Eigen::ArrayXXf::Ones(1000, 30); + state.wz.setConstant(2.5f); critic.score(data); EXPECT_GT(costs.sum(), 0); // 4.0 weight * 0.1 model_dt * (0.2 - 0.4/2.5) * 30 timesteps = 0.48 @@ -142,6 +140,7 @@ TEST(CriticTests, GoalAngleCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + path.reset(10); Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = @@ -160,7 +159,6 @@ TEST(CriticTests, GoalAngleCritic) // provide state poses and path too far from `threshold_to_consider` to consider state.pose.pose.position.x = 1.0; - path.reset(10); path.x(9) = 10.0; path.y(9) = 0.0; path.yaws(9) = 3.14; @@ -194,6 +192,7 @@ TEST(CriticTests, GoalCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + path.reset(10); Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = @@ -212,20 +211,19 @@ TEST(CriticTests, GoalCritic) // provide state poses and path far, should not trigger state.pose.pose.position.x = 1.0; - path.reset(10); path.x(9) = 10.0; path.y(9) = 0.0; critic.score(data); EXPECT_NEAR(costs(2), 0.0, 1e-6); // (0 * 5.0 weight EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // Should all be 0 * 1000 - costs = Eigen::ArrayXf::Zero(1000); + costs.setZero(); // provide state pose and path close path.x(9) = 0.5; path.y(9) = 0.0; critic.score(data); EXPECT_NEAR(costs(2), 2.5, 1e-6); // (sqrt(10.0 * 10.0) * 5.0 weight - EXPECT_NEAR(costs.sum(), 2500.0, 1e-6); // should be 2.5 * 1000 + EXPECT_NEAR(costs.sum(), 2500.0, 1e-3); // should be 2.5 * 1000 } TEST(CriticTests, PathAngleCritic) @@ -244,6 +242,7 @@ TEST(CriticTests, PathAngleCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + path.reset(10); Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = @@ -264,7 +263,6 @@ TEST(CriticTests, PathAngleCritic) // provide state poses and path close, within pose tolerance so won't do anything state.pose.pose.position.x = 0.0; state.pose.pose.position.y = 0.0; - path.reset(10); path.x(9) = 0.15; critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -286,7 +284,7 @@ TEST(CriticTests, PathAngleCritic) // Set mode to no directional preferences + reset costs critic.setMode(1); - costs = Eigen::ArrayXf::Zero(1000); + costs.setZero(); // provide state pose and path close but outside of tol. with more than PI/2 angular diff. path.x(6) = 1.0; // angle between path point and pose < max_angle_to_furthest_ @@ -310,7 +308,7 @@ TEST(CriticTests, PathAngleCritic) // Set mode to consider path directionality + reset costs critic.setMode(2); - costs = Eigen::ArrayXf::Zero(1000); + costs.setZero(); // provide state pose and path close but outside of tol. with more than PI/2 angular diff. path.x(6) = 1.0; // angle between path point and pose < max_angle_to_furthest_ @@ -359,6 +357,7 @@ TEST(CriticTests, PreferForwardCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + path.reset(10); Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = @@ -378,19 +377,18 @@ TEST(CriticTests, PreferForwardCritic) // provide state poses and path far away, not within positional tolerances state.pose.pose.position.x = 1.0; - path.reset(10); path.x(9) = 10.0; critic.score(data); EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f); // provide state pose and path close to trigger behavior but with all forward motion path.x(9) = 0.15; - state.vx = Eigen::ArrayXXf::Ones(1000, 30); + state.vx.setOnes(); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f); // provide state pose and path close to trigger behavior but with all reverse motion - state.vx = -1.0 * Eigen::ArrayXXf::Ones(1000, 30); + state.vx.setConstant(-1.0f); critic.score(data); EXPECT_GT(costs.sum(), 0.0f); EXPECT_NEAR(costs(0), 15.0f, 1e-3f); // 1.0 * 0.1 model_dt * 5.0 weight * 30 length @@ -412,6 +410,7 @@ TEST(CriticTests, TwirlingCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + path.reset(10); Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = @@ -432,28 +431,28 @@ TEST(CriticTests, TwirlingCritic) // provide state poses and path far away, not within positional tolerances state.pose.pose.position.x = 1.0; - path.reset(10); path.x(9) = 10.0; critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path close to trigger behavior but with no angular variation path.x(9) = 0.15; - state.wz = Eigen::ArrayXXf::Zero(1000, 30); + state.wz.setZero(); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // Provide nearby with some motion - auto traj_view = state.wz.row(0); - traj_view = 10.0; + state.wz.row(0).setConstant(10.0f); critic.score(data); EXPECT_NEAR(costs(0), 100.0, 1e-6); // (mean(10.0) * 10.0 weight - costs = Eigen::ArrayXf::Zero(1000); + costs.setZero(); // Now try again with some wiggling noise - traj_view = Eigen::ArrayXf::Random(30).abs() / 2.0f; + std::mt19937 engine; + std::normal_distribution normal_dist = std::normal_distribution(0.0f, 0.5f); + state.wz.row(0) = Eigen::ArrayXf::NullaryExpr(30, [&] () {return normal_dist(engine);}); critic.score(data); - EXPECT_NEAR(costs(0), 3.3, 4e-1); // (mean of noise with mu=0, sigma=0.5 * 10.0 weight + EXPECT_NEAR(costs(0), 2.581, 4e-1); // (mean of noise with mu=0, sigma=0.5 * 10.0 weight } TEST(CriticTests, PathFollowCritic) @@ -472,6 +471,7 @@ TEST(CriticTests, PathFollowCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + path.reset(6); Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = @@ -492,7 +492,6 @@ TEST(CriticTests, PathFollowCritic) // provide state poses and path close within positional tolerances state.pose.pose.position.x = 2.0; - path.reset(6); path.x(5) = 1.7; critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -520,6 +519,7 @@ TEST(CriticTests, PathAlignCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + path.reset(6); Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = @@ -540,7 +540,6 @@ TEST(CriticTests, PathAlignCritic) // provide state poses and path close within positional tolerances state.pose.pose.position.x = 1.0; - path.reset(10); path.x(9) = 0.85; critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -585,7 +584,7 @@ TEST(CriticTests, PathAlignCritic) path.x(19) = 0.9; path.x(20) = 0.9; path.x(21) = 0.9; - generated_trajectories.x = 0.66 * Eigen::ArrayXXf::Ones(1000, 30); + generated_trajectories.x.setConstant(0.66f); critic.score(data); // 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term EXPECT_NEAR(costs.sum(), 6600.0, 1e-2); @@ -601,9 +600,9 @@ TEST(CriticTests, PathAlignCritic) } data.path_pts_valid.reset(); // Recompute on new path - costs = Eigen::ArrayXf::Zero(1000); - path.x = 1.5 * Eigen::ArrayXf::Ones(22); - path.y = 1.5 * Eigen::ArrayXf::Ones(22); + costs.setZero(); + path.x.setConstant(1.5f); + path.y.setConstant(1.5f); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); } @@ -622,6 +621,7 @@ TEST(CriticTests, VelocityDeadbandCritic) costmap_ros->on_configure(lstate); models::State state; + state.reset(1000, 30); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; @@ -642,16 +642,16 @@ TEST(CriticTests, VelocityDeadbandCritic) // Scoring testing // provide velocities out of deadband bounds, should not have any costs - state.vx = 0.80 * Eigen::ArrayXXf::Ones(1000, 30); - state.vy = 0.60 * Eigen::ArrayXXf::Ones(1000, 30); - state.wz = 0.80 * Eigen::ArrayXXf::Ones(1000, 30); + state.vx.setConstant(0.80f); + state.vy.setConstant(0.60f); + state.wz.setConstant(0.80f); critic.score(data); EXPECT_NEAR(costs.sum(), 0, 1e-6); // Test cost value - state.vx = 0.01 * Eigen::ArrayXXf::Ones(1000, 30); - state.vy = 0.02 * Eigen::ArrayXXf::Ones(1000, 30); - state.wz = 0.021 * Eigen::ArrayXXf::Ones(1000, 30); + state.vx.setConstant(0.01f); + state.vy.setConstant(0.02f); + state.wz.setConstant(0.021f); critic.score(data); // 35.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7 EXPECT_NEAR(costs(1), 19.845, 0.01); diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp index 0e3735c654..f70f047d19 100644 --- a/nav2_mppi_controller/test/noise_generator_test.cpp +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -86,9 +86,9 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain) EXPECT_EQ(state.cvx(0), 0); EXPECT_EQ(state.cvy(0), 0); EXPECT_EQ(state.cwz(0), 0); - EXPECT_EQ(state.cvx(9), 9); - EXPECT_EQ(state.cvy(9), 9); - EXPECT_EQ(state.cwz(9), 9); + EXPECT_EQ(state.cvx(0, 9), 9); + EXPECT_EQ(state.cvy(0, 9), 9); + EXPECT_EQ(state.cwz(0, 9), 9); // Request an update with noise requested generator.generateNextNoises(); @@ -97,16 +97,16 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain) EXPECT_NE(state.cvx(0), 0); EXPECT_EQ(state.cvy(0), 0); // Not populated in non-holonomic EXPECT_NE(state.cwz(0), 0); - EXPECT_NE(state.cvx(9), 9); - EXPECT_EQ(state.cvy(9), 9); // Not populated in non-holonomic - EXPECT_NE(state.cwz(9), 9); + EXPECT_NE(state.cvx(0, 9), 9); + EXPECT_EQ(state.cvy(0, 9), 9); // Not populated in non-holonomic + EXPECT_NE(state.cwz(0, 9), 9); EXPECT_NEAR(state.cvx(0), 0, 0.3); EXPECT_NEAR(state.cvy(0), 0, 0.3); EXPECT_NEAR(state.cwz(0), 0, 0.3); - EXPECT_NEAR(state.cvx(9), 9, 0.3); - EXPECT_NEAR(state.cvy(9), 9, 0.3); - EXPECT_NEAR(state.cwz(9), 9, 0.3); + EXPECT_NEAR(state.cvx(0, 9), 9, 0.3); + EXPECT_NEAR(state.cvy(0, 9), 9, 0.3); + EXPECT_NEAR(state.cwz(0, 9), 9, 0.3); // Test holonomic setting generator.reset(settings, true); // Now holonomically @@ -116,16 +116,16 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain) EXPECT_NE(state.cvx(0), 0); EXPECT_NE(state.cvy(0), 0); // Now populated in non-holonomic EXPECT_NE(state.cwz(0), 0); - EXPECT_NE(state.cvx(9), 9); - EXPECT_NE(state.cvy(9), 9); // Now populated in non-holonomic - EXPECT_NE(state.cwz(9), 9); + EXPECT_NE(state.cvx(0, 9), 9); + EXPECT_NE(state.cvy(0, 9), 9); // Now populated in non-holonomic + EXPECT_NE(state.cwz(0, 9), 9); EXPECT_NEAR(state.cvx(0), 0, 0.3); EXPECT_NEAR(state.cvy(0), 0, 0.3); EXPECT_NEAR(state.cwz(0), 0, 0.3); - EXPECT_NEAR(state.cvx(9), 9, 0.3); - EXPECT_NEAR(state.cvy(9), 9, 0.3); - EXPECT_NEAR(state.cwz(9), 9, 0.3); + EXPECT_NEAR(state.cvx(0, 9), 9, 0.3); + EXPECT_NEAR(state.cvy(0, 9), 9, 0.3); + EXPECT_NEAR(state.cwz(0, 9), 9, 0.3); generator.shutdown(); } diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 7e17606b5c..2e9b633205 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" @@ -177,7 +178,7 @@ TEST(UtilsTests, AnglesTests) } } - auto norm_ang = normalize_angles(angles); + auto norm_ang = normalize_angles(angles).eval(); for (unsigned int i = 0; i != norm_ang.size(); i++) { EXPECT_TRUE((norm_ang(i) >= -M_PIF) && (norm_ang(i) <= M_PIF)); } @@ -185,7 +186,7 @@ TEST(UtilsTests, AnglesTests) // Test shortest angular distance Eigen::ArrayXf zero_angles(100); zero_angles.setZero(); - auto ang_dist = shortest_angular_distance(angles, zero_angles); + auto ang_dist = shortest_angular_distance(angles, zero_angles).eval(); for (unsigned int i = 0; i != ang_dist.size(); i++) { EXPECT_TRUE((ang_dist(i) >= -M_PIF) && (ang_dist(i) <= M_PIF)); } @@ -330,7 +331,10 @@ TEST(UtilsTests, SmootherTest) noisey_sequence.wz = 0.3 * Eigen::ArrayXf::Ones(30); // Make the sequence noisy - auto noises = ((Eigen::ArrayXf::Random(30)).abs()) / 5.0f; + std::mt19937 engine; + std::normal_distribution normal_dist = std::normal_distribution(0.0f, 0.2f); + auto noises = Eigen::ArrayXf::NullaryExpr( + 30, [&] () {return normal_dist(engine);}); noisey_sequence.vx += noises; noisey_sequence.vy += noises; noisey_sequence.wz += noises; From f9ed129599ef6624834ab9813565c12a36747438 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Wed, 2 Oct 2024 21:56:36 +0530 Subject: [PATCH 21/21] fixed few review comments Signed-off-by: Ayush1285 --- .../include/nav2_mppi_controller/optimizer.hpp | 2 +- nav2_mppi_controller/src/noise_generator.cpp | 6 +++--- nav2_mppi_controller/src/optimizer.cpp | 6 +++--- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 66780d21f3..b1c6585623 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -196,7 +196,7 @@ class Optimizer * @param state fill state */ void integrateStateVelocities( - Eigen::Array & trajectories, + Eigen::Array & trajectories, const Eigen::ArrayXXf & state) const; /** diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index de08cf3b34..a704ef8b5c 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -82,9 +82,9 @@ void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_h // Recompute the noises on reset, initialization, and fallback { std::unique_lock guard(noise_lock_); - noises_vx_ = Eigen::ArrayXXf::Zero(settings_.batch_size, settings_.time_steps); - noises_vy_ = Eigen::ArrayXXf::Zero(settings_.batch_size, settings_.time_steps); - noises_wz_ = Eigen::ArrayXXf::Zero(settings_.batch_size, settings_.time_steps); + noises_vx_.setZero(settings_.batch_size, settings_.time_steps); + noises_vy_.setZero(settings_.batch_size, settings_.time_steps); + noises_wz_.setZero(settings_.batch_size, settings_.time_steps); ready_ = true; } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 3e25947287..5009db7504 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -300,7 +300,7 @@ void Optimizer::propagateStateVelocitiesFromInitials( } void Optimizer::integrateStateVelocities( - Eigen::Array & trajectory, + Eigen::Array & trajectory, const Eigen::ArrayXXf & sequence) const { float initial_yaw = static_cast(tf2::getYaw(state_.pose.pose.orientation)); @@ -386,8 +386,8 @@ Eigen::ArrayXXf Optimizer::getOptimizedTrajectory() { const bool is_holo = isHolonomic(); Eigen::ArrayXXf sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2); - Eigen::Array trajectories = - Eigen::Array(settings_.time_steps, 3); + Eigen::Array trajectories = + Eigen::Array(settings_.time_steps, 3); sequence.col(0) = control_sequence_.vx; sequence.col(1) = control_sequence_.wz;